diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3ca7780e5ff453..4f1cb47773c39c 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -5508,6 +5508,7 @@ def RunMissionScript(self): def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): '''test handling of MAV_CMD_GUIDED_CHANGE_ALTITUDE''' + self.start_subtest("set home relative altitude") self.takeoff(30, relative=True) self.change_mode('GUIDED') for alt in 50, 70: @@ -5519,6 +5520,7 @@ def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): self.wait_altitude(alt-1, alt+1, timeout=30, relative=True) # test for #24535 + self.start_subtest("switch to loiter and resume maintains altitude") self.change_mode('LOITER') self.delay_sim_time(5) self.change_mode('GUIDED') @@ -5529,6 +5531,26 @@ def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): timeout=30, relative=True, ) + # test that this works if switching between RELATIVE (HOME) and GLOBAL(AMSL) + self.start_subtest("set global/AMSL altitude switch to loiter and resume") + alt = 650 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE, + p7=alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + ) + self.wait_altitude(alt-1, alt+1, timeout=30, relative=False) + self.change_mode('LOITER') + self.delay_sim_time(5) + self.change_mode('GUIDED') + self.wait_altitude( + alt-3, # NOTE: reuse of alt from above CHANGE_ALTITUDE + alt+3, + minimum_duration=10, + timeout=30, + relative=False, + ) + self.delay_sim_time(5) self.fly_home_land_and_disarm() def _MAV_CMD_PREFLIGHT_CALIBRATION(self, command):