diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 30a2d79f82fc3eb6a9e952342a682b70ca11dce6..72a4d621a2b5050d4269099aeec78dcfc424720d 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -12,6 +12,7 @@ static void do_within_distance(const AP_Mission::Mission_Command& cmd); static void do_change_alt(const AP_Mission::Mission_Command& cmd); static void do_change_speed(const AP_Mission::Mission_Command& cmd); static void do_set_home(const AP_Mission::Mission_Command& cmd); +static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); @@ -78,6 +79,9 @@ start_command(const AP_Mission::Mission_Command& cmd) set_mode(RTL); break; + case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: + do_continue_and_change_alt(cmd); + // Conditional commands case MAV_CMD_CONDITION_DELAY: @@ -212,6 +216,9 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret case MAV_CMD_NAV_RETURN_TO_LAUNCH: return verify_RTL(); + case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: + return verify_continue_and_change_alt(); + // Conditional commands case MAV_CMD_CONDITION_DELAY: @@ -342,6 +349,11 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd) loiter_set_direction_wp(cmd); } +static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) +{ + next_WP_loc.alt = cmd.content.location.alt + home.alt; +} + /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ @@ -496,6 +508,18 @@ static bool verify_RTL() } } +static bool verify_continue_and_change_alt() +{ + if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) { + return true; + } + + //keep flying the same course + nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); + + return false; +} + /********************************************************************************/ // Condition (May) commands /********************************************************************************/