From bd91b9727fd5aac42fb360822fe5228bbcfdce72 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 14 May 2015 20:31:43 +1000 Subject: [PATCH] AP_NavEKF: Add pre-arm check for horizontal inertial errors 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. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 78d65424de..8f4e532420 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -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; }