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

Copter: surface tracking always includes ekf alt error when setting target rangefinder alt

This commit is contained in:
Randy Mackay 2019-08-22 21:57:52 +09:00 committed by Andrew Tridgell
parent 7d66b904ef
commit 972c916f1b
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/surface_tracking.cpp
View File

@ -12,13 +12,13 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
return target_rate; return target_rate;
} }
const float current_alt = copter.inertial_nav.get_altitude(); // calculate current ekf based altitude error
const float current_alt_target = copter.pos_control->get_alt_target(); const float current_alt_error = copter.pos_control->get_alt_target() - copter.inertial_nav.get_altitude();
// reset target altitude if this controller has just been engaged // reset target altitude if this controller has just been engaged
const uint32_t now = millis(); const uint32_t now = millis();
if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt; target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error;
last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms; last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
} }
last_update_ms = now; last_update_ms = now;
@ -32,12 +32,12 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
// handle glitch recovery by resetting target // handle glitch recovery by resetting target
if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) { if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) {
// shift to the new rangefinder reading // shift to the new rangefinder reading
target_alt_cm = copter.rangefinder_state.alt_cm; target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error;
last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms; 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) // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
const float distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - (current_alt_target - current_alt); const float distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - current_alt_error;
float velocity_correction = distance_error * copter.g.rangefinder_gain; float velocity_correction = distance_error * copter.g.rangefinder_gain;
velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX); velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);