From 972c916f1b26301edebacb5a7678a3e6cd8b0fbf Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 22 Aug 2019 21:57:52 +0900 Subject: [PATCH] Copter: surface tracking always includes ekf alt error when setting target rangefinder alt --- ArduCopter/surface_tracking.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index da90cfb160..eb9348a344 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -12,13 +12,13 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) return target_rate; } - const float current_alt = copter.inertial_nav.get_altitude(); - const float current_alt_target = copter.pos_control->get_alt_target(); + // calculate current ekf based altitude error + 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 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; + target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error; last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms; } last_update_ms = now; @@ -32,12 +32,12 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) // 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; + target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error; 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) - 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; velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);