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

AP_NavEKF: Protect against bad delta time from the INS library 

Prevent arithmetic divide by zero exceptions and handle invalid delta time in a consistent way by setting invalid times to the average.
This commit is contained in:
Paul Riseborough 2015-11-16 08:51:46 +11:00 committed by Andrew Tridgell
parent 2fb5a4489b
commit e32e6cfa67
1 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF/AP_NavEKF_core.cpp
View File

@ -3799,6 +3799,10 @@ bool NavEKF_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dV
if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel);
dVel_dt = ins.get_delta_velocity_dt(ins_index);
// catch invalid delta time
if (dVel_dt <= 0.0f) {
dVel_dt = dtIMUavg;
}
return true;
}
return false;
@ -3819,8 +3823,15 @@ void NavEKF_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
dtIMUavg = 1.0f/ins.get_sample_rate();
dtDelAng = max(ins.get_delta_time(),1.0e-4f);
// calculate the average time between IMU updates
dtIMUavg = 1.0f/max(ins.get_sample_rate(),1E-5f);
// calculate the most recent time between gyro delta angle updates
if (ins.get_delta_time() > 0.0f) {
dtDelAng = ins.get_delta_time();
} else {
dtDelAng = dtIMUavg;
}
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = hal.scheduler->millis();