From c881fac1de5d410f30ac2ddfd31a5267fa22558f Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Thu, 17 Jun 2021 23:08:25 -0400 Subject: [PATCH] AP_NavEKF3: Fix #17789 core's ekf origin altitude different if flying --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 4 ---- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 7 ++++++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index bd575d2f82..b1df687d48 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -607,10 +607,6 @@ bool NavEKF3_core::setOrigin(const Location &loc) } EKF_origin = loc; - // if flying, correct for height change from takeoff so that the origin is at field elevation - if (inFlight) { - EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z); - } ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt; // define Earth rotation vector in the NED navigation frame at the origin calcEarthRateNED(earthRateNED, EKF_origin.lat); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 65040ca9d7..3432b605ec 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -659,8 +659,13 @@ void NavEKF3_core::readGpsData() // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed if (gpsGoodToAlign && !validOrigin) { + Location gpsloc_fieldelevation = gpsloc; + // if flying, correct for height change from takeoff so that the origin is at field elevation + if (inFlight) { + gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z); + } - if (!setOrigin(gpsloc)) { + if (!setOrigin(gpsloc_fieldelevation)) { // set an error as an attempt was made to set the origin more than once INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return;