diff --git a/libraries/AP_Math/examples/location/location.cpp b/libraries/AP_Math/examples/location/location.cpp index d614484404..99612572dd 100644 --- a/libraries/AP_Math/examples/location/location.cpp +++ b/libraries/AP_Math/examples/location/location.cpp @@ -6,6 +6,7 @@ #include #include +#include void setup(); void loop(); diff --git a/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp b/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp index 2e4d9a0cde..1f46975fef 100644 --- a/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp +++ b/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp @@ -174,33 +174,38 @@ void MissionTest::init_mission() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; - cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : first waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -208,9 +213,12 @@ void MissionTest::init_mission() // Command #3 : second waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; cmd.p1 = 0; - cmd.content.location.lat = 1234567890; - cmd.content.location.lng = -1234567890; - cmd.content.location.alt = 22; + cmd.content.location = Location{ + 1234567890, + -1234567890, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -226,9 +234,12 @@ void MissionTest::init_mission() // Command #5 : RTL cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH; cmd.p1 = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; - cmd.content.location.alt = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -245,33 +256,39 @@ void MissionTest::init_mission_no_nav_commands() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : "do" command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : "do" command cmd.id = MAV_CMD_DO_CHANGE_SPEED; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -302,11 +319,13 @@ void MissionTest::init_mission_endless_loop() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -321,22 +340,26 @@ void MissionTest::init_mission_endless_loop() // Command #2 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #3 : waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -354,33 +377,39 @@ void MissionTest::init_mission_jump_to_nonnav() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : do-roi command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -395,11 +424,13 @@ void MissionTest::init_mission_jump_to_nonnav() // Command #4 : waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 22; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -418,66 +449,78 @@ void MissionTest::init_mission_starts_with_do_commands() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : First "do" command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : Second "do" command cmd.id = MAV_CMD_DO_CHANGE_SPEED; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #3 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #4 : Third "do" command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 22; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #5 : waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 33; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 33, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -495,66 +538,75 @@ void MissionTest::init_mission_ends_with_do_commands() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.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("failed to add command\n"); - } + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; // Command #2 : "do" command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 22; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #3 : waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 33; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 33, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #4 : "do" command after last nav command (but not at end of mission) cmd.id = MAV_CMD_DO_CHANGE_SPEED; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #5 : "do" command at end of mission cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 22; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -571,66 +623,78 @@ void MissionTest::init_mission_ends_with_jump_command() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : "do" command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 22; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #3 : waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 33; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 33, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #4 : "do" command after last nav command (but not at end of mission) cmd.id = MAV_CMD_DO_CHANGE_SPEED; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #5 : "do" command at end of mission cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 22; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -692,33 +756,39 @@ void MissionTest::run_resume_test() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : first waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -726,20 +796,25 @@ void MissionTest::run_resume_test() // Command #3 : second waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; cmd.p1 = 0; - cmd.content.location.lat = 1234567890; - cmd.content.location.lng = -1234567890; - cmd.content.location.alt = 22; + cmd.content.location = Location{ + 1234567890, + -1234567890, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #4 : do command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -747,9 +822,12 @@ void MissionTest::run_resume_test() // Command #5 : RTL cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH; cmd.p1 = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; - cmd.content.location.alt = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -798,44 +876,52 @@ void MissionTest::run_set_current_cmd_test() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : do command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #3 : first waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -843,20 +929,25 @@ void MissionTest::run_set_current_cmd_test() // Command #4 : second waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; cmd.p1 = 0; - cmd.content.location.lat = 1234567890; - cmd.content.location.lng = -1234567890; - cmd.content.location.alt = 22; + cmd.content.location = Location{ + 1234567890, + -1234567890, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #5 : do command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -864,9 +955,12 @@ void MissionTest::run_set_current_cmd_test() // Command #6 : RTL cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH; cmd.p1 = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; - cmd.content.location.alt = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -903,44 +997,52 @@ void MissionTest::run_set_current_cmd_while_stopped_test() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : do command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 22; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #3 : first waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -948,20 +1050,25 @@ void MissionTest::run_set_current_cmd_while_stopped_test() // Command #4 : second waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; cmd.p1 = 0; - cmd.content.location.lat = 1234567890; - cmd.content.location.lng = -1234567890; - cmd.content.location.alt = 22; + cmd.content.location = Location{ + 1234567890, + -1234567890, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #5 : do command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -969,9 +1076,12 @@ void MissionTest::run_set_current_cmd_while_stopped_test() // Command #6 : RTL cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH; cmd.p1 = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; - cmd.content.location.alt = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -1040,44 +1150,52 @@ void MissionTest::run_replace_cmd_test() // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 0; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #1 : take-off to 10m cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 10; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; + cmd.content.location = Location{ + 0, + 0, + 10, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #2 : do command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } // Command #3 : first waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -1085,9 +1203,12 @@ void MissionTest::run_replace_cmd_test() // Command #4 : second waypoint cmd.id = MAV_CMD_NAV_WAYPOINT; cmd.p1 = 0; - cmd.content.location.lat = 1234567890; - cmd.content.location.lng = -1234567890; - cmd.content.location.alt = 22; + cmd.content.location = Location{ + 1234567890, + -1234567890, + 22, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -1095,9 +1216,12 @@ void MissionTest::run_replace_cmd_test() // Command #6 : RTL cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH; cmd.p1 = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; - cmd.content.location.alt = 0; + cmd.content.location = Location{ + 0, + 0, + 0, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command\n"); } @@ -1117,11 +1241,13 @@ void MissionTest::run_replace_cmd_test() // replace command #4 with a do-command // Command #4 : do command cmd.id = MAV_CMD_DO_SET_ROI; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = 11; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + 11, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.replace_cmd(4, cmd)) { hal.console->printf("failed to replace command 4\n"); }else{ @@ -1150,11 +1276,13 @@ void MissionTest::run_max_cmd_test() while (!failed_to_add) { // Command #0 : home cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location.options = 0; cmd.p1 = 0; - cmd.content.location.alt = num_commands; - cmd.content.location.lat = 12345678; - cmd.content.location.lng = 23456789; + cmd.content.location = Location{ + 12345678, + 23456789, + num_commands, + Location::ALT_FRAME_ABSOLUTE + }; if (!mission.add_cmd(cmd)) { hal.console->printf("failed to add command #%u, library says max is %u\n",(unsigned int)num_commands, (unsigned int)mission.num_commands_max()); failed_to_add = true;