From c05a0bc67fd6ed47cb1ee6ee7eb277c66d8be2b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 3 Dec 2014 18:31:23 +1100 Subject: [PATCH] Plane: prevent a change altitude command from causing a sudden descent when a change altitude command comes in while a large glide slope altitude change is present we could end up using the old glide slope with the new altitude. This resets the altitude offset, causing a direct altitude change --- ArduPlane/GCS_Mavlink.pde | 2 ++ ArduPlane/commands_logic.pde | 2 ++ 2 files changed, 4 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index c32ba7c5cd..72410215a4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -945,6 +945,8 @@ void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) next_WP_loc.alt += home.alt; } next_WP_loc.flags.relative_alt = false; + next_WP_loc.flags.terrain_alt = cmd.content.location.flags.terrain_alt; + reset_offset_altitude(); } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index dbf5c7762a..36381cdcd0 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -364,6 +364,7 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd) static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) { next_WP_loc.alt = cmd.content.location.alt + home.alt; + reset_offset_altitude(); } /********************************************************************************/ @@ -563,6 +564,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd) change_target_altitude(condition_rate/10); next_WP_loc.alt = condition_value; // For future nav calculations reset_offset_altitude(); + setup_glide_slope(); } static void do_within_distance(const AP_Mission::Mission_Command& cmd)