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

AP_NavEKF2: Fix bug in handling of invalid range finder data

Browse Source
This commit is contained in:
priseborough 2016-08-20 11:31:09 +10:00 committed by Randy Mackay
parent 175faf1e41
commit 8797714203
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -82,6 +82,9 @@ void NavEKF2_core::readRangeFinder(void)
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange.push(rangeDataNew);
// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms;
} else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
// before takeoff we assume on-ground range value if there is no data
rangeDataNew.time_ms = imuSampleTime_ms;
@ -96,13 +99,13 @@ void NavEKF2_core::readRangeFinder(void)
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange.push(rangeDataNew);
}
// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms;
}
}
}
}
// write the raw optical flow measurements
// this needs to be called externally.