Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: don't run when previously disarmed and time has slipped

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-16 18:50:14 -07:00 committed by Randy Mackay
parent 085faaac6a
commit 10476333d8
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -699,6 +699,19 @@ void NavEKF::UpdateFilter()
// read IMU data and convert to delta angles and velocities
readIMUData();
static bool prev_armed = false;
bool armed = getVehicleArmStatus();
// the vehicle was previously disarmed and time has slipped
// gyro auto-zero has likely just been done - skip this timestep
if (!prev_armed && dtIMUactual > dtIMUavg*5.0f) {
// stop the timer used for load measurement
perf_end(_perf_UpdateFilter);
prev_armed = armed;
return;
}
prev_armed = armed;
// detect if the filter update has been delayed for too long
if (dtIMUactual > 0.2f) {
// we have stalled for too long - reset states