Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Fix bug preventing reset of velocity after OF fusion timeout

Browse Source
This commit is contained in:
Paul Riseborough 2015-03-30 01:38:43 -07:00 committed by Jonathan Challinger
parent 95cd3480ec
commit d4c60ca956
2 changed files with 1 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -934,7 +934,7 @@ void NavEKF::SelectFlowFusion()
// Check if the optical flow data is still valid // Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
// Check if the fusion has timed out (flow measurements have been rejected for too long) // 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 // check is the terrain offset estimate is still valid
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000); gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// Perform tilt check // Perform tilt check
@ -1002,8 +1002,6 @@ void NavEKF::SelectFlowFusion()
newDataFlow = false; newDataFlow = false;
// indicate that flow fusion has been performed. This is used for load spreading. // indicate that flow fusion has been performed. This is used for load spreading.
flowFusePerformed = true; 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 // 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; timeAtLastAuxEKF_ms = imuSampleTime_ms;
flowValidMeaTime_ms = imuSampleTime_ms; flowValidMeaTime_ms = imuSampleTime_ms;
flowMeaTime_ms = 0; flowMeaTime_ms = 0;
prevFlowUseTime_ms = imuSampleTime_ms;
prevFlowFuseTime_ms = imuSampleTime_ms; prevFlowFuseTime_ms = imuSampleTime_ms;
gndHgtValidTime_ms = 0; gndHgtValidTime_ms = 0;
ekfStartTime_ms = imuSampleTime_ms; ekfStartTime_ms = imuSampleTime_ms;

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -672,7 +672,6 @@ private:
float innovRng; // range finder observation innovation (m) float innovRng; // range finder observation innovation (m)
float rngMea; // range finder measurement (m) float rngMea; // range finder measurement (m)
bool inhibitGndState; // true when the terrain position state is to remain constant 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 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 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 float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator