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

Copter: provide filtered range finder altitude to AC_WPNav

This commit is contained in:
Randy Mackay 2016-04-28 12:30:41 +09:00
parent 2bafc36ded
commit 59070653cc
3 changed files with 26 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.h
View File

@ -175,7 +175,9 @@ private:
bool enabled:1;
bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder
} rangefinder_state = { true, false, 0 };
uint32_t last_healthy_ms;
LowPassFilterFloat alt_cm_filt; // altitude filter
} rangefinder_state = { true, false, 0, 0 };
#endif
AP_RPM rpm_sensor;

4
ArduCopter/config.h
View File

@ -140,6 +140,10 @@
# define RANGEFINDER_TIMEOUT_MS 1000 // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
#endif
#ifndef RANGEFINDER_WPNAV_FILT_HZ
# define RANGEFINDER_WPNAV_FILT_HZ 0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class
#endif
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
# define RANGEFINDER_TILT_CORRECTION ENABLED
#endif

26
ArduCopter/sensors.cpp
View File

@ -30,6 +30,7 @@ void Copter::init_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.init();
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
#endif
}
@ -39,13 +40,7 @@ void Copter::read_rangefinder(void)
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.update();
// exit immediately if rangefinder is disabled
if (rangefinder.status() != RangeFinder::RangeFinder_Good) {
rangefinder_state.alt_healthy = false;
return;
}
rangefinder_state.alt_healthy = (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX);
rangefinder_state.alt_healthy = ((rangefinder.status() == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX));
int16_t temp_alt = rangefinder.distance_cm();
@ -55,6 +50,23 @@ void Copter::read_rangefinder(void)
#endif
rangefinder_state.alt_cm = temp_alt;
// filter rangefinder for use by AC_WPNav
uint32_t now = AP_HAL::millis();
if (rangefinder_state.alt_healthy) {
if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
// reset filter if we haven't used it within the last second
rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
} else {
rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.1f);
}
rangefinder_state.last_healthy_ms = now;
}
// 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());
#else
rangefinder_state.alt_healthy = false;
rangefinder_state.alt_cm = 0;