From f67f6b01f426fd1d306a4a9acd2cf239313c3ed1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 29 Oct 2015 16:18:04 +1100 Subject: [PATCH] AP_AHRS: Publish the EKF position and velocity reset deltas --- libraries/AP_AHRS/AP_AHRS.h | 12 ++++++++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 26 ++++++++++++++++++++++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 8 ++++++++ 3 files changed, 46 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index bffe898418..888ed234ae 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -392,6 +392,18 @@ public: return false; }; + // return the amount of NE position change in metres due to the last reset + // returns the time of the last reset or 0 if no reset has ever occurred + virtual uint32_t getLastPosNorthEastReset(Vector2f &pos) { + return 0; + }; + + // return the amount of NE velocity change in metres/sec due to the last reset + // returns the time of the last reset or 0 if no reset has ever occurred + virtual uint32_t getLastVelNorthEastReset(Vector2f &vel) { + return 0; + }; + // Resets the baro so that it reads zero at the current height // Resets the EKF height to zero // Adjusts the EKf origin height so that the EKF height + origin height is the same as before diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 9432a49bea..b60db8536b 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -740,6 +740,32 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) return false; } +// return the amount of NE position change in metres due to the last reset +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos) +{ + switch (ekf_type()) { + case 1: + return EKF1.getLastPosNorthEastReset(pos); + case 2: + return EKF2.getLastPosNorthEastReset(pos); + } + return 0; +} + +// return the amount of NE velocity change in metres/sec due to the last reset +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) +{ + switch (ekf_type()) { + case 1: + return EKF1.getLastVelNorthEastReset(vel); + case 2: + return EKF2.getLastVelNorthEastReset(vel); + } + return 0; +} + // Resets the baro so that it reads zero at the current height // Resets the EKF height to zero // Adjusts the EKf origin height so that the EKF height + origin height is the same as before diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 099aaddf7e..47f77bc0f8 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -167,6 +167,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 in metres due to the last reset + // 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 in metres/sec due to the last reset + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastVelNorthEastReset(Vector2f &vel); + // Resets the baro so that it reads zero at the current height // Resets the EKF height to zero // Adjusts the EKf origin height so that the EKF height + origin height is the same as before