From d4c60ca9563ec364bb19bc96c44d2a8a2ad699c5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 30 Mar 2015 01:38:43 -0700 Subject: [PATCH] AP_NavEKF: Fix bug preventing reset of velocity after OF fusion timeout --- libraries/AP_NavEKF/AP_NavEKF.cpp | 5 +---- libraries/AP_NavEKF/AP_NavEKF.h | 1 - 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 90c7b60fba..c550751f42 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -934,7 +934,7 @@ void NavEKF::SelectFlowFusion() // Check if the optical flow data is still valid flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); // Check if the fusion has timed out (flow measurements have been rejected for too long) - bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowUseTime_ms) > 5000); + bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); // check is the terrain offset estimate is still valid gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000); // Perform tilt check @@ -1002,8 +1002,6 @@ void NavEKF::SelectFlowFusion() newDataFlow = false; // indicate that flow fusion has been performed. This is used for load spreading. flowFusePerformed = true; - // update the time stamp - prevFlowUseTime_ms = imuSampleTime_ms; } // Apply corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output @@ -4385,7 +4383,6 @@ void NavEKF::InitialiseVariables() timeAtLastAuxEKF_ms = imuSampleTime_ms; flowValidMeaTime_ms = imuSampleTime_ms; flowMeaTime_ms = 0; - prevFlowUseTime_ms = imuSampleTime_ms; prevFlowFuseTime_ms = imuSampleTime_ms; gndHgtValidTime_ms = 0; ekfStartTime_ms = imuSampleTime_ms; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 7a0e76b627..944a72e5af 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -672,7 +672,6 @@ private: float innovRng; // range finder observation innovation (m) float rngMea; // range finder measurement (m) bool inhibitGndState; // true when the terrain position state is to remain constant - uint32_t prevFlowUseTime_ms; // time the last flow measurement scheduled for fusion (doesn't mean it passed the innovatio consistency checks) uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator