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

AP_NavEKF2: prevent NaN if RNGFND_GNDCLEAR is zero

Browse Source
This commit is contained in:
priseborough 2017-02-15 07:50:19 +11:00 committed by Randy Mackay
parent bf51da224a
commit 5cd403c0b9
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_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -24,7 +24,8 @@ void NavEKF2_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);
// read range finder at 20Hz
// TODO better way of knowing if it has new data