diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ea6f0a1e3827ec..11f14e3db6066d 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -5533,6 +5533,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: @@ -5544,6 +5545,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 guided maintains home relative altitude") self.change_mode('LOITER') self.delay_sim_time(5) self.change_mode('GUIDED') @@ -5554,6 +5556,55 @@ 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 guided") + alt = 625 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE, + p7=alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + ) + self.wait_altitude(alt-3, alt+3, timeout=30, relative=False) + self.change_mode('LOITER') + self.delay_sim_time(5) + self.change_mode('GUIDED') + self.wait_altitude( + alt-5, # NOTE: reuse of alt from above CHANGE_ALTITUDE + alt+5, + minimum_duration=10, + timeout=30, + relative=False, + ) + # test that this works if switching between RELATIVE (HOME) and terrain + self.start_subtest("set terrain altitude, switch to loiter and resume guided") + self.change_mode('GUIDED') + alt = 100 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE, + p7=alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, + ) + self.wait_altitude( + alt-5, # NOTE: reuse of alt from abovE + alt+5, # use a 5m buffer as the plane needs to go up and down a bit to maintain terrain distance + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + self.change_mode('LOITER') + self.delay_sim_time(5) + self.change_mode('GUIDED') + self.wait_altitude( + alt-5, # NOTE: reuse of alt from abovE + alt+5, # use a 5m buffer as the plane needs to go up and down a bit to maintain terrain distance + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + + self.delay_sim_time(5) self.fly_home_land_and_disarm() def _MAV_CMD_PREFLIGHT_CALIBRATION(self, command): diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index cffa714ce99497..f5b6b7e9bded26 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7282,6 +7282,10 @@ def get_altitude(self, relative=False, timeout=30, altitude_source=None): altitude_source = "GLOBAL_POSITION_INT.relative_alt" else: altitude_source = "GLOBAL_POSITION_INT.alt" + if altitude_source == "TERRAIN_REPORT.current_height": + terrain = self.assert_receive_message('TERRAIN_REPORT') + return terrain.current_height + (msg, field) = altitude_source.split('.') msg = self.poll_message(msg, quiet=True) divisor = 1000.0 # mm is pretty common in mavlink