Mission: example sketch tests set current command

libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.pde

@@ -163,6 +163,9 @@ void run_mission_test()
     //      mission should complete after the do-jump is executed the appropriate number of times
     //init_mission_ends_with_jump_command();
 
+    // run_set_current_cmd_test - tests setting the current command while the mission is running
+    //run_set_current_cmd_test();
+
     // print current mission
     print_mission();
 
@@ -183,7 +186,19 @@ void run_resume_test()
     AP_Mission::Mission_Command cmd;
 
     // create a mission
-    // Command #0 : take-off to 10m
+
+    // Command #0 : home
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 0;
+    cmd.content.location.lat = 12345678;
+    cmd.content.location.lng = 23456789;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #1 : take-off to 10m
     cmd.id = MAV_CMD_NAV_TAKEOFF;
     cmd.content.location.options = 0;
     cmd.content.location.p1 = 0;
@@ -194,7 +209,7 @@ void run_resume_test()
         hal.console->printf_P(PSTR("failed to add command\n"));
     }
 
-    // Command #1 : first waypoint
+    // Command #2 : first waypoint
     cmd.id = MAV_CMD_NAV_WAYPOINT;
     cmd.content.location.options = 0;
     cmd.content.location.p1 = 0;
@@ -205,7 +220,7 @@ void run_resume_test()
         hal.console->printf_P(PSTR("failed to add command\n"));
     }
 
-    // Command #2 : second waypoint
+    // Command #3 : second waypoint
     cmd.id = MAV_CMD_NAV_WAYPOINT;
     cmd.content.location.p1 = 0;
     cmd.content.location.lat = 1234567890;
@@ -215,7 +230,7 @@ void run_resume_test()
         hal.console->printf_P(PSTR("failed to add command\n"));
     }
 
-    // Command #3 : do command
+    // Command #4 : do command
     cmd.id = MAV_CMD_DO_SET_ROI;
     cmd.content.location.options = 0;
     cmd.content.location.p1 = 0;
@@ -226,7 +241,7 @@ void run_resume_test()
         hal.console->printf_P(PSTR("failed to add command\n"));
     }
 
-    // Command #4 : RTL
+    // Command #5 : RTL
     cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH;
     cmd.content.location.p1 = 0;
     cmd.content.location.lat = 0;
@@ -271,6 +286,110 @@ void run_resume_test()
     }
 }
 
+// run_set_current_cmd_test - tests setting the current command during a mission
+void run_set_current_cmd_test()
+{
+    AP_Mission::Mission_Command cmd;
+
+    // create a mission
+
+    // Command #0 : home
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 0;
+    cmd.content.location.lat = 12345678;
+    cmd.content.location.lng = 23456789;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #1 : take-off to 10m
+    cmd.id = MAV_CMD_NAV_TAKEOFF;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 10;
+    cmd.content.location.lat = 0;
+    cmd.content.location.lng = 0;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #2 : do command
+    cmd.id = MAV_CMD_DO_SET_ROI;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 11;
+    cmd.content.location.lat = 12345678;
+    cmd.content.location.lng = 23456789;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #3 : first waypoint
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 11;
+    cmd.content.location.lat = 12345678;
+    cmd.content.location.lng = 23456789;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #4 : second waypoint
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.lat = 1234567890;
+    cmd.content.location.lng = -1234567890;
+    cmd.content.location.alt = 22;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #5 : do command
+    cmd.id = MAV_CMD_DO_SET_ROI;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 11;
+    cmd.content.location.lat = 12345678;
+    cmd.content.location.lng = 23456789;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #5 : RTL
+    cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.lat = 0;
+    cmd.content.location.lng = 0;
+    cmd.content.location.alt = 0;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // print current mission
+    print_mission();
+
+    // start mission
+    hal.console->printf_P(PSTR("\nRunning missions\n"));
+    mission.start();
+
+    // update the mission for X iterations to let it go to about command 3 or 4
+    for(uint8_t i=0; i<11; i++) {
+        mission.update();
+    }
+
+    // simulate user setting current command to 2
+    hal.console->printf_P(PSTR("Setting current command to 2\n"));
+    mission.set_current_cmd(2);
+
+    // update the mission forever
+    while(true) {
+        mission.update();
+    }
+}
+
 // init_mission - initialise the mission to hold something
 void init_mission()
 {