From 4419b3c617df6180d8788858f5a2d75b564e6550 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 19 Apr 2016 16:21:56 +0900 Subject: [PATCH] AHRS_NavEKF: fix get_position by using ekf origin The EKF's getPosNED returns a vertical position relative to the EKF origin but previously this function was using it as if it was relative to ahrs's home --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 95750147343d5d..68c860d911d4b5 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -329,22 +329,21 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con bool AP_AHRS_NavEKF::get_position(struct Location &loc) const { Vector3f ned_pos; + Location origin; switch (active_EKF_type()) { #if AP_AHRS_WITH_EKF1 case EKF_TYPE1: - if (EKF1.getLLH(loc) && EKF1.getPosNED(ned_pos)) { - // fixup altitude using relative position from AHRS home, not - // EKF origin - loc.alt = get_home().alt - ned_pos.z*100; + if (EKF1.getLLH(loc) && EKF1.getPosNED(ned_pos) && EKF1.getOriginLLH(origin)) { + // fixup altitude using relative position from EKF origin + loc.alt = origin.alt - ned_pos.z*100; return true; } break; #endif case EKF_TYPE2: - if (EKF2.getLLH(loc) && EKF2.getPosNED(-1,ned_pos)) { - // fixup altitude using relative position from AHRS home, not - // EKF origin - loc.alt = get_home().alt - ned_pos.z*100; + if (EKF2.getLLH(loc) && EKF2.getPosNED(-1,ned_pos) && EKF2.getOriginLLH(origin)) { + // fixup altitude using relative position from EKF origin + loc.alt = origin.alt - ned_pos.z*100; return true; } break;