Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-21 20:53:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Add pre-arm check for horizontal inertial errors

Browse Source 
This check will declare the EKF as unhealthy if the horizontal position innovations exceed a threshold  before motors are armed.
This will help to prevent a takeoff with bad inertial data caused by bad accel or gyro offsets.
This commit is contained in:
Paul Riseborough
2015-05-15 10:35:25 +09:00
committed by Randy Mackay
parent 1b13250fd8
commit bd91b9727f
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_NavEKF/AP_NavEKF.cpp
+3 -2
View File
@@ -442,8 +442,9 @@ bool NavEKF::healthy(void) const
if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
return false;
}
// barometer innovations must be within limits when on-ground
if (!vehicleArmed && (fabsf(innovVelPos[5]) > 1.0f)) {
// barometer and position innovations must be within limits when on-ground
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (!vehicleArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
return false;
}