From 3c00e9b0da64ad0c23cb4a8cf0f1ad1364d187e0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 12 Apr 2024 11:26:04 +1000 Subject: [PATCH] autotest: add test for GuidedModeThrust --- Tools/autotest/arducopter.py | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index b71247338ac89..5e82437504395 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -10843,6 +10843,27 @@ def LoiterToGuidedHomeVSOrigin(self): self.disarm_vehicle(force=True) self.reboot_sitl() # to "unstick" home + def GuidedModeThrust(self): + '''test handling of option-bit-3, where mavlink commands are + intrepreted as thrust not climb rate''' + self.set_parameter('GUID_OPTIONS', 8) + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + self.mav.mav.set_attitude_target_send( + 0, # time_boot_ms + 1, # target sysid + 1, # target compid + 0, # bitmask of things to ignore + mavextra.euler_to_quat([0, 0, 0]), # att + 0, # roll rate (rad/s) + 0, # pitch rate (rad/s) + 0, # yaw rate (rad/s) + 0.5 + ) # thrust, 0 to 1 + self.wait_altitude(0.5, 100, relative=True, timeout=10) + self.do_RTL() + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -10922,6 +10943,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GPSForYawCompassLearn, self.CameraLogMessages, self.LoiterToGuidedHomeVSOrigin, + self.GuidedModeThrust, ]) return ret @@ -10952,6 +10974,7 @@ def disabled_tests(self): "GroundEffectCompensation_touchDownExpected": "Flapping", "FlyMissionTwice": "See https://github.com/ArduPilot/ardupilot/pull/18561", "GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion", + "GuidedModeThrust": "land detector raises internal error as we're not saying we're about to take off but just did", }