mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Mount: SoloGimbal: avoid calling safe_sqrtf
This commit is contained in:
parent
126065e95c
commit
0afc9bf724
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user