diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index fd8a079871..becae6ce4b 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -438,6 +438,12 @@ public: return 0; }; + // return the amount of vertical position change due to the last reset in meters + // returns the time of the last reset or 0 if no reset has ever occurred + virtual uint32_t getLastPosDownReset(float &posDelta) const { + 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 75844faaf7..bf795a9fda 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -1191,6 +1191,24 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const return 0; } + +// return the amount of vertical position change due to the last reset in meters +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) const +{ + switch (ekf_type()) { + case 1: + return 0; + case 2: + return EKF2.getLastPosDownReset(posDelta); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKF_TYPE_SITL: + return 0; +#endif + } + 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 c256d563e6..c452f48003 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -193,6 +193,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 meters + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastPosDownReset(float &posDelta) const; + // 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