autotest: add test for rover MAV_CMD_DO_CHANGE_SPEED

This commit is contained in:
Peter Barker 2023-08-23 22:16:14 +10:00 committed by Randy Mackay
parent 08fd7973e2
commit 2b9b3c07cb
1 changed files with 30 additions and 0 deletions

View File

@ -6363,6 +6363,35 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.wait_distance_to_home(0, 5, timeout=30) self.wait_distance_to_home(0, 5, timeout=30)
self.disarm_vehicle() self.disarm_vehicle()
def MAV_CMD_DO_CHANGE_SPEED(self):
'''test MAV_CMD_NAV_RETURN_TO_LAUNCH mavlink command'''
self.change_mode('GUIDED')
self.wait_ready_to_arm()
self.arm_vehicle()
original_loc = self.mav.location()
here = original_loc
target_loc = self.offset_location_ne(here, 2000, 0)
self.send_guided_mission_item(target_loc)
self.wait_distance_to_home(20, 100)
speeds = 3, 7, 12, 4
for speed in speeds:
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed)
self.wait_groundspeed(speed-0.5, speed+0.5, minimum_duration=5)
self.send_guided_mission_item(original_loc)
for speed in speeds:
self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed)
self.wait_groundspeed(speed-0.5, speed+0.5, minimum_duration=5)
self.change_mode('RTL')
self.wait_distance_to_home(0, 5, timeout=30)
self.disarm_vehicle()
def tests(self): def tests(self):
'''return list of all tests''' '''return list of all tests'''
ret = super(AutoTestRover, self).tests() ret = super(AutoTestRover, self).tests()
@ -6396,6 +6425,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.SET_ATTITUDE_TARGET, self.SET_ATTITUDE_TARGET,
self.SET_POSITION_TARGET_LOCAL_NED, self.SET_POSITION_TARGET_LOCAL_NED,
self.MAV_CMD_DO_SET_MISSION_CURRENT, self.MAV_CMD_DO_SET_MISSION_CURRENT,
self.MAV_CMD_DO_CHANGE_SPEED,
self.Button, self.Button,
self.Rally, self.Rally,
self.Offboard, self.Offboard,