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

Mission: fix example sketch

Browse Source 
Also replace printf with print where possible
This commit is contained in:
Randy Mackay 2015-03-16 20:13:17 +09:00
parent 385b3744ea
commit b01f9505b1
1 changed files with 68 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -45,7 +45,7 @@ AP_Baro baro;
// GPS declaration
static AP_GPS gps;
AP_Compass compass;
Compass compass;
AP_AHRS_DCM ahrs(ins, baro, gps);
// global constants that control how many verify calls must be made for a command before it completes
@ -98,7 +98,7 @@ bool verify_cmd(const AP_Mission::Mission_Command& cmd)
// mission_complete - function that is called once the mission completes
void mission_complete(void)
{
hal.console->printf_P(PSTR("\nMission Complete!\n"));
hal.console->print_P(PSTR("\nMission Complete!\n"));
}
// declaration
@ -179,7 +179,7 @@ void run_mission_test()
print_mission();
// start mission
hal.console->printf_P(PSTR("\nRunning missions\n"));
hal.console->print_P(PSTR("\nRunning missions\n"));
mission.start();
// update mission forever
@ -204,7 +204,7 @@ void init_mission()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -215,7 +215,7 @@ void init_mission()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : first waypoint
@ -226,7 +226,7 @@ void init_mission()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : second waypoint
@ -236,7 +236,7 @@ void init_mission()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : do-jump to first waypoint 3 times
@ -244,7 +244,7 @@ void init_mission()
cmd.content.jump.target = 2;
cmd.content.jump.num_times = 1;
if (!mission.add_cmd(cmd)) {
hal.console->printf_P(PSTR("failed to add command\n"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #5 : RTL
@ -254,7 +254,7 @@ void init_mission()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
}
@ -275,7 +275,7 @@ void init_mission_no_nav_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : "do" command
@ -286,7 +286,7 @@ void init_mission_no_nav_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : "do" command
@ -297,13 +297,13 @@ void init_mission_no_nav_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : "do" command
cmd.id = MAV_CMD_DO_SET_SERVO;
if (!mission.add_cmd(cmd)) {
hal.console->printf_P(PSTR("failed to add command\n"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : do-jump to first command 3 times
@ -311,7 +311,7 @@ void init_mission_no_nav_commands()
cmd.content.jump.target = 1;
cmd.content.jump.num_times = 1;
if (!mission.add_cmd(cmd)) {
hal.console->printf_P(PSTR("failed to add command\n"));
hal.console->print_P(PSTR("failed to add command\n"));
}
}
@ -332,7 +332,7 @@ void init_mission_endless_loop()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : do-jump command to itself
@ -340,7 +340,7 @@ void init_mission_endless_loop()
cmd.content.jump.target = 1;
cmd.content.jump.num_times = 2;
if (!mission.add_cmd(cmd)) {
hal.console->printf_P(PSTR("failed to add command\n"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : take-off to 10m
@ -351,7 +351,7 @@ void init_mission_endless_loop()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : waypoint
@ -362,7 +362,7 @@ void init_mission_endless_loop()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
}
@ -384,7 +384,7 @@ void init_mission_jump_to_nonnav()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -395,7 +395,7 @@ void init_mission_jump_to_nonnav()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : do-roi command
@ -406,7 +406,7 @@ void init_mission_jump_to_nonnav()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : do-jump command to #2
@ -414,7 +414,7 @@ void init_mission_jump_to_nonnav()
cmd.content.jump.target = 2;
cmd.content.jump.num_times = 2;
if (!mission.add_cmd(cmd)) {
hal.console->printf_P(PSTR("failed to add command\n"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : waypoint
@ -425,7 +425,7 @@ void init_mission_jump_to_nonnav()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
}
@ -448,7 +448,7 @@ void init_mission_starts_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : First "do" command
@ -459,7 +459,7 @@ void init_mission_starts_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : Second "do" command
@ -470,7 +470,7 @@ void init_mission_starts_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : take-off to 10m
@ -481,7 +481,7 @@ void init_mission_starts_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : Third "do" command
@ -492,7 +492,7 @@ void init_mission_starts_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #5 : waypoint
@ -503,7 +503,7 @@ void init_mission_starts_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
}
@ -525,7 +525,7 @@ void init_mission_ends_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -536,7 +536,7 @@ void init_mission_ends_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : "do" command
@ -547,7 +547,7 @@ void init_mission_ends_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : waypoint
@ -558,7 +558,7 @@ void init_mission_ends_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : "do" command after last nav command (but not at end of mission)
@ -569,7 +569,7 @@ void init_mission_ends_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #5 : "do" command at end of mission
@ -580,7 +580,7 @@ void init_mission_ends_with_do_commands()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
}
@ -601,7 +601,7 @@ void init_mission_ends_with_jump_command()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -612,7 +612,7 @@ void init_mission_ends_with_jump_command()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : "do" command
@ -623,7 +623,7 @@ void init_mission_ends_with_jump_command()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : waypoint
@ -634,7 +634,7 @@ void init_mission_ends_with_jump_command()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : "do" command after last nav command (but not at end of mission)
@ -645,7 +645,7 @@ void init_mission_ends_with_jump_command()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #5 : "do" command at end of mission
@ -656,7 +656,7 @@ void init_mission_ends_with_jump_command()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #6 : do-jump command to #2 two times
@ -664,7 +664,7 @@ void init_mission_ends_with_jump_command()
cmd.content.jump.target = 3;
cmd.content.jump.num_times = 2;
if (!mission.add_cmd(cmd)) {
hal.console->printf_P(PSTR("failed to add command\n"));
hal.console->print_P(PSTR("failed to add command\n"));
}
}
@ -722,7 +722,7 @@ void run_resume_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -733,7 +733,7 @@ void run_resume_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : first waypoint
@ -744,7 +744,7 @@ void run_resume_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : second waypoint
@ -754,7 +754,7 @@ void run_resume_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : do command
@ -765,7 +765,7 @@ void run_resume_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #5 : RTL
@ -775,7 +775,7 @@ void run_resume_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// print current mission
@ -828,7 +828,7 @@ void run_set_current_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -839,7 +839,7 @@ void run_set_current_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : do command
@ -850,7 +850,7 @@ void run_set_current_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : first waypoint
@ -861,7 +861,7 @@ void run_set_current_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : second waypoint
@ -871,7 +871,7 @@ void run_set_current_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #5 : do command
@ -882,7 +882,7 @@ void run_set_current_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #6 : RTL
@ -892,7 +892,7 @@ void run_set_current_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// print current mission
@ -933,7 +933,7 @@ void run_set_current_cmd_while_stopped_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -944,7 +944,7 @@ void run_set_current_cmd_while_stopped_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : do command
@ -955,7 +955,7 @@ void run_set_current_cmd_while_stopped_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : first waypoint
@ -966,7 +966,7 @@ void run_set_current_cmd_while_stopped_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : second waypoint
@ -976,7 +976,7 @@ void run_set_current_cmd_while_stopped_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #5 : do command
@ -987,7 +987,7 @@ void run_set_current_cmd_while_stopped_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #6 : RTL
@ -997,7 +997,7 @@ void run_set_current_cmd_while_stopped_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// print current mission
@ -1070,7 +1070,7 @@ void run_replace_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #1 : take-off to 10m
@ -1081,7 +1081,7 @@ void run_replace_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #2 : do command
@ -1092,7 +1092,7 @@ void run_replace_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #3 : first waypoint
@ -1103,7 +1103,7 @@ void run_replace_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #4 : second waypoint
@ -1113,7 +1113,7 @@ void run_replace_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// Command #6 : RTL
@ -1123,7 +1123,7 @@ void run_replace_cmd_test()
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"));
hal.console->print_P(PSTR("failed to add command\n"));
}
// print current mission