Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: prevent NaN if RNGFND_GNDCLEAR is zero

This commit is contained in:
priseborough 2017-02-15 07:50:30 +11:00 committed by Randy Mackay
parent 5cd403c0b9
commit e399f57ffa
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -22,7 +22,8 @@ void NavEKF3_core::readRangeFinder(void)
uint8_t maxIndex;
uint8_t minIndex;
// get theoretical correct range when the vehicle is on the ground
rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f;
// don't allow range to go below 5cm becasue this can cause problems with optical flow processing
rngOnGnd = MAX(frontend->_rng.ground_clearance_cm() * 0.01f, 0.05f);
// limit update rate to maximum allowed by data buffers
if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {