Plane: Implementation of MAV_CMD_CONTINUE_AND_CHANGE_ALT.

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
 /********************************************************************************/