autotest: add test for Rover MAV_CMD_NAV_RETURN_TO_LAUNCH

This commit is contained in:
Peter Barker 2023-08-23 17:37:44 +10:00 committed by Peter Barker
parent 6820e96ca8
commit b93e1d471f
1 changed files with 23 additions and 0 deletions

View File

@ -6358,6 +6358,28 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.disarm_vehicle()
def MAV_CMD_NAV_RETURN_TO_LAUNCH(self):
'''test MAV_CMD_NAV_RETURN_TO_LAUNCH mavlink command'''
self.change_mode('GUIDED')
self.wait_ready_to_arm()
self.arm_vehicle()
here = self.mav.location()
target_loc = self.offset_location_ne(here, 2000, 0)
self.send_guided_mission_item(target_loc)
self.wait_distance_to_home(20, 100)
self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH)
self.wait_mode('RTL')
self.change_mode('GUIDED')
self.run_cmd_int(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH)
self.wait_mode('RTL')
self.wait_distance_to_home(0, 5, timeout=30)
self.disarm_vehicle()
def tests(self):
'''return list of all tests'''
ret = super(AutoTestRover, self).tests()
@ -6424,6 +6446,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.DepthFinder,
self.ChangeModeByNumber,
self.EStopAtBoot,
self.MAV_CMD_NAV_RETURN_TO_LAUNCH,
self.StickMixingAuto,
self.AutoDock,
self.PrivateChannel,