Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: fixed get_position for EKF to use correct relative altitude

Browse Source 
we need to use the EKF relative height plus the current AHRS home
This commit is contained in:
Andrew Tridgell 2015-02-21 10:12:53 +11:00
parent 869fb23062
commit 6781a8d329
1 changed files with 5 additions and 1 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

@ -168,7 +168,11 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
// dead-reckoning support // dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc) const bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
{ {
if (using_EKF() && EKF.getLLH(loc)) { Vector3f ned_pos;
if (using_EKF() && EKF.getLLH(loc) && EKF.getPosNED(ned_pos)) {
// fixup altitude using relative position from AHRS home, not
// EKF origin
loc.alt = get_home().alt - ned_pos.z*100;
return true; return true;
} }
return AP_AHRS_DCM::get_position(loc); return AP_AHRS_DCM::get_position(loc);