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

AP_NavEKF2: Fix bug in handling of invalid range finder data

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

9
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,10 +99,10 @@ 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;
}
rngValidMeaTime_ms = imuSampleTime_ms;
}
}
}