aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: protect against position-delta sensor data being NaN

Browse Source
This commit is contained in:
Randy Mackay 2020-06-15 13:28:15 +09:00
parent 331f2f5fe7
commit ca0ae57b56
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -120,6 +120,11 @@ void NavEKF3_core::readRangeFinder(void)
void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset) void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{ {
// protect against NaN
if (isnan(quality) || delPos.is_nan() || delAng.is_nan() || isnan(delTime) || posOffset.is_nan()) {
return;
}
// limit update rate to maximum allowed by sensor buffers and fusion process // limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised // don't try to write to buffer until the filter has been initialised
if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) { if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {