Browse Source

AP_NavEKF3: fix vertical flyaways when rangefinder stops providing data

c415-sdk
Dr.-Ing. Amilcar do Carmo Lucas 5 years ago committed by Randy Mackay
parent
commit
fca8d33c50
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@ -54,6 +54,8 @@ void NavEKF3_core::readRangeFinder(void)
} }
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f; storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
} else {
continue;
} }
// check for three fresh samples // check for three fresh samples

Loading…
Cancel
Save