AP_NavEKF3: fixed getLLH alt for local origin height

this fixes a bug introduced in #21834

this fix in #21834 was correct for getPosD, but should not have been
applied to getLLH

this caused cruise mode in plane to descend/ascend by the difference
between the public and local origins on mode entry

fixes #21984

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -2007,8 +2007,8 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
 
     // Record the position delta between current core and new primary core and the timestamp of the core change
     // Add current delta in case it hasn't been consumed yet
-    core[old_primary].getPosD(posDownOldPrimary);
+    core[old_primary].getPosD_local(posDownOldPrimary);
-    core[new_primary].getPosD(posDownNewPrimary);
+    core[new_primary].getPosD_local(posDownNewPrimary);
     pos_down_reset_data.core_delta = posDownNewPrimary - posDownOldPrimary + pos_down_reset_data.core_delta;
     pos_down_reset_data.last_primary_change = imuSampleTime_us / 1000;
     pos_down_reset_data.core_changed = true;

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -252,9 +252,9 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
     return false;
 }
 
-// Write the last calculated D position of the body frame origin relative to the EKF origin (m).
+// Write the last calculated D position of the body frame origin relative to the EKF local origin
 // Return true if the estimate is valid
-bool NavEKF3_core::getPosD(float &posD) const
+bool NavEKF3_core::getPosD_local(float &posD) const
 {
     // The EKF always has a height estimate regardless of mode of operation
     // Correct for the IMU offset (EKF calculations are at the IMU)
@@ -268,15 +268,24 @@ bool NavEKF3_core::getPosD(float &posD) const
         posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
     }
 
+    // Return the current height solution status
+    return filterStatus.flags.vert_pos;
+
+}
+
+// Write the last calculated D position of the body frame origin relative to the public origin
+// Return true if the estimate is valid
+bool NavEKF3_core::getPosD(float &posD) const
+{
+    bool ret = getPosD_local(posD);
+
     // adjust posD for difference between our origin and the public_origin
     Location local_origin;
     if (getOriginLLH(local_origin)) {
         posD += (public_origin.alt - local_origin.alt) * 0.01;
     }
 
-    // Return the current height solution status
+    return ret;
-    return filterStatus.flags.vert_pos;
-
 }
 
 // return the estimated height of body frame origin above ground level
@@ -296,7 +305,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
     Location origin;
     if (getOriginLLH(origin)) {
         float posD;
-        if(getPosD(posD) && PV_AidingMode != AID_NONE) {
+        if (getPosD_local(posD) && PV_AidingMode != AID_NONE) {
             // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
             loc.set_alt_cm(origin.alt - posD*100.0, Location::AltFrame::ABSOLUTE);
             if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -138,7 +138,10 @@ public:
     // If false returned, do not use for flight control
     bool getPosNE(Vector2f &posNE) const;
 
-    // Write the last calculated D position relative to the reference point (m).
+    // get position D from local origin
+    bool getPosD_local(float &posD) const;
+
+    // Write the last calculated D position relative to the public origin
     // If a calculated solution is not available, use the best available data and return false
     // If false returned, do not use for flight control
     bool getPosD(float &posD) const;