From d976d71e99d33ff3974133358f779c79fe0f7efd Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 16 Jul 2026 17:13:24 +1000 Subject: [PATCH] autotest: add frame-aware Location class mavutil.location has no altitude frame field; its alt is AMSL only by convention and the suite routinely stuffs relative-to-home and terrain altitudes into it, leading to silent frame-mixing bugs. Add a Location class mirroring AP_Common's Location: meters altitude tagged with an AltFrame (ABSOLUTE/ABOVE_HOME/ABOVE_ORIGIN/ ABOVE_TERRAIN). It deliberately has no alt attribute. Altitude is only reachable via get_alt_m(frame), and asking in the wrong frame is an error, not a conversion: code that assumes a frame fails loudly instead of silently misreading the altitude. Converting is its own explicit step, TestSuite.change_alt_frame(), which polls home, origin or terrain data from the vehicle as required. --- Tools/autotest/vehicle_test_suite.py | 332 +++++++++++++++++++++++++-- 1 file changed, 316 insertions(+), 16 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 92e6f16cf1d..c498605702f 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -198,6 +198,136 @@ class ArmedAtEndOfTestException(ErrorException): pass +class LocationAltFrameException(ErrorException): + """Thrown when a Location's altitude is accessed in the wrong frame""" + pass + + +class AltFrame(enum.Enum): + '''altitude frame for Location, mirroring AP_Common Location::AltFrame''' + ABSOLUTE = 0 # above mean sea level (AMSL) + ABOVE_HOME = 1 + ABOVE_ORIGIN = 2 # above EKF origin + ABOVE_TERRAIN = 3 + + +class Location(object): + '''a latitude/longitude/altitude-with-frame, mirroring AP_Common's + Location. Use this in preference to mavutil.location, which has no + field for the frame the altitude is in, and in practice is used to + hold altitudes in a variety of frames. + + lat/lng are in degrees. The altitude is in metres, tagged with the + AltFrame it is measured in, and is only accessible via get_alt_m(), + which raises LocationAltFrameException unless the caller names the + frame the altitude is stored in; use TestSuite.change_alt_frame() + to convert between frames. There is deliberately no "alt" + attribute, so code assuming a frame fails loudly rather than + misinterpreting the altitude. + ''' + + def __init__(self, lat_deg: float, lng_deg: float, alt_m: float, alt_frame: AltFrame): + if not isinstance(alt_frame, AltFrame): + raise ValueError("alt_frame must be an AltFrame, got %s" % str(alt_frame)) + self.lat = lat_deg + self.lng = lng_deg + self._alt_m = alt_m + self._alt_frame = alt_frame + + @classmethod + def latlon_only(cls, lat_deg: float, lng_deg: float) -> Location: + '''a Location with no altitude at all; altitude access raises. + Use for 2D targets instead of a lie like alt=0''' + ret = cls(lat_deg, lng_deg, 0, AltFrame.ABSOLUTE) + ret._alt_m = None + ret._alt_frame = None + return ret + + @classmethod + def from_mavutil(cls, loc) -> Location: + '''create from a mavutil.location, whose alt is AMSL by + convention; the caller must ensure that is true of this one''' + return cls(loc.lat, loc.lng, loc.alt, AltFrame.ABSOLUTE) + + @property + def alt_frame(self) -> AltFrame: + '''frame the altitude is stored in, None if lat/lng-only''' + return self._alt_frame + + def has_alt(self) -> bool: + return self._alt_frame is not None + + def get_alt_m(self, frame: AltFrame) -> float: + '''return altitude in metres in the given frame. frame must + match the frame the altitude is stored in - this is a demand + that the caller know what frame it is working in, not a + conversion; see TestSuite.change_alt_frame() for that''' + if not isinstance(frame, AltFrame): + raise ValueError("frame must be an AltFrame, got %s" % str(frame)) + if self._alt_frame is None: + raise LocationAltFrameException("Location is lat/lng-only, has no altitude") + if frame != self._alt_frame: + raise LocationAltFrameException( + "altitude is in frame %s, requested %s; use TestSuite.change_alt_frame() to convert" % + (self._alt_frame.name, frame.name)) + return self._alt_m + + def set_alt_m(self, alt_m: float, frame: AltFrame) -> None: + if not isinstance(frame, AltFrame): + raise ValueError("frame must be an AltFrame, got %s" % str(frame)) + self._alt_m = alt_m + self._alt_frame = frame + + def offset_up_m(self, alt_offset_m: float) -> None: + '''adjust altitude upwards by alt_offset_m metres, keeping its frame''' + if self._alt_frame is None: + raise LocationAltFrameException("Location is lat/lng-only, has no altitude") + self._alt_m += alt_offset_m + + def copy(self) -> Location: + ret = Location.latlon_only(self.lat, self.lng) + ret._alt_m = self._alt_m + ret._alt_frame = self._alt_frame + return ret + + def mav_frame(self) -> int: + '''return the MAV_FRAME matching this Location's altitude + frame, for sending in COMMAND_INT, mission items and elsewhere. + The _INT frame variants were superseded as synonyms of these in + MAVLink in 2024-03, so are never returned''' + frame_map = { + AltFrame.ABSOLUTE: mavutil.mavlink.MAV_FRAME_GLOBAL, + AltFrame.ABOVE_HOME: mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + AltFrame.ABOVE_TERRAIN: mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, + } + if self._alt_frame not in frame_map: + raise LocationAltFrameException( + "no MAV_FRAME for altitude frame %s" % + ("None" if self._alt_frame is None else self._alt_frame.name)) + return frame_map[self._alt_frame] + + @staticmethod + def alt_frame_from_mav_frame(mav_frame: int) -> AltFrame: + '''return the AltFrame matching a MAV_FRAME_GLOBAL_* frame''' + frame_map = { + mavutil.mavlink.MAV_FRAME_GLOBAL: AltFrame.ABSOLUTE, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT: AltFrame.ABSOLUTE, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT: AltFrame.ABOVE_HOME, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: AltFrame.ABOVE_HOME, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT: AltFrame.ABOVE_TERRAIN, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: AltFrame.ABOVE_TERRAIN, + } + if mav_frame not in frame_map: + raise LocationAltFrameException("no AltFrame for MAV_FRAME %u" % mav_frame) + return frame_map[mav_frame] + + def __str__(self): + if self._alt_frame is None: + return "Location(lat=%.7f lng=%.7f no-alt)" % (self.lat, self.lng) + return "Location(lat=%.7f lng=%.7f alt=%.2fm-%s)" % ( + self.lat, self.lng, self._alt_m, self._alt_frame.name) + + NUM_RC_CHANNELS = 16 @@ -577,16 +707,29 @@ class WaitAndMaintain(object): class WaitAndMaintainLocation(WaitAndMaintain): def __init__(self, test_suite, target, accuracy=5, height_accuracy=1, location_source=None, **kwargs): super(WaitAndMaintainLocation, self).__init__(test_suite, **kwargs) + if isinstance(target, Location) and height_accuracy is not None: + if not target.has_alt(): + raise ValueError("lat/lng-only target Location requires height_accuracy=None") + # comparisons are made against AMSL current position, so + # convert the target up-front: + target = test_suite.change_alt_frame(target, AltFrame.ABSOLUTE) self.target = target self.height_accuracy = height_accuracy self.accuracy = accuracy self.location_source = location_source + def target_alt_amsl_m(self): + '''target altitude in metres AMSL''' + if isinstance(self.target, Location): + return self.target.get_alt_m(AltFrame.ABSOLUTE) + # mavutil.location alt is AMSL-by-convention: + return self.target.alt + def announce_start_text(self): t = self.target if self.height_accuracy is not None: return ("Waiting for distance to Location (%.4f, %.4f, %.2f) (h_err<%f, v_err<%.2f " % - (t.lat, t.lng, t.alt, self.accuracy, self.height_accuracy)) + (t.lat, t.lng, self.target_alt_amsl_m(), self.accuracy, self.height_accuracy)) return ("Waiting for distance to Location (%.4f, %.4f) (h_err<%f" % (t.lat, t.lng, self.accuracy)) @@ -602,7 +745,7 @@ class WaitAndMaintainLocation(WaitAndMaintain): return self.test_suite.get_distance(value, self.target) def vertical_error(self, value): - return math.fabs(value.alt - self.target.alt) + return math.fabs(value.alt - self.target_alt_amsl_m()) def validate_value(self, value): if self.horizontal_error(value) > self.accuracy: @@ -624,7 +767,7 @@ class WaitAndMaintainLocation(WaitAndMaintain): def progress_text(self, current_value): if self.height_accuracy is not None: - return (f"Want=({self.target.lat:.7f},{self.target.lng:.7f},{self.target.alt:.2f}) Got=({current_value.lat:.7f},{current_value.lng:.7f},{current_value.alt:.2f}) dist={self.horizontal_error(current_value):.2f} vdist={self.vertical_error(current_value):.2f}") # noqa + return (f"Want=({self.target.lat:.7f},{self.target.lng:.7f},{self.target_alt_amsl_m():.2f}) Got=({current_value.lat:.7f},{current_value.lng:.7f},{current_value.alt:.2f}) dist={self.horizontal_error(current_value):.2f} vdist={self.vertical_error(current_value):.2f}") # noqa return (f"Want=({self.target.lat},{self.target.lng}) distance={self.horizontal_error(current_value)}") @@ -4279,7 +4422,8 @@ class TestSuite(abc.ABC): return ret def sim_location(self): - """Return current simulator location.""" + """Return current simulator location. Deprecated; use + get_location('SIMSTATE') instead.""" m = self.assert_receive_message('SIMSTATE') return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, @@ -7634,6 +7778,25 @@ class TestSuite(abc.ABC): m = self.assert_receive_message('TERRAIN_REPORT', very_verbose=True) return m.terrain_height + def get_terrain_height_at(self, loc, timeout: float = 10) -> float: + '''return terrain height (metres AMSL) at loc's lat/lng, via + TERRAIN_CHECK. TERRAIN_REPORTs for other locations (e.g. those + emitted for the vehicle's current position) are ignored''' + lat_int = int(loc.lat * 1e7) + lng_int = int(loc.lng * 1e7) + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not get TERRAIN_REPORT for location") + self.mav.mav.terrain_check_send(lat_int, lng_int) + m = self.assert_receive_message('TERRAIN_REPORT') + # match reports to our request; 100 * 1e-7 degrees is ~1m + if abs(m.lat - lat_int) > 100 or abs(m.lon - lng_int) > 100: + continue + if m.spacing == 0: + raise NotAchievedException("No terrain data at location") + return m.terrain_height + def get_altitude(self, relative=False, timeout=30, altitude_source=None): '''returns vehicles altitude in metres, possibly relative-to-home''' if altitude_source is None: @@ -8235,7 +8398,7 @@ class TestSuite(abc.ABC): def get_mav_location(self, location_source: str = None): '''return a mavutil.location object for the given source; source must produce a good lat/lng or exception will be - raised''' + raised. Deprecated; use get_location() instead''' if location_source is None: location_source = 'GLOBAL_POSITION_INT' m = self.assert_receive_message(location_source) @@ -8253,6 +8416,49 @@ class TestSuite(abc.ABC): return mavutil.location(lat, lon, alt_m, 0) + def get_location(self, + location_source: str = None, + frame: AltFrame = AltFrame.ABSOLUTE, + ) -> Location: + '''return the current vehicle location as a (frame-aware) + Location, with the altitude taken in the requested frame. Use + this in preference to the mavutil.location producers + (mav.location(), get_mav_location()). Note that lat/lng and + (for ABSOLUTE and ABOVE_HOME) altitude come from a single + GLOBAL_POSITION_INT, unlike mav.location() which mixes + GPS_RAW_INT and VFR_HUD. location_source of SIMSTATE returns a + lat/lng-only Location as SIMSTATE carries no altitude''' + # drain the link so the message we then block for reflects the + # current position rather than being one which has sat in the + # receive queue: + self.drain_mav() + if location_source == 'SIMSTATE': + self.send_poll_message('SIMSTATE') + m = self.assert_receive_message('SIMSTATE') + lat = m.lat * 1e-7 + lng = m.lng * 1e-7 + if lat == 0 and lng == 0: + raise ValueError(f"Bad lat/lng {lat=} {lng=}") + return Location.latlon_only(lat, lng) + if location_source is not None and location_source != 'GLOBAL_POSITION_INT': + raise ValueError(f"Unknown location source {location_source}") + self.send_poll_message('GLOBAL_POSITION_INT') + m = self.assert_receive_message('GLOBAL_POSITION_INT') + lat = m.lat * 1e-7 + lng = m.lon * 1e-7 + if lat == 0 and lng == 0: + raise ValueError(f"Bad lat/lng {lat=} {lng=}") + if frame == AltFrame.ABSOLUTE: + return Location(lat, lng, m.alt * 0.001, frame) + if frame == AltFrame.ABOVE_HOME: + return Location(lat, lng, m.relative_alt * 0.001, frame) + if frame == AltFrame.ABOVE_TERRAIN: + self.send_poll_message('TERRAIN_REPORT') + terrain = self.assert_receive_message('TERRAIN_REPORT') + return Location(lat, lng, terrain.current_height, frame) + # ABOVE_ORIGIN has no direct message source; convert: + return self.change_alt_frame(Location(lat, lng, m.alt * 0.001, AltFrame.ABSOLUTE), frame) + def wait_distance(self, distance, accuracy=2, timeout=30, location_source=None, **kwargs): """Wait for flight of a given distance.""" start = self.get_mav_location(location_source) @@ -8630,10 +8836,20 @@ class TestSuite(abc.ABC): self.delay_sim_time(0.2, "let RC inputs settle") self.context_pop() - def send_do_reposition(self, - loc, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT): - '''send a DO_REPOSITION command for a location''' + def send_do_reposition(self, loc, frame=None): + '''send a DO_REPOSITION command for a location. loc is ideally + a (frame-aware) Location, in which case the MAV_FRAME comes + from the Location itself and frame must be left None. Passing + a mavutil.location and a frame is deprecated, as the object's + altitude frame can silently contradict the passed frame''' + if isinstance(loc, Location): + if frame is not None: + raise ValueError("frame comes from the Location; do not pass one") + frame, alt = self.mav_frame_and_alt_m(loc) + else: + if frame is None: + frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + alt = loc.alt self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, 0, @@ -8642,7 +8858,7 @@ class TestSuite(abc.ABC): 0, int(loc.lat*1e7), # lat* 1e7 int(loc.lng*1e7), # lon* 1e7 - loc.alt, + alt, frame=frame ) @@ -10032,22 +10248,82 @@ Also, ignores heartbeats not from our target system''' return self.get_distance_int(m, here) def home_position_as_mav_location(self): + '''deprecated; use home_position_as_location() instead''' m = self.poll_home_position() return mavutil.location(m.latitude*1.0e-7, m.longitude*1.0e-7, m.altitude*1.0e-3, 0) + def home_position_as_location(self) -> Location: + '''return home position as a (frame-aware) Location; home + altitude is AMSL''' + m = self.poll_home_position() + return Location(m.latitude*1.0e-7, m.longitude*1.0e-7, m.altitude*1.0e-3, AltFrame.ABSOLUTE) + + def change_alt_frame(self, loc: Location, desired_frame: AltFrame) -> Location: + '''return a copy of loc with its altitude converted to + desired_frame. Conversion goes via AMSL, polling home + position / global origin / terrain data from the vehicle as + required, much as the C++ Location::change_alt_frame leans on + the AHRS and terrain singletons''' + if not isinstance(loc, Location): + raise ValueError("change_alt_frame requires a Location") + src_frame = loc.alt_frame + if src_frame is None: + raise LocationAltFrameException("cannot convert altitude of lat/lng-only Location") + if src_frame == desired_frame: + return loc.copy() + + def frame_zero_amsl_m(frame): + '''AMSL altitude of the given frame's zero point at loc''' + if frame == AltFrame.ABSOLUTE: + return 0.0 + if frame == AltFrame.ABOVE_HOME: + return self.poll_home_position().altitude * 1.0e-3 + if frame == AltFrame.ABOVE_ORIGIN: + return self.poll_message("GPS_GLOBAL_ORIGIN").altitude * 1.0e-3 + if frame == AltFrame.ABOVE_TERRAIN: + return self.get_terrain_height_at(loc) + raise ValueError(f"Unknown frame {frame}") + + alt_amsl_m = loc.get_alt_m(src_frame) + frame_zero_amsl_m(src_frame) + ret = loc.copy() + ret.set_alt_m(alt_amsl_m - frame_zero_amsl_m(desired_frame), desired_frame) + return ret + + def mav_frame_and_alt_m(self, loc: Location): + '''return a (MAV_FRAME, alt_m) tuple for sending loc's altitude + over MAVLink in COMMAND_INT, mission items and elsewhere. + ABOVE_ORIGIN is converted to ABOVE_HOME as the MAVLink global + frames have no origin-relative variant''' + if loc.alt_frame == AltFrame.ABOVE_ORIGIN: + loc = self.change_alt_frame(loc, AltFrame.ABOVE_HOME) + return loc.mav_frame(), loc.get_alt_m(loc.alt_frame) + def offset_location_ne(self, location, metres_north, metres_east): - '''return a new location offset from passed-in location''' + '''return a new location offset from passed-in location; + preserves the type (and, for Location, the altitude frame) of + its input''' (target_lat, target_lng) = mavextra.gps_offset(location.lat, location.lng, metres_east, metres_north) + if isinstance(location, Location): + ret = location.copy() + ret.lat = target_lat + ret.lng = target_lng + return ret return mavutil.location(target_lat, target_lng, location.alt, location.heading) def offset_location_up(self, location, metres_up): - '''return a new location offset from passed-in location''' + '''return a new location offset from passed-in location; + preserves the type (and, for Location, the altitude frame) of + its input''' + if isinstance(location, Location): + ret = location.copy() + ret.offset_up_m(metres_up) + return ret return mavutil.location( location.lat, location.lng, @@ -10056,12 +10332,20 @@ Also, ignores heartbeats not from our target system''' ) def offset_location_heading_distance(self, location, bearing, distance): + '''return a new location offset from passed-in location; + preserves the type (and, for Location, the altitude frame) of + its input''' (target_lat, target_lng) = mavextra.gps_newpos( location.lat, location.lng, bearing, distance ) + if isinstance(location, Location): + ret = location.copy() + ret.lat = target_lat + ret.lng = target_lng + return ret return mavutil.location( target_lat, target_lng, @@ -10070,12 +10354,18 @@ Also, ignores heartbeats not from our target system''' ) def set_home(self, loc): - '''set home to supplied loc - adds implicit reboot at end of test''' + '''set home to supplied loc - adds implicit reboot at end of test. + The command's altitude is AMSL; a Location in another frame is + converted (against the *current* home/origin/terrain)''' + if isinstance(loc, Location): + alt = self.change_alt_frame(loc, AltFrame.ABSOLUTE).get_alt_m(AltFrame.ABSOLUTE) + else: + alt = loc.alt self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_SET_HOME, p5=int(loc.lat*1e7), p6=int(loc.lng*1e7), - p7=loc.alt, + p7=alt, ) # we need to reboot the vehicle after setting home as it will # no longer drift with the vehicle position while disarmed. @@ -12889,12 +13179,22 @@ Also, ignores heartbeats not from our target system''' self.check_fence_upload_download(items) def rally_MISSION_ITEM_INT_from_loc(self, loc): + '''create a rally point MISSION_ITEM_INT from loc. A + (frame-aware) Location's altitude frame is carried into the + item's frame, above-origin being converted to above-home as + missions have no origin-relative frame. A mavutil.location's + alt is taken as AMSL''' + if isinstance(loc, Location): + frame, alt = self.mav_frame_and_alt_m(loc) + else: + frame = mavutil.mavlink.MAV_FRAME_GLOBAL + alt = loc.alt return self.create_MISSION_ITEM_INT( mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT, x=int(loc.lat*1e7), y=int(loc.lng*1e7), - z=loc.alt, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + z=alt, + frame=frame, mission_type=mavutil.mavlink.MAV_MISSION_TYPE_RALLY )