From 056ccbd9fdb928be06e2999f61a8d889164d5f54 Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Fri, 30 Aug 2024 10:49:57 -0600 Subject: [PATCH] Tools: test use Location::AltFrame for guided_state.target_alt_frame --- Tools/autotest/arduplane.py | 54 +++++++++++++++++++++++++++- Tools/autotest/vehicle_test_suite.py | 9 ++++- 2 files changed, 61 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 098aa2bdc78b27..94b51a29c7619c 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2942,7 +2942,7 @@ def TerrainLoiter(self): gpi = self.assert_receive_message('GLOBAL_POSITION_INT') terrain = self.assert_receive_message('TERRAIN_REPORT') rel_alt = terrain.current_height - self.progress("%um above terrain (%um bove home)" % + self.progress("%um above terrain (%um above home)" % (rel_alt, gpi.relative_alt/1000.0)) if rel_alt > alt*1.2 or rel_alt < alt * 0.8: raise NotAchievedException("Not terrain following") @@ -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 guided maintains home relative altitude") self.change_mode('LOITER') self.delay_sim_time(5) self.change_mode('GUIDED') @@ -5529,6 +5531,56 @@ 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") + # need to do this to activate terrain data + 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, + 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, + 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 8ac22b6edea18f..46f8ab4725de9a 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7242,11 +7242,18 @@ 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') + if terrain is None: + raise NotAchievedException("Did not get TERRAIN_REPORT message") + 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 if altitude_source == "SIM_STATE.alt": divisor = 1.0 + return getattr(msg, field) / divisor def assert_altitude(self, alt, accuracy=1, **kwargs): @@ -7617,7 +7624,7 @@ def wait_and_maintain_range(self, self.progress("Waiting for %s between (%s) and (%s)" % (value_name, str(minimum), str(maximum))) last_print_time = 0 while self.get_sim_time_cached() < tstart + timeout: # if we failed to received message with the getter the sim time isn't updated # noqa - last_value = current_value_getter() + last_value = current_value_getter() if called_function is not None: if print_diagnostics_as_target_not_range: called_function(last_value, target)