diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d8dd4b23156834..739c89e4f8f60b 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -25,6 +25,7 @@ from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException from vehicle_test_suite import Test from vehicle_test_suite import MAV_POS_TARGET_TYPE_MASK +from vehicle_test_suite import WaitModeTimeout from pymavlink.rotmat import Vector3 @@ -10667,6 +10668,120 @@ def MAV_CMD_NAV_TAKEOFF_command_int(self): self.reboot_sitl() # unlock home position + def AutoRTL(self): + '''Test Auto RTL mode using do land start and do land rejoin mission items''' + alt = 50 + guided_loc = self.home_relative_loc_ne(1000, 0) + guided_loc.alt += alt + + # Arm, take off and fly to guided location + self.takeoff(mode='GUIDED') + self.fly_guided_move_to(guided_loc, timeout=240) + + # Try auto RTL mode, should fail with no mission + try: + self.change_mode('AUTO_RTL', timeout=10) + raise NotAchievedException("Should not change mode with no mission") + except WaitModeTimeout: + pass + except Exception as e: + raise e + + # pymavlink does not understand the new rejoin command yet, at some point it will + cmd_rejoin = 188 # mavutil.mavlink.MAV_CMD_DO_LAND_REJOIN + + # Do land start and do land rejoin should both fail + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.run_cmd(cmd_rejoin, want_result=mavutil.mavlink.MAV_RESULT_FAILED) + + # Load mission with do land start + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 750, 0, alt), # 2 + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 3 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, alt), # 4 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 250, 0, alt), # 5 + ]) + + # Rejoin should still fail + self.run_cmd(cmd_rejoin, want_result=mavutil.mavlink.MAV_RESULT_FAILED) + + # Do land start should jump to the waypoint following the item + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.drain_mav() + self.assert_current_waypoint(4) + + # Back to guided location + self.change_mode('GUIDED') + self.fly_guided_move_to(guided_loc) + + # mode change to Auto RTL should do the same + self.change_mode('AUTO_RTL') + self.drain_mav() + self.assert_current_waypoint(4) + + # Back to guided location + self.change_mode('GUIDED') + self.fly_guided_move_to(guided_loc) + + # Add a rejoin item + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1250, 0, alt), # 1 + self.create_MISSION_ITEM_INT(cmd_rejoin), # 2 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 900, 0, alt), # 3 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 750, 0, alt), # 4 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 550, 0, alt), # 5 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, alt), # 6 + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 7 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 250, 0, alt), # 8 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -250, 0, alt), # 9 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -500, 0, alt), # 10 + ]) + + # rejoin should now work + self.run_cmd(cmd_rejoin, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.drain_mav() + self.assert_current_waypoint(3) + + # Back to guided location + self.change_mode('GUIDED') + self.fly_guided_move_to(guided_loc) + + # mode change to Auto RTL should rejoin + self.change_mode('AUTO_RTL') + self.drain_mav() + self.assert_current_waypoint(3) + + # do land start should still work + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.drain_mav() + self.assert_current_waypoint(8) + + # Move a bit closer in guided + rejoin_test = self.home_relative_loc_ne(600, 0) + rejoin_test.alt += alt + self.change_mode('GUIDED') + self.fly_guided_move_to(rejoin_test, timeout=100) + + # check the mission is rejoined further along + self.run_cmd(cmd_rejoin, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.drain_mav() + self.assert_current_waypoint(5) + + # fly over home + home = self.home_relative_loc_ne(0, 0) + home.alt += alt + self.change_mode('GUIDED') + self.fly_guided_move_to(home, timeout=140) + + # Should never rejoin after do land start + self.run_cmd(cmd_rejoin, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.drain_mav() + self.assert_current_waypoint(6) + + # Done + self.land_and_disarm() + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -10740,6 +10855,7 @@ def tests2b(self): # this block currently around 9.5mins here self.MAV_CMD_SET_EKF_SOURCE_SET, self.MAV_CMD_NAV_TAKEOFF, self.MAV_CMD_NAV_TAKEOFF_command_int, + self.AutoRTL, ]) return ret