From 2b9b3c07cbc9a2057da253d400ee8c717f67f19a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Aug 2023 22:16:14 +1000 Subject: [PATCH] autotest: add test for rover MAV_CMD_DO_CHANGE_SPEED --- Tools/autotest/rover.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 6a39799c41..e9fd5376da 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -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.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): '''return list of all 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_POSITION_TARGET_LOCAL_NED, self.MAV_CMD_DO_SET_MISSION_CURRENT, + self.MAV_CMD_DO_CHANGE_SPEED, self.Button, self.Rally, self.Offboard,