AP_NavEKF : fix velocity timeout bug during optical flow operation
This commit is contained in:
parent
cc8e6dbfad
commit
9b9934ac06
@ -3011,6 +3011,8 @@ void NavEKF::FuseOptFlow()
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds
|
||||
if (flowTestRatio[obsIndex] < 1.0f) {
|
||||
// Reset the timer for velocity fusion timeout. This prevents a velocity timeout from being declared if we have to momentarily go into velocity hold mode.
|
||||
velFailTime = imuSampleTime_ms;
|
||||
// correct the state vector
|
||||
for (uint8_t j = 0; j <= 21; j++)
|
||||
{
|
||||
|
Loading…
Reference in New Issue
Block a user