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