From 69d4bd24819948ba6e188e0b1fd713a0945c4587 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 30 Oct 2015 10:03:13 +1100 Subject: [PATCH] AP_NavEKF: Protect against baro data errors in constant position mode Large baro data errors when flying without GPS could cause total failure of the EKF. This patch provides protection against this happening in-flight but allows for large innovations during preflight alignment. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 007e8008b7..084f871057 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2154,8 +2154,10 @@ void NavEKF::FuseVelPosNED() // fail if the ratio is > 1, but don't fail if bad IMU data hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime; - // Fuse height data if healthy or timed out or in constant position mode - if (hgtHealth || hgtTimeout || constPosMode) { + // Fuse height data if healthy + // Force a reset if timed out to prevent the possibility of inertial errors causing persistent loss of height reference + // Force fusion in constant position mode on the ground to allow large accelerometer biases to be learned without rejecting barometer + if (hgtHealth || hgtTimeout || (constPosMode && !vehicleArmed)) { // Calculate a filtered value to be used by pre-flight health checks // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution if (!vehicleArmed) {