Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: handle rangefinder glitches in alt tracking

Browse Source 
this handles glitches from the rangefinder when tracking the
ground. It requires 3 readings in a row to settle on a new target
altitude

thanks to discussions with Leonard and Randy
This commit is contained in:
Andrew Tridgell 2016-11-20 17:26:51 +11:00
parent 939fb8ddd1
commit bcea3ac8d4
3 changed files with 34 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/Attitude.cpp
View File

@ -244,9 +244,31 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
target_rangefinder_alt += target_rate * dt;
}
// do not let target altitude get too far from current altitude above ground
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z());
/*
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
*/
int32_t glitch_cm = rangefinder_state.alt_cm - target_rangefinder_alt;
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;
}
if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
// shift to the new rangefinder reading
target_rangefinder_alt = rangefinder_state.alt_cm;
rangefinder_state.glitch_count = 0;
}
if (rangefinder_state.glitch_count != 0) {
// we are currently glitching, just use the target rate
return target_rate;
}
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);

1
ArduCopter/Copter.h
View File

@ -195,6 +195,7 @@ 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;
} rangefinder_state = { false, false, 0, 0 };
AP_RPM rpm_sensor;

8
ArduCopter/config.h
View File

@ -151,6 +151,14 @@
# define RANGEFINDER_TILT_CORRECTION ENABLED
#endif
#ifndef RANGEFINDER_GLITCH_ALT_CM
# define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch
#endif
#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES
# define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading
#endif
//////////////////////////////////////////////////////////////////////////////
// Proximity sensor
//