Browse Source

AP_NavEKF: Increase flow data valid timeout to handle arming delays

When Copter arms, the AHRS/EKF may not be run for a few hundred msec depending on conditions. This can cause the arming check to fail the optical flow sensor and place the EKF in a constant position mode.
master
priseborough 10 years ago committed by Randy Mackay
parent
commit
04810c012d
  1. 2
      libraries/AP_NavEKF/AP_NavEKF.cpp

2
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -911,7 +911,7 @@ void NavEKF::SelectFlowFusion() @@ -911,7 +911,7 @@ void NavEKF::SelectFlowFusion()
{
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 200);
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);
// check is the terrain offset estimate is still valid

Loading…
Cancel
Save