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

Copter: avoid terrain failsafe from a single out-of-range rangefinder reading

This commit is contained in:
Randy Mackay 2019-06-26 11:16:30 +09:00
parent 67ff96d8de
commit 8e095d4a81
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/sensors.cpp
View File

@ -40,8 +40,10 @@ void Copter::read_rangefinder(void)
// filter rangefinder for use by AC_WPNav
uint32_t now = AP_HAL::millis();
const bool timed_out = now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
if (rangefinder_state.alt_healthy) {
if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
if (timed_out) {
// reset filter if we haven't used it within the last second
rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
} else {
@ -51,7 +53,9 @@ void Copter::read_rangefinder(void)
}
// send rangefinder altitude and health to waypoint navigation library
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
if (rangefinder_state.alt_healthy || timed_out) {
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
}
#else
rangefinder_state.enabled = false;