From 4fdec67546a7d2ecfa7e827c9b9ad73d0759969d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Thu, 29 Oct 2015 15:03:51 +1100
Subject: [PATCH] AP_NavEKF2: Publish the position and velocity state reset
 deltas

Used by the control loops to compensate for step changes in position and velocity when the EKF needs to do a reset of these states.
---
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 16 ++++++++++++++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.h      | 12 ++++++++++++
 2 files changed, 28 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index 5aac1e53b7..5fb72d8ef9 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -128,6 +128,22 @@ uint32_t NavEKF2_core::getLastYawResetAngle(float &yawAng)
     return lastYawReset_ms;
 }
 
+// return the amount of NE 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 NavEKF2_core::getLastPosNorthEastReset(Vector2f &pos)
+{
+    pos = posResetNE;
+    return lastPosReset_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)
+{
+    vel = velResetNE;
+    return lastVelReset_ms;
+}
+
 // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
 void NavEKF2_core::getWind(Vector3f &wind) const
 {
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index fb24b8b129..3ec75a5687 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -248,6 +248,14 @@ public:
     // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
     uint32_t getLastYawResetAngle(float &yawAng);
 
+    // return the amount of NE 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 getLastPosNorthEastReset(Vector2f &pos);
+
+    // 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);
+
 private:
     // Reference to the global EKF frontend for parameters
     NavEKF2 &frontend;
@@ -765,6 +773,10 @@ private:
     bool sideSlipFusionDelayed;     // true when the sideslip fusion has been delayed
     bool magFuseTiltInhibit;        // true when the 3-axis magnetoemter fusion is prevented from changing tilt angle
     uint32_t magFuseTiltInhibit_ms; // time in msec that the condition indicated by magFuseTiltInhibit was commenced
+    Vector2f posResetNE;            // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
+    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
 
     // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.