From 3a72cd1ce22588e89497783b950bd2cb61fbc78b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 22 Oct 2022 20:01:14 +1100 Subject: [PATCH] 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 | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 21 +++++++++++++++------ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 5 ++++- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 631a63e7ff160..7219ba6c62083 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/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[new_primary].getPosD(posDownNewPrimary); + core[old_primary].getPosD_local(posDownOldPrimary); + 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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index aced7b8b02ac4..143d3ce92f695 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/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 filterStatus.flags.vert_pos; - + return ret; } // 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) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 13250a71ec82a..d329b6da5f309 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -138,7 +138,10 @@ class NavEKF3_core : public NavEKF_core_common // 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;