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

Copter: rangefinder glitch detection moved to read_rangefinder

This commit is contained in:
Randy Mackay 2019-08-22 21:05:45 +09:00
parent 808efa4c79
commit d5d7fbe761
3 changed files with 32 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/Copter.h
View File

@ -288,7 +288,9 @@ private:
int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder
uint32_t last_healthy_ms;
LowPassFilterFloat alt_cm_filt; // altitude filter
int8_t glitch_count;
int16_t alt_cm_glitch_protected; // last glitch protected altitude
int8_t glitch_count; // non-zero number indicates rangefinder is glitching
uint32_t glitch_cleared_ms; // system time glitch cleared
} rangefinder_state;
class SurfaceTracking {
@ -307,6 +309,7 @@ private:
private:
float target_alt_cm; // desired altitude in cm above the ground
uint32_t last_update_ms; // system time of last update to target_alt_cm
uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery
bool valid_for_logging; // true if target_alt_cm is valid for logging
} surface_tracking;

21
ArduCopter/sensors.cpp
View File

@ -35,8 +35,29 @@ void Copter::read_rangefinder(void)
temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
#endif
// tilt corrected but unfiltered, not glitch protected alt
rangefinder_state.alt_cm = temp_alt;
// glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
// are considered a glitch and glitch_count becomes non-zero
// glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
// glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
const int32_t glitch_cm = rangefinder_state.alt_cm - rangefinder_state.alt_cm_glitch_protected;
if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
rangefinder_state.glitch_count = MAX(rangefinder_state.glitch_count+1, 1);
} else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
rangefinder_state.glitch_count = MIN(rangefinder_state.glitch_count-1, -1);
} else {
rangefinder_state.glitch_count = 0;
rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
}
if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
// clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
rangefinder_state.glitch_count = 0;
rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
rangefinder_state.glitch_cleared_ms = AP_HAL::millis();
}
// filter rangefinder for use by AC_WPNav
uint32_t now = AP_HAL::millis();

31
ArduCopter/surface_tracking.cpp
View File

@ -6,8 +6,9 @@
float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
{
#if RANGEFINDER_ENABLED == ENABLED
if (!copter.rangefinder_alt_ok()) {
// if rangefinder is not ok, do not use surface tracking
// if rangefinder alt is not ok or glitching, do not do surface tracking
if (!copter.rangefinder_alt_ok() ||
(copter.rangefinder_state.glitch_count != 0)) {
return target_rate;
}
@ -18,6 +19,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
const uint32_t now = millis();
if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt;
last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
}
last_update_ms = now;
@ -27,30 +29,11 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
}
valid_for_logging = true;
/*
handle rangefinder glitches. When we get a rangefinder reading
more than RANGEFINDER_GLITCH_ALT_CM different from the current
rangefinder reading then we consider it a glitch and reject
until we get RANGEFINDER_GLITCH_NUM_SAMPLES samples in a
row. When that happens we reset the target altitude to the new
reading
*/
const int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm;
if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
copter.rangefinder_state.glitch_count = MAX(copter.rangefinder_state.glitch_count+1,1);
} else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
copter.rangefinder_state.glitch_count = MIN(copter.rangefinder_state.glitch_count-1,-1);
} else {
copter.rangefinder_state.glitch_count = 0;
}
if (abs(copter.rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
// handle glitch recovery by resetting target
if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) {
// shift to the new rangefinder reading
target_alt_cm = copter.rangefinder_state.alt_cm;
copter.rangefinder_state.glitch_count = 0;
}
if (copter.rangefinder_state.glitch_count != 0) {
// we are currently glitching, just use the target rate
return target_rate;
last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
}
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)