From 0665f9c32bb6e144c5a157f557eaf91074b869b5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 10:52:08 +1000 Subject: [PATCH] AP_NavEKF3: store and use lastKnownPositionD When returning our last-known-position we were supplying lat/lng but not altitude. This usually really doesn't matter as this result generally isn't used, especially for altitude. OTOH, it may prevent a bug into the future. --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 3 +++ libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 1 + libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 + 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 5b9910c03d..e812bf469e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -381,6 +381,9 @@ void NavEKF3_core::setAidingMode() meaHgtAtTakeOff = baroDataDelayed.hgt; // reset the vertical position state to faster recover from baro errors experienced during touchdown stateStruct.position.z = -meaHgtAtTakeOff; + // store the current height to be used to keep reporting + // the last known position + lastKnownPositionD = stateStruct.position.z; // reset relative aiding sensor fusion activity status flowFusionActive = false; bodyVelFusionActive = false; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 4c63c0c6b3..2b490a922a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -279,7 +279,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const float posD; if(getPosD(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, Location::AltFrame::ABSOLUTE); + loc.set_alt_cm(origin.alt - posD*100.0, Location::AltFrame::ABSOLUTE); if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) { // The EKF is able to provide a position estimate loc.lat = EKF_origin.lat; @@ -309,6 +309,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const loc.lng = EKF_origin.lng; loc.offset(lastKnownPositionNE.x + posOffsetNED.x, lastKnownPositionNE.y + posOffsetNED.y); + loc.alt = EKF_origin.alt - lastKnownPositionD*100.0; return false; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 797d317935..681436f509 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -237,6 +237,7 @@ void NavEKF3_core::InitialiseVariables() dt = 0; velDotNEDfilt.zero(); lastKnownPositionNE.zero(); + lastKnownPositionD = 0; prevTnb.zero(); memset(&P[0][0], 0, sizeof(P)); memset(&KH[0][0], 0, sizeof(KH)); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index da31f6cb41..7aa20d5970 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1047,6 +1047,7 @@ private: uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec) uint32_t ekfStartTime_ms; // time the EKF was started (msec) Vector2F lastKnownPositionNE; // last known position + float lastKnownPositionD; // last known height uint32_t lastLaunchAccelTime_ms; ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold