From 1641aa42ec37884caf204195c5d97eb97663e334 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 29 Sep 2023 11:23:00 +1000 Subject: [PATCH] autotest: add test for MAV_CMD_DO_LAND_START --- Tools/autotest/arducopter.py | 39 ++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index ca6b4b1e8a..fc0659c8df 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -10308,6 +10308,44 @@ class AutoTestCopter(AutoTest): command(mavutil.mavlink.MAV_CMD_NAV_LAND) self.wait_mode('LAND') + def start_flying_simple_rehome_mission(self, items): + '''uploads items, changes mode to auto, waits ready to arm and arms + vehicle. If the first item it a takeoff you can expect the + vehicle to fly after this method returns + ''' + + self.upload_simple_relhome_mission(items) + + self.set_parameter("AUTO_OPTIONS", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + + def _MAV_CMD_DO_LAND_START(self, run_cmd): + alt = 5 + self.start_flying_simple_rehome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0), + (mavutil.mavlink.MAV_CMD_DO_LAND_START, 0, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 2000, alt), + (mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0), + ]) + + self.wait_current_waypoint(2) + run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START) + self.wait_current_waypoint(5) + # we pretend to be in RTL mode while doing this: + self.wait_mode("AUTO_RTL") + self.do_RTL() + + def MAV_CMD_DO_LAND_START(self): + '''test handling of mavlink-received MAV_CMD_DO_LAND_START command''' + self._MAV_CMD_DO_LAND_START(self.run_cmd) + self.zero_throttle() + self._MAV_CMD_DO_LAND_START(self.run_cmd_int) + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -10373,6 +10411,7 @@ class AutoTestCopter(AutoTest): self.SafetySwitch, self.BrakeZ, self.MAV_CMD_DO_FLIGHTTERMINATION, + self.MAV_CMD_DO_LAND_START, ]) return ret