Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Remove potential for division by zero

This commit is contained in:
Paul Riseborough 2015-10-30 17:07:38 +11:00 committed by Randy Mackay
parent 5533a9a149
commit b15bf3243e
2 changed files with 29 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -479,7 +479,7 @@ void NavEKF2_core::FuseMagnetometer()
// calculate the measurement innovation
innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
// calculate the innovation test ratio
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(frontend._magInnovGate) * varInnovMag[obsIndex]);
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(max(frontend._magInnovGate,1)) * varInnovMag[obsIndex]);
// check the last values from all components and set magnetometer health accordingly
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
// Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
@ -632,7 +632,23 @@ void NavEKF2_core::fuseCompass()
}
varInnov += H_MAG[rowIndex]*PH[rowIndex];
}
float varInnovInv = 1.0f / varInnov;
float varInnovInv;
if (varInnov >= R_MAG) {
varInnovInv = 1.0f / varInnov;
// All three magnetometer components are used in this measurement, so we output health status on three axes
faultStatus.bad_xmag = false;
faultStatus.bad_ymag = false;
faultStatus.bad_zmag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
// All three magnetometer components are used in this measurement, so we output health status on three axes
faultStatus.bad_xmag = true;
faultStatus.bad_ymag = true;
faultStatus.bad_zmag = true;
return;
}
for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
Kfusion[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
@ -655,7 +671,7 @@ void NavEKF2_core::fuseCompass()
}
// calculate the innovation test ratio
yawTestRatio = sq(innovation) / (sq(frontend._magInnovGate) * varInnov);
yawTestRatio = sq(innovation) / (sq(max(frontend._magInnovGate,1)) * varInnov);
// Declare the magnetometer unhealthy if the innovation test fails
if (yawTestRatio > 1.0f) {

14
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -423,7 +423,15 @@ void NavEKF2_core::StoreIMU_reset()
void NavEKF2_core::RecallIMU()
{
imuDataDelayed = storedIMU[fifoIndexDelayed];
// make sure that the delta time used for the delta angles and velocities are is no less than 10% of dtIMUavg to prevent
// divide by zero problems when converting to rates or acceleration
float minDT = 0.1f*dtIMUavg;
imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT);
}
// read the delta velocity and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
const AP_InertialSensor &ins = _ahrs->get_ins();
@ -435,9 +443,6 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d
return false;
}
/********************************************************
* Global Position Measurement *
********************************************************/
@ -632,7 +637,8 @@ bool NavEKF2_core::RecallGPS()
}
}
// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
const AP_InertialSensor &ins = _ahrs->get_ins();