AP_NavEKF2: capture and publish vertical position reset deltas

Calculate the change in vertical position due to a internal EKF state reset or a EKF core switch

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -624,6 +624,9 @@ bool NavEKF2::InitialiseFilter(void)
         ret &= core[i].InitialiseFilterBootstrap();
     }
 
+    // zero the structs used capture reset events
+    memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
+
     check_log_write();
     return ret;
 }
@@ -684,6 +687,7 @@ void NavEKF2::UpdateFilter(void)
         if (newPrimaryIndex != primary) {
             updateLaneSwitchYawResetData(has_switched, newPrimaryIndex, primary);
             updateLaneSwitchPosResetData(has_switched, newPrimaryIndex, primary);
+            updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
             primary = newPrimaryIndex;
             has_switched = true;
         }
@@ -1254,8 +1258,39 @@ const char *NavEKF2::prearm_failure_reason(void) const
     return core[primary].prearm_failure_reason();
 }
 
+// return the amount of vertical position change due to the last reset in metres
+// returns the time of the last reset or 0 if no reset has ever occurred
+uint32_t NavEKF2::getLastPosDownReset(float &posDelta)
+{
+    if (!core) {
+        return 0;
+    }
+
+    // Record last time controller got the position reset
+    pos_down_reset_data.last_function_call = imuSampleTime_us / 1000;
+    posDelta = 0.0f;
+    uint32_t lastPosReset_ms = 0;
+
+    // There has been a change in the primary core that the controller has not consumed
+    if (pos_down_reset_data.core_changed) {
+        posDelta = pos_down_reset_data.core_delta;
+        lastPosReset_ms = pos_down_reset_data.last_primary_change;
+        pos_down_reset_data.core_changed = false;
+    }
+
+    // There has been a reset inside the core since we switched
+    float tempPosDelta;
+    uint32_t lastCorePosReset_ms = core[primary].getLastPosDownReset(tempPosDelta);
+    if (lastCorePosReset_ms > lastPosReset_ms) {
+        posDelta += tempPosDelta;
+        lastPosReset_ms = lastCorePosReset_ms;
+    }
+
+    return lastPosReset_ms;
+}
+
 // update the yaw reset data to capture changes due to a lane switch
-void NavEKF2::updateLaneSwitchYawResetData(bool has_switched, uint8_t new_primary, uint8_t old_primary)
+void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary)
 {
     Vector3f eulers_old_primary, eulers_new_primary;
     float old_yaw_delta;
@@ -1266,8 +1301,7 @@ void NavEKF2::updateLaneSwitchYawResetData(bool has_switched, uint8_t new_primar
     }
 
     // If current primary has reset yaw after controller got it, add it to the delta
-    // Prevent adding the delta if we have already changed primary in this filter update
+    if (core[old_primary].getLastYawResetAngle(old_yaw_delta) > yaw_reset_data.last_function_call) {
-    if (!has_switched && core[old_primary].getLastYawResetAngle(old_yaw_delta) > yaw_reset_data.last_function_call) {
         yaw_reset_data.core_delta += old_yaw_delta;
     }
 
@@ -1306,4 +1340,31 @@ void NavEKF2::updateLaneSwitchPosResetData(bool has_switched, uint8_t new_primar
     pos_reset_data.core_changed = true;
 }
 
+// Update the vertical position reset data to capture changes due to a core switch
+// This should be called after the decision to switch cores has been made, but before the
+// new primary EKF update has been run
+void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary)
+{
+    float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta;
+
+    // If core position reset data has been consumed reset delta to zero
+    if (!pos_down_reset_data.core_changed) {
+        pos_down_reset_data.core_delta = 0.0f;
+    }
+
+    // If current primary has reset position after controller got it, add it to the delta
+    if (core[old_primary].getLastPosDownReset(oldPosDownDelta) > pos_down_reset_data.last_function_call) {
+        pos_down_reset_data.core_delta += oldPosDownDelta;
+    }
+
+    // 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);
+    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;
+
+}
+
 #endif //HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -279,6 +279,10 @@ public:
     // returns the time of the last reset or 0 if no reset has ever occurred
     uint32_t getLastVelNorthEastReset(Vector2f &vel) const;
 
+    // return the amount of vertical position change due to the last reset in metres
+    // returns the time of the last reset or 0 if no reset has ever occurred
+    uint32_t getLastPosDownReset(float &posDelta);
+
     // report any reason for why the backend is refusing to initialise
     const char *prearm_failure_reason(void) const;
 
@@ -394,6 +398,13 @@ private:
         Vector2f core_delta;          // the amount of NE position change between cores when a change happened
     } pos_reset_data;
 
+    struct {
+        uint32_t last_function_call;  // last time getLastPosDownReset was called
+        bool core_changed;            // true when a core change happened and hasn't been consumed, false otherwise
+        uint32_t last_primary_change; // last time a primary has changed
+        float core_delta;             // the amount of D position change between cores when a change happened
+    } pos_down_reset_data;
+
     // update the yaw reset data to capture changes due to a lane switch
     // has_switched - true if the primary instance has already been changed during this filter update cycle
     // new_primary - index of the ekf instance that we are about to switch to as the primary
@@ -405,4 +416,9 @@ private:
     // new_primary - index of the ekf instance that we are about to switch to as the primary
     // old_primary - index of the ekf instance that we are currently using as the primary
     void updateLaneSwitchPosResetData(bool has_switched, uint8_t new_primary, uint8_t old_primary);
+
+    // update the position reset data to capture changes due to a lane switch
+    // new_primary - index of the ekf instance that we are about to switch to as the primary
+    // old_primary - index of the ekf instance that we are currently using as the primary
+    void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary);
 };

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -152,6 +152,14 @@ uint32_t NavEKF2_core::getLastPosNorthEastReset(Vector2f &pos) const
     return lastPosReset_ms;
 }
 
+// return the amount of vertical position change due to the last vertical position reset in metres
+// returns the time of the last reset or 0 if no reset has ever occurred
+uint32_t NavEKF2_core::getLastPosDownReset(float &posD) const
+{
+    posD = posResetD;
+    return lastPosResetD_ms;
+}
+
 // return the amount of NE velocity change due to the last velocity reset in metres/sec
 // returns the time of the last reset or 0 if no reset has ever occurred
 uint32_t NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel) const

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -99,6 +99,9 @@ void NavEKF2_core::ResetPosition(void)
 // reset the vertical position state using the last height measurement
 void NavEKF2_core::ResetHeight(void)
 {
+    // Store the position before the reset so that we can record the reset delta
+    posResetD = stateStruct.position.z;
+
     // write to the state vector
     stateStruct.position.z = -hgtMea;
     outputDataNew.position.z = stateStruct.position.z;
@@ -116,6 +119,12 @@ void NavEKF2_core::ResetHeight(void)
         storedOutput[i].position.z = stateStruct.position.z;
     }
 
+    // Calculate the position jump due to the reset
+    posResetD = stateStruct.position.z - posResetD;
+
+    // store the time of the reset
+    lastPosResetD_ms = imuSampleTime_ms;
+
     // reset the corresponding covariances
     zeroRows(P,8,8);
     zeroCols(P,8,8);

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -138,6 +138,7 @@ void NavEKF2_core::InitialiseVariables()
     lastPreAlignGpsCheckTime_ms = imuSampleTime_ms;
     lastPosReset_ms = 0;
     lastVelReset_ms = 0;
+    lastPosResetD_ms = 0;
     lastRngMeasTime_ms = 0;
     terrainHgtStableSet_ms = 0;
 
@@ -239,6 +240,7 @@ void NavEKF2_core::InitialiseVariables()
     sideSlipFusionDelayed = false;
     posResetNE.zero();
     velResetNE.zero();
+    posResetD = 0.0f;
     hgtInnovFiltState = 0.0f;
     if (_ahrs->get_compass()) {
         magSelectIndex = _ahrs->get_compass()->get_primary();

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -261,6 +261,10 @@ public:
     // returns the time of the last reset or 0 if no reset has ever occurred
     uint32_t getLastPosNorthEastReset(Vector2f &pos) const;
 
+    // return the amount of D position change due to the last position reset in metres
+    // returns the time of the last reset or 0 if no reset has ever occurred
+    uint32_t getLastPosDownReset(float &posD) const;
+
     // return the amount of NE velocity change due to the last velocity reset in metres/sec
     // returns the time of the last reset or 0 if no reset has ever occurred
     uint32_t getLastVelNorthEastReset(Vector2f &vel) const;
@@ -808,6 +812,8 @@ private:
     uint32_t lastPosReset_ms;       // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
     Vector2f velResetNE;            // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
     uint32_t lastVelReset_ms;       // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
+    float posResetD;                // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
+    uint32_t lastPosResetD_ms;      // System time at which the last position reset occurred. Returned by getLastPosDownReset
     float yawTestRatio;             // square of magnetometer yaw angle innovation divided by fail threshold
     Quaternion prevQuatMagReset;    // Quaternion from the last time the magnetic field state reset condition test was performed
     uint8_t fusionHorizonOffset;    // number of IMU samples that the fusion time horizon  has been shifted to prevent multiple EKF instances fusing data at the same time