From 0afc9bf7247719ea5a81d60d1ed46e4b375f20c9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 5 Nov 2018 12:55:40 +1100 Subject: [PATCH] AP_Mount: SoloGimbal: avoid calling safe_sqrtf --- libraries/AP_Mount/SoloGimbalEKF.cpp | 6 +++--- libraries/AP_Mount/SoloGimbalEKF.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/SoloGimbalEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp index ebdb37ff36..5784326db8 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.cpp +++ b/libraries/AP_Mount/SoloGimbalEKF.cpp @@ -43,7 +43,7 @@ void SoloGimbalEKF::reset() memset(&states,0,sizeof(states)); memset((void *)&gSense,0,sizeof(gSense)); memset(&Cov,0,sizeof(Cov)); - TiltCorrection = 0; + TiltCorrectionSquared = 0; StartTime_ms = 0; FiltInit = false; lastMagUpdate = 0; @@ -137,7 +137,7 @@ void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const // Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold // Force it to align if too much time has lapsed - if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) { + if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrectionSquared < sq(1e-4f)) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) { //calculate the initial heading using magnetometer, estimated tilt and declination alignHeading(); YawAligned = true; @@ -661,7 +661,7 @@ void SoloGimbalEKF::fuseVelocity() } // calculate tilt component of angle correction - TiltCorrection = safe_sqrt(sq(angErrVec.x) + sq(angErrVec.y)); + TiltCorrectionSquared = sq(angErrVec.x) + sq(angErrVec.y); } // check for new magnetometer data and update store measurements if available diff --git a/libraries/AP_Mount/SoloGimbalEKF.h b/libraries/AP_Mount/SoloGimbalEKF.h index b7033ef6b4..477f2020e0 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.h +++ b/libraries/AP_Mount/SoloGimbalEKF.h @@ -120,7 +120,7 @@ private: float Cov[9][9]; // covariance matrix Matrix3f Tsn; // Sensor to NED rotation matrix - float TiltCorrection; // Angle correction applied to tilt from last velocity fusion (rad) + float TiltCorrectionSquared; // Angle correction applied to tilt from last velocity fusion (rad) bool newDataMag; // true when new magnetometer data is waiting to be used uint32_t StartTime_ms; // time the EKF was started (msec) bool FiltInit; // true when EKF is initialised