diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 5e95046599..17d2dc3485 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -819,20 +819,20 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAng) const // 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::getLastPosNorthEastReset(Vector2f &pos) +uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &pos) const { if (!core) { - return false; + return 0; } return core->getLastPosNorthEastReset(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 NavEKF2::getLastVelNorthEastReset(Vector2f &vel) +uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel) const { if (!core) { - return false; + return 0; } return core->getLastVelNorthEastReset(vel); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 5ce742f33c..8cd0ac9f71 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -229,11 +229,11 @@ public: // 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); + uint32_t getLastPosNorthEastReset(Vector2f &pos) 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); + uint32_t getLastVelNorthEastReset(Vector2f &vel) const; // allow the enable flag to be set by Replay void set_enable(bool enable) { _enable.set(enable); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 10f61d1717..bc6e2a5380 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -130,7 +130,7 @@ uint32_t NavEKF2_core::getLastYawResetAngle(float &yawAng) const // 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) +uint32_t NavEKF2_core::getLastPosNorthEastReset(Vector2f &pos) const { pos = posResetNE; return lastPosReset_ms; @@ -138,7 +138,7 @@ uint32_t NavEKF2_core::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 NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel) +uint32_t NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel) const { vel = velResetNE; return lastVelReset_ms; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 52a8a2c28e..1ba4aafa94 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -250,11 +250,11 @@ public: // 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); + uint32_t getLastPosNorthEastReset(Vector2f &pos) 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); + uint32_t getLastVelNorthEastReset(Vector2f &vel) const; private: // Reference to the global EKF frontend for parameters