Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: use get_distance_NE instead of location_diff

This commit is contained in:
Pierre Kancir 2019-04-08 15:16:19 +02:00 committed by Tom Pittenger
parent e4987f17b9
commit 6da820ac7b
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -811,7 +811,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const
}
Location loc;
get_position(loc);
const Vector2f diff2d = location_diff(get_home(), loc);
const Vector2f diff2d = get_home().get_distance_NE(loc);
const struct SITL::sitl_fdm &fdm = _sitl->state;
vec = Vector3f(diff2d.x, diff2d.y,
-(fdm.altitude - get_home().alt*0.01f));
@ -863,7 +863,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const
case EKF_TYPE_SITL: {
Location loc;
get_position(loc);
posNE = location_diff(get_home(), loc);
posNE = get_home().get_distance_NE(loc);
return true;
}
#endif
@ -881,7 +881,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_home(Vector2f &posNE) const
return false;
}
const Vector2f offset = location_diff(originLLH, _home);
const Vector2f offset = originLLH.get_distance_NE(_home);
posNE.x = originNE.x - offset.x;
posNE.y = originNE.y - offset.y;