From ed7e359f4d867ea22711376ef48527473a9d893f Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Mon, 15 Jul 2019 12:23:46 +0100 Subject: [PATCH] Plane: Make target altitude track current altitude when gliding. --- ArduPlane/altitude.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 30ab359df1..33841d6de2 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -40,6 +40,10 @@ void Plane::adjust_altitude_target() landing.adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, auto_state.wp_distance, target_altitude.offset_cm); } else if (landing.get_target_altitude_location(target_location)) { set_target_altitude_location(target_location); + } else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { + // Reset target alt to current alt, to prevent large altitude errors when gliding. + set_target_altitude_location(current_loc); + reset_offset_altitude(); } else if (reached_loiter_target()) { // once we reach a loiter target then lock to the final // altitude target