diff --git a/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt b/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt new file mode 100644 index 0000000000..41e2938f9c --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt @@ -0,0 +1,8 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165238 584.089966 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361438 149.165031 50.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361505 149.163723 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365845 149.164175 52.219997 1 +4 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.368656 149.165976 54.410000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366148 149.165741 50.849998 1 +6 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362920 149.165127 50.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index dd118eacbd..17a4c22fdb 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4909,6 +4909,23 @@ class AutoTestPlane(AutoTest): self.disarm_vehicle() self.reboot_sitl() + def _MAV_CMD_DO_GO_AROUND(self, command): + self.load_mission("mission.txt") + self.set_parameter("RTL_AUTOLAND", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_current_waypoint(6) + command(mavutil.mavlink.MAV_CMD_DO_GO_AROUND, p1=150) + self.wait_current_waypoint(5) + self.wait_altitude(135, 165, relative=True) + self.wait_disarmed(timeout=300) + + def MAV_CMD_DO_GO_AROUND(self): + '''test MAV_CMD_DO_GO_AROUND as a mavlink command''' + self._MAV_CMD_DO_GO_AROUND(self.run_cmd) + self._MAV_CMD_DO_GO_AROUND(self.run_cmd_int) + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -5004,6 +5021,7 @@ class AutoTestPlane(AutoTest): self.MAV_CMD_PREFLIGHT_CALIBRATION, self.MAV_CMD_DO_INVERTED_FLIGHT, self.MAV_CMD_DO_AUTOTUNE_ENABLE, + self.MAV_CMD_DO_GO_AROUND, ]) return ret