Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Mission: example sketch tests set current command

Browse Source
This commit is contained in:
Randy Mackay 2014-03-01 10:20:22 +09:00
parent 3cbbd4ebb9
commit ef21e32fbc
1 changed files with 124 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.pde
View File

@ -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()
{