From 015f700bc01e9507538a7a0e61d50076d69ccdcc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 24 Sep 2015 15:50:14 +0900 Subject: [PATCH] AP_NavEKF2: getLastYawResetAngle returns last reset time --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 5 ++--- libraries/AP_NavEKF2/AP_NavEKF2.h | 5 ++--- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 27 ++++++++++-------------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 7 +++--- 4 files changed, 18 insertions(+), 26 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 36b806252d..6dec644edf 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -771,9 +771,8 @@ bool NavEKF2::getHeightControlLimit(float &height) const } // return the amount of yaw angle change due to the last yaw angle reset in radians -// returns true if a reset yaw angle has been updated and not queried -// this function should not have more than one client -bool NavEKF2::getLastYawResetAngle(float &yawAng) +// returns the time of the last yaw angle reset or 0 if no reset has ever occurred +uint32_t NavEKF2::getLastYawResetAngle(float &yawAng) { if (!core) { return false; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index c517187678..9eb3836448 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -214,9 +214,8 @@ public: bool getHeightControlLimit(float &height) const; // return the amount of yaw angle change due to the last yaw angle reset in radians - // returns true if a reset yaw angle has been updated and not queried - // this function should not have more than one client - bool getLastYawResetAngle(float &yawAng); + // returns the time of the last yaw angle reset or 0 if no reset has ever occurred + uint32_t getLastYawResetAngle(float &yawAng); // 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_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 43e03dca0c..2b71deb1a8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -4015,10 +4015,12 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset Vector3f tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); - yawResetAngle = wrap_PI(yaw - tempEuler.z); - // set the flag to indicate that an in-flight yaw reset has been performed - // this will be cleared when the reset value is retrieved - yawResetAngleWaiting = true; + // this check ensures we accumulate the resets that occur within a single iteration of the EKF + if (imuSampleTime_ms != lastYawReset_ms) { + yawResetAngle = 0.0f; + } + yawResetAngle += wrap_PI(yaw - tempEuler.z); + lastYawReset_ms = imuSampleTime_ms; // calculate an initial quaternion using the new yaw value initQuat.from_euler(roll, pitch, yaw); } else { @@ -4230,7 +4232,7 @@ void NavEKF2_core::InitialiseVariables() highYawRate = false; yawRateFilt = 0.0f; yawResetAngle = 0.0f; - yawResetAngleWaiting = false; + lastYawReset_ms = 0; tiltErrFilt = 1.0f; tiltAlignComplete = false; yawAlignComplete = false; @@ -4810,18 +4812,11 @@ void NavEKF2_core::alignMagStateDeclination() } // return the amount of yaw angle change due to the last yaw angle reset in radians -// returns true if a reset yaw angle has been updated and not queried -// this function should not have more than one client -bool NavEKF2_core::getLastYawResetAngle(float &yawAng) +// returns the time of the last yaw angle reset or 0 if no reset has ever occurred +uint32_t NavEKF2_core::getLastYawResetAngle(float &yawAng) { - if (yawResetAngleWaiting) { - yawAng = yawResetAngle; - yawResetAngleWaiting = false; - return true; - } else { - yawAng = yawResetAngle; - return false; - } + yawAng = yawResetAngle; + return lastYawReset_ms; } // Fuse compass measurements usinga simple declination observation model that doesn't use magnetic field states diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index ece0d899d2..e4a8ed985a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -207,9 +207,8 @@ public: bool getHeightControlLimit(float &height) const; // return the amount of yaw angle change due to the last yaw angle reset in radians - // returns true if a reset yaw angle has been updated and not queried - // this function should not have more than one client - bool getLastYawResetAngle(float &yawAng); + // returns the time of the last yaw angle reset or 0 if no reset has ever occurred + uint32_t getLastYawResetAngle(float &yawAng); private: // Reference to the global EKF frontend for parameters @@ -668,7 +667,7 @@ private: float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference bool useGpsVertVel; // true if GPS vertical velocity should be used float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased. - bool yawResetAngleWaiting; // true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function + uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle Vector3f tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion float tiltErrFilt; // Filtered tilt error metric bool tiltAlignComplete; // true when tilt alignment is complete