From c40c3632bb98ca25ae511acfa40d628705bcf847 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Fri, 9 Jan 2015 23:32:13 +1100
Subject: [PATCH] AP_NavEKF: Critical Bug Fix
Prevents possible loss of attitude reference for flights without optical flow and GPS.
The optical flow measurement timeout can reset the velocity states which decouples the position states from IMU errors and therefore significantly reduces the amount of attitude error correction.
---
libraries/AP_NavEKF/AP_NavEKF.cpp | 7 ++++---
1 file changed, 4 insertions(+), 3 deletions(-)
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 16ec330fb..89526baf3 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -921,11 +921,12 @@ void NavEKF::SelectFlowFusion()
// check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature.
bool delayFusion = ((covPredStep || magFusePerformed) && !(flowFuseNow || inhibitLoadLeveling));
- // if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in position hold mode
- if (!flowDataValid && !constPosMode && PV_AidingMode == AID_RELATIVE) {
+ // if we don't have valid flow measurements and are relying on them, dead reckon using current velocity vector
+ // If the flow measurements have been rejected for too long and we are relying on them, then reset the velocities to an estimate based on the flow and range data
+ if (!flowDataValid && PV_AidingMode == AID_RELATIVE) {
constVelMode = true;
constPosMode = false; // always clear constant position mode if constant velocity mode is active
- } else if (flowDataValid && flowFusionTimeout) {
+ } else if (flowDataValid && flowFusionTimeout && PV_AidingMode == AID_RELATIVE) {
// we need to reset the velocities to a value estimated from the flow data
// estimate the range
float initPredRng = max((terrainState - state.position[2]),0.1f) / Tnb_flow.c.z;
--
GitLab