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
hughhugh · Apr 21, 2016 · 4419b3c · 4419b3c
1 parent a7f959e
commit 4419b3c
Showing 1 changed file with 7 additions and 8 deletions.
diff --git 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;