From 503aba17a88020f8850d05041888d989f2ebdf3f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 19:49:53 +1000 Subject: [PATCH 01/22] mp_util: name the commands pymavlink 2.4.49 does not have pymavlink 2.4.49, the latest release, has no MAV_CMD_NAV_ARC_WAYPOINT or MAV_CMD_DO_ORBIT in any of its dialects, while setup.py asks only for pymavlink>=2.4.38. The 2D map module, its mission arcs, the 3D map and mission_circle_radius() all look them up in the dialect, so with a released pymavlink they raise AttributeError: drawing a mission with a loiter in it fails, and 21 of the tests do too. Name the two commands in mp_util, and use those. The arc waypoint was given a fallback of its own in each of the three modules which draw it; there is one name for it now, and DO_ORBIT, which is still looked up in the dialect, has one too. --- MAVProxy/modules/lib/mp_util.py | 9 +++++++-- MAVProxy/modules/mavproxy_map/__init__.py | 12 ++++-------- MAVProxy/modules/mavproxy_map/mp_slipmap_util.py | 7 +------ MAVProxy/modules/mavproxy_map3d/elements.py | 7 +------ tests/test_mission_geometry.py | 10 +++++----- 5 files changed, 18 insertions(+), 27 deletions(-) diff --git a/MAVProxy/modules/lib/mp_util.py b/MAVProxy/modules/lib/mp_util.py index 4240d288e8..f358e01b0a 100644 --- a/MAVProxy/modules/lib/mp_util.py +++ b/MAVProxy/modules/lib/mp_util.py @@ -13,6 +13,11 @@ import re from math import cos, sin, tan, atan2, sqrt, radians, degrees, pi, log, fmod +# MAVLink commands newer than some pymavlink releases know about -- 2.4.49 +# has neither -- so they are named here rather than looked up in the dialect +MAV_CMD_DO_ORBIT = 34 +MAV_CMD_NAV_ARC_WAYPOINT = 36 + # Some platforms (CYGWIN and others) many not have the wx library # use imp to see if wx is on the path has_wxpython = False @@ -272,7 +277,7 @@ def mission_circle_radius(command, params, default_radius=None, vehicle=None): mavlink.MAV_CMD_NAV_LOITER_TURNS: 2, mavlink.MAV_CMD_NAV_LOITER_TIME: 2, mavlink.MAV_CMD_NAV_LOITER_TO_ALT: 1, - getattr(mavlink, 'MAV_CMD_DO_ORBIT', 34): 0, + MAV_CMD_DO_ORBIT: 0, }.get(command) if index is None: return None @@ -299,7 +304,7 @@ def mission_circle_turns(command, params): mavlink = mavutil.mavlink if command == mavlink.MAV_CMD_NAV_LOITER_TURNS: turns = params[0] - elif command == getattr(mavlink, 'MAV_CMD_DO_ORBIT', 34): + elif command == MAV_CMD_DO_ORBIT: # DO_ORBIT counts in radians rather than turns turns = params[3] / (2 * pi) else: diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 95e387a9de..8a42a23fe6 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -18,10 +18,6 @@ from pymavlink import mavutil from PIL import ImageColor -# Older pymavlink releases lack this common.xml command. -MAV_CMD_NAV_ARC_WAYPOINT = getattr(mavutil.mavlink, "MAV_CMD_NAV_ARC_WAYPOINT", 36) -MAV_CMD_DO_ORBIT = getattr(mavutil.mavlink, "MAV_CMD_DO_ORBIT", 34) - # pymavlink may not yet carry the enumeration entry for the # home-centred inclusion circle. Fall back to its known value (from # development.xml) so we don't raise AttributeError on older pymavlink: @@ -157,14 +153,14 @@ def __init__(self, mpstate): # waypoint commands mavutil.mavlink.MAV_CMD_NAV_WAYPOINT: (0, 255, 255), mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT: (64, 255, 64), - MAV_CMD_NAV_ARC_WAYPOINT: (64, 255, 255), + mp_util.MAV_CMD_NAV_ARC_WAYPOINT: (64, 255, 255), # circling commands mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM: (255, 64, 255), mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS: (255, 64, 255), mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME: (255, 64, 255), mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: (255, 64, 255), - MAV_CMD_DO_ORBIT: (255, 64, 255), + mp_util.MAV_CMD_DO_ORBIT: (255, 64, 255), # other commands mavutil.mavlink.MAV_CMD_DO_LAND_START: (255, 127, 0), @@ -173,12 +169,12 @@ def __init__(self, mpstate): mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: "TOff", mavutil.mavlink.MAV_CMD_DO_LAND_START: "DLS", mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT: "SW", - MAV_CMD_NAV_ARC_WAYPOINT: "AW", + mp_util.MAV_CMD_NAV_ARC_WAYPOINT: "AW", mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM: "LU", mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS: "LT", mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME: "LTime", mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: "LAlt", - MAV_CMD_DO_ORBIT: "Orbit", + mp_util.MAV_CMD_DO_ORBIT: "Orbit", mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: "VL", } diff --git a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py index 1b9a383471..a2ac0a69d6 100644 --- a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py +++ b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py @@ -14,14 +14,9 @@ import numpy as np import warnings -from pymavlink import mavutil - from MAVProxy.modules.mavproxy_map import mp_tile from MAVProxy.modules.lib import mp_util -# Older pymavlink releases lack this common.xml command. -MAV_CMD_NAV_ARC_WAYPOINT = getattr(mavutil.mavlink, "MAV_CMD_NAV_ARC_WAYPOINT", 36) - font = cv2.FONT_HERSHEY_DUPLEX @@ -45,7 +40,7 @@ def mission_arcs(wploader, wp_indexes): wp = wploader.wp(wp_indexes[i]) if wp is None: continue - if wp.command != MAV_CMD_NAV_ARC_WAYPOINT: + if wp.command != mp_util.MAV_CMD_NAV_ARC_WAYPOINT: continue # the arc runs from the previous point in the mission view arcs[i-1] = wp.param1 diff --git a/MAVProxy/modules/mavproxy_map3d/elements.py b/MAVProxy/modules/mavproxy_map3d/elements.py index a431feeaa7..4990ab0786 100644 --- a/MAVProxy/modules/mavproxy_map3d/elements.py +++ b/MAVProxy/modules/mavproxy_map3d/elements.py @@ -9,15 +9,10 @@ import vtk -from pymavlink import mavutil - from MAVProxy.modules.lib import mp_util from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem from MAVProxy.modules.mavproxy_map3d.terrain import enu, R -# Older pymavlink releases lack this common.xml command. -MAV_CMD_NAV_ARC_WAYPOINT = getattr(mavutil.mavlink, "MAV_CMD_NAV_ARC_WAYPOINT", 36) - # MAV_FRAME altitude conventions FRAME_GLOBAL = (0, 5) # AMSL FRAME_RELATIVE = (3, 6) # relative to home @@ -533,7 +528,7 @@ def set_mission(self, items): flown = [i for i in flown if not (i.lat == 0 and i.lon == 0)] for (index, item) in enumerate(flown): amsl = self._resolve_amsl(item.alt, item.frame) - if (item.command == MAV_CMD_NAV_ARC_WAYPOINT and + if (item.command == mp_util.MAV_CMD_NAV_ARC_WAYPOINT and previous is not None): # the leg into an arc waypoint is a circular arc rather # than a straight line; climb linearly along it diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 21c6a41fe8..27e99137ea 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -134,7 +134,7 @@ def test_radius_parameter_per_command(self): (m.MAV_CMD_NAV_LOITER_TURNS, (3, 0, -55, 0), -55), (m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0), 90), (m.MAV_CMD_NAV_LOITER_TO_ALT, (1, -65, 0, 0), -65), - (getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, 0), 80), + (mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, 0), 80), ] for (command, params, expected) in cases: assert mp_util.mission_circle_radius(command, params) == expected @@ -142,7 +142,7 @@ def test_radius_parameter_per_command(self): def test_items_which_do_not_circle(self): m = self.mavlink for command in (m.MAV_CMD_NAV_WAYPOINT, - 36, # MAV_CMD_NAV_ARC_WAYPOINT (absent in older pymavlink) + mp_util.MAV_CMD_NAV_ARC_WAYPOINT, m.MAV_CMD_NAV_TAKEOFF, m.MAV_CMD_DO_JUMP): assert mp_util.mission_circle_radius(command, (1, 2, 3, 4)) is None @@ -162,10 +162,10 @@ def test_turn_counts(self): m.MAV_CMD_NAV_LOITER_TURNS, (3, 0, 60, 0)) == 3 # DO_ORBIT counts in radians assert mp_util.mission_circle_turns( - getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, math.radians(270))) == pytest.approx(0.75) + mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, math.radians(270))) == pytest.approx(0.75) # circling forever, and items which do not count turns assert mp_util.mission_circle_turns( - getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, 0)) is None + mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, 0)) is None assert mp_util.mission_circle_turns( m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0)) is None @@ -192,7 +192,7 @@ def test_a_hovering_vehicle_only_circles_for_some_items(self): m = self.mavlink # it flies these as circles ... for (command, params) in ((m.MAV_CMD_NAV_LOITER_TURNS, (2, 0, 60, 0)), - (getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, 0))): + (mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, 0))): assert mp_util.mission_circle_radius( command, params, vehicle=m.MAV_TYPE_QUADROTOR) is not None # ... and holds position for these, climbing straight up rather than From cac8ac85766815678722285f6a7d407b493fc3fd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 15 Sep 2026 16:36:18 +1000 Subject: [PATCH 02/22] map3d: take longitudes the short way round the antimeridian Everything the 3D map draws -- terrain, the flight path, the mission, fences, rally points and KML -- goes through enu() into a local frame about the view's origin, which subtracted longitudes as they are. Either side of the antimeridian was then drawn the world apart: with the origin at 179.998, a point at -179.999 a few hundred metres east landed nearly 40,000km west. Wrap the difference, as ArduPilot's diff_longitude() does. A point given past 180 instead of wrapped, as ring_points() gives them, still lands where it did. The rest of the map made the same mistake. The camera's focal point went past 180 as it moved east over the antimeridian, picking terrain tiles from the western edge of the world rather than the ones beside it, and tiles were fetched by how far they were from the camera in unwrapped longitude, so those just across were fetched last. Terrain heights under a point past 180 were looked for in no tile at all, terrain samples fetched the wrong tile, looking at a point across the antimeridian turned the camera the world away, and a fence edge across it was sampled for terrain the long way round. MAVExplorer centred its view on the average longitude of the flight path, which for a flight across the antimeridian is on the far side of the world. --- MAVProxy/modules/mavproxy_map3d/elements.py | 6 +- MAVProxy/modules/mavproxy_map3d/map3d_ui.py | 5 +- MAVProxy/modules/mavproxy_map3d/terrain.py | 19 ++- MAVProxy/tools/MAVExplorer.py | 58 ++++---- tests/test_mission_geometry.py | 142 ++++++++++++++++++++ 5 files changed, 200 insertions(+), 30 deletions(-) diff --git a/MAVProxy/modules/mavproxy_map3d/elements.py b/MAVProxy/modules/mavproxy_map3d/elements.py index 4990ab0786..0f8d6792a2 100644 --- a/MAVProxy/modules/mavproxy_map3d/elements.py +++ b/MAVProxy/modules/mavproxy_map3d/elements.py @@ -11,7 +11,7 @@ from MAVProxy.modules.lib import mp_util from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem -from MAVProxy.modules.mavproxy_map3d.terrain import enu, R +from MAVProxy.modules.mavproxy_map3d.terrain import enu, R, wrap_longitude # MAV_FRAME altitude conventions FRAME_GLOBAL = (0, 5) # AMSL @@ -703,6 +703,8 @@ def _terrain_samples(self, points, closed): for i in range(max(0, segment_count)): lat1, lon1 = points[i] lat2, lon2 = points[(i + 1) % len(points)] + # along the edge the short way round, as it is drawn + dlon = wrap_longitude(lon2 - lon1) e1, n1, _ = enu(lat1, lon1, 0.0, self.lat0, self.lon0) e2, n2, _ = enu(lat2, lon2, 0.0, self.lat0, self.lon0) distance = math.hypot(e2 - e1, n2 - n1) @@ -711,7 +713,7 @@ def _terrain_samples(self, points, closed): for j in range(count): t = float(j) / count yield (lat1 + (lat2 - lat1) * t, - lon1 + (lon2 - lon1) * t) + lon1 + dlon * t) if points: yield points[0] if closed else points[-1] diff --git a/MAVProxy/modules/mavproxy_map3d/map3d_ui.py b/MAVProxy/modules/mavproxy_map3d/map3d_ui.py index bb145470cf..cbb26fa79f 100644 --- a/MAVProxy/modules/mavproxy_map3d/map3d_ui.py +++ b/MAVProxy/modules/mavproxy_map3d/map3d_ui.py @@ -14,7 +14,7 @@ from MAVProxy.modules.mavproxy_map import mp_tile from MAVProxy.modules.mavproxy_map3d.camera import TerrainCamera, TerrainStyle from MAVProxy.modules.mavproxy_map3d.terrain import ( - TerrainManager, R, TILE_DOWNLOAD_THREADS) + TerrainManager, R, TILE_DOWNLOAD_THREADS, wrap_longitude) from MAVProxy.modules.mavproxy_map3d.elements import ElementManager import vtk @@ -319,7 +319,8 @@ def render(self): self.widget.GetRenderWindow().Render() def look_at_latlon(self, lat, lon, amsl, dist=None): - e = math.radians(lon - self.terrain.lon0) * R * math.cos(math.radians(self.terrain.lat0)) + e = (math.radians(wrap_longitude(lon - self.terrain.lon0)) * R * + math.cos(math.radians(self.terrain.lat0))) n = math.radians(lat - self.terrain.lat0) * R u = amsl * self.state.zexag self.tc.look_at((e, n, u), dist=dist) diff --git a/MAVProxy/modules/mavproxy_map3d/terrain.py b/MAVProxy/modules/mavproxy_map3d/terrain.py index ca973af7d2..48c8341a14 100644 --- a/MAVProxy/modules/mavproxy_map3d/terrain.py +++ b/MAVProxy/modules/mavproxy_map3d/terrain.py @@ -43,8 +43,15 @@ module=r"quantized_mesh_tile\.terrain") +def wrap_longitude(lon): + '''a longitude, or a difference of two, as the one from -180 up to 180 + it is. Differences wrapped take the short way round, so that either side + of the antimeridian is next to the other rather than the world apart''' + return (lon + 180.0) % 360.0 - 180.0 + + def enu(lat, lon, h, lat0, lon0): - e = math.radians(lon - lon0) * R * math.cos(math.radians(lat0)) + e = math.radians(wrap_longitude(lon - lon0)) * R * math.cos(math.radians(lat0)) n = math.radians(lat - lat0) * R return e, n, h @@ -108,6 +115,7 @@ def sample_terrain(lat, lon, zoom=12, cache_only=False): cache_only returns None rather than fetching/decoding a missing tile, so callers on a latency-sensitive thread can defer the work.''' g = GlobalGeodetic(True) + lon = wrap_longitude(lon) x, y = g.LonLatToTile(lon, lat, zoom) key = (zoom, x, y) dec = _sample_cache.get(key) @@ -358,7 +366,8 @@ def _worker(self): def focal_latlon(self, tc): foclat = self.lat0 + math.degrees(tc.focal[1] / R) - foclon = self.lon0 + math.degrees(tc.focal[0] / (R * math.cos(math.radians(self.lat0)))) + foclon = wrap_longitude(self.lon0 + math.degrees( + tc.focal[0] / (R * math.cos(math.radians(self.lat0))))) return foclat, foclon def desired_set(self, foclat, foclon): @@ -380,6 +389,10 @@ def desired_set(self, foclat, foclon): def height_at(self, lat, lon): '''Return rendered terrain world Z at lat/lon, preferring fine tiles.''' e, n, _ = enu(lat, lon, 0.0, self.lat0, self.lon0) + lon = wrap_longitude(lon) + if lon == -180.0: + # the antimeridian is either tile's edge; 180 is the one tiles have + lon = 180.0 for key, tile in sorted(self.tiles.items(), reverse=True): west, south, east, north = tile.bbox if not (west <= lon <= east and south <= lat <= north): @@ -401,7 +414,7 @@ def tile_priority(key): clat = 0.5 * (south + north) clon = 0.5 * (west + east) dlat = clat - foclat - dlon = (clon - foclon) * math.cos(math.radians(foclat)) + dlon = wrap_longitude(clon - foclon) * math.cos(math.radians(foclat)) return (-z, dlat * dlat + dlon * dlon) for (z, x, y) in sorted(want, key=tile_priority): diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index e89e814e7d..544085753b 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -661,6 +661,40 @@ def cmd_map(args): map3d_views = [] +def path_view(path): + '''where a 3D map of a flight path looks from: the middle of the path, + its lowest altitude and how far across it is, as (lat, lon, amsl, span). + Longitudes are taken from the first point's the short way round, so a + flight across the antimeridian is centred on it rather than on the far + side of the world''' + # note: sum/min/max are shadowed in this namespace (mavextra import *), so + # accumulate explicitly + first_lon = path[0][1] + sumlat = sumlon = 0.0 + minlat = maxlat = path[0][0] + minlon = maxlon = 0.0 + ground0 = path[0][2] + for p in path: + la, lo, al = p[:3] + lo = mp_util.wrap_180(lo - first_lon) + sumlat += la + sumlon += lo + if al < ground0: + ground0 = al + minlat = la if la < minlat else minlat + maxlat = la if la > maxlat else maxlat + minlon = lo if lo < minlon else minlon + maxlon = lo if lo > maxlon else maxlon + lat0 = sumlat / len(path) + lon0 = mp_util.wrap_180(first_lon + sumlon / len(path)) + span_ns = mp_util.gps_distance(minlat, 0.0, maxlat, 0.0) + span_ew = mp_util.gps_distance(minlat, 0.0, minlat, maxlon - minlon) + span = span_ns if span_ns > span_ew else span_ew + if span < 1000.0: + span = 1000.0 + return (lat0, lon0, ground0, span) + + def cmd_map3d(args): '''show a 3D map view: draped satellite imagery over terrain''' try: @@ -696,29 +730,7 @@ def cmd_map3d(args): print("No POS messages found for 3D map") return - # note: sum/min/max are shadowed in this namespace (mavextra import *), so - # accumulate explicitly - sumlat = sumlon = 0.0 - minlat = maxlat = path[0][0] - minlon = maxlon = path[0][1] - ground0 = path[0][2] - for p in path: - la, lo, al = p[:3] - sumlat += la - sumlon += lo - if al < ground0: - ground0 = al - minlat = la if la < minlat else minlat - maxlat = la if la > maxlat else maxlat - minlon = lo if lo < minlon else minlon - maxlon = lo if lo > maxlon else maxlon - lat0 = sumlat / len(path) - lon0 = sumlon / len(path) - span_ns = mp_util.gps_distance(minlat, minlon, maxlat, minlon) - span_ew = mp_util.gps_distance(minlat, minlon, minlat, maxlon) - span = span_ns if span_ns > span_ew else span_ew - if span < 1000.0: - span = 1000.0 + (lat0, lon0, ground0, span) = path_view(path) # resolve mission item altitudes to AMSL before sending. Terrain-frame # waypoints (MAV_FRAME_GLOBAL_TERRAIN_ALT = 10/11) are "z above terrain", so diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 27e99137ea..55ef2a2d68 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -24,6 +24,148 @@ def swept_angle(centre, points): for i in range(1, len(bearings))) +class TestProjection(object): + """the local frame everything in the 3D map is drawn in""" + + def test_the_antimeridian_is_crossed_the_short_way(self): + pytest.importorskip("vtk") + from MAVProxy.modules.mavproxy_map3d.terrain import enu + lat = -16.5 + (east, _, _) = enu(lat, -179.999, 0.0, lat, 179.998) + (west, _, _) = enu(lat, 179.998, 0.0, lat, -179.999) + metres = math.radians(0.003) * 6378137.0 * math.cos(math.radians(lat)) + assert east == pytest.approx(metres, rel=0.01) + assert west == pytest.approx(-metres, rel=0.01) + # and away from it, as it always was + (e, n, u) = enu(HERE[0] + 0.01, HERE[1] + 0.01, 5.0, HERE[0], HERE[1]) + assert e > 0 and n > 0 and u == 5.0 + + # an origin just west of the antimeridian, and a point just east of it + SEAM = (-16.5, 179.998) + + def terrain(self): + pytest.importorskip("vtk") + from quantized_mesh_tile.global_geodetic import GlobalGeodetic + from MAVProxy.modules.mavproxy_map3d.terrain import TerrainManager + manager = TerrainManager.__new__(TerrainManager) + (manager.lat0, manager.lon0) = self.SEAM + manager.g = GlobalGeodetic(True) + (manager.zoom_fine, manager.lod_min) = (12, 8) + (manager.ring, manager.fine_radius) = (1, 2) + manager.tiles = {} + return manager + + def test_the_camera_looks_across_the_antimeridian(self): + from types import SimpleNamespace + manager = self.terrain() + east = math.radians(0.003) * 6378137.0 * math.cos(math.radians(self.SEAM[0])) + for (focal_east, lon) in ((east, -179.999), (-east, 179.995)): + camera = SimpleNamespace(focal=(focal_east, 0.0, 0.0)) + (lat, focal_lon) = manager.focal_latlon(camera) + assert (lat, focal_lon) == pytest.approx((self.SEAM[0], lon), abs=1e-6) + # and the tiles wanted about a point just east of it are those either + # side of it, not the ones at the western edge of the western tile row + (lat, lon) = manager.focal_latlon(SimpleNamespace(focal=(east, 0.0, 0.0))) + fine = sorted(x for (z, x, y) in manager.desired_set(lat, lon) + if z == manager.zoom_fine) + columns = manager.g.GetNumberOfXTilesAtZoom(manager.zoom_fine) + assert set(fine) == {0, 1, 2, columns - 2, columns - 1} + + def test_terrain_nearest_the_camera_is_fetched_first_across_the_antimeridian(self): + from types import SimpleNamespace + manager = self.terrain() + queued = [] + manager.jobs = SimpleNamespace(put=queued.append) + (manager.inflight, manager.mesh_revision) = (set(), 0) + east = math.radians(0.003) * 6378137.0 * math.cos(math.radians(self.SEAM[0])) + manager.update(SimpleNamespace( + focal=(east, 0.0, 0.0), pos=(0.0, 0.0, 0.0), + cam=SimpleNamespace(GetViewAngle=lambda: 30.0))) + (x, row) = manager.g.LonLatToTile(-179.999, self.SEAM[0], manager.zoom_fine) + columns = manager.g.GetNumberOfXTilesAtZoom(manager.zoom_fine) + order = [x for (_, _, (z, x, y)) in queued + if (z, y) == (manager.zoom_fine, row)] + # either side of the antimeridian in turn, not all of the east first + assert order == [0, columns - 1, 1, columns - 2, 2] + + def test_terrain_heights_are_found_either_side_of_the_antimeridian(self): + from types import SimpleNamespace + manager = self.terrain() + west_tile = SimpleNamespace(bbox=(170.0, -20.0, 180.0, -10.0), + height_at=lambda e, n: 10.0) + east_tile = SimpleNamespace(bbox=(-180.0, -20.0, -170.0, -10.0), + height_at=lambda e, n: 20.0) + manager.tiles = {(12, 1, 0): west_tile, (12, 0, 0): east_tile} + # a ring about a point near it runs on past 180 rather than wrapping + assert manager.height_at(-16.5, 180.001) == 20.0 + assert manager.height_at(-16.5, -180.0) == 10.0 + assert manager.height_at(-16.5, 179.999) == 10.0 + assert manager.height_at(-16.5, -179.999) == 20.0 + + def test_terrain_is_sampled_either_side_of_the_antimeridian(self, monkeypatch): + pytest.importorskip("vtk") + import numpy as np + from MAVProxy.modules.mavproxy_map3d import terrain + fetched = [] + + def decode(z, x, y): + fetched.append((z, x, y)) + return {"bbox": (-180.0, -20.0, -170.0, -10.0), + "verts": np.array([(-180.0, -10.0, 1.0), (-170.0, -10.0, 1.0), + (-180.0, -20.0, 1.0), (-170.0, -20.0, 1.0)])} + monkeypatch.setattr(terrain, 'decode_terrain', decode) + monkeypatch.setattr(terrain, '_sample_cache', {}) + assert terrain.sample_terrain(-16.5, 180.001) == pytest.approx(1.0) + assert fetched == [(12,) + terrain.GlobalGeodetic(True).LonLatToTile( + -179.999, -16.5, 12)] + + def test_the_view_is_turned_across_the_antimeridian(self): + pytest.importorskip("vtk") + pytest.importorskip("wx") + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + looked = [] + frame = SimpleNamespace( + terrain=SimpleNamespace(lat0=self.SEAM[0], lon0=self.SEAM[1]), + tc=SimpleNamespace(look_at=lambda focal, dist=None: looked.append(focal)), + state=SimpleNamespace(zexag=1.0), on_camera_change=lambda: None) + Map3DFrame.look_at_latlon(frame, self.SEAM[0], -179.999, 0.0) + east = math.radians(0.003) * 6378137.0 * math.cos(math.radians(self.SEAM[0])) + assert looked[0] == pytest.approx((east, 0.0, 0.0), abs=1.0) + + def test_draped_lines_are_sampled_the_short_way_round(self): + pytest.importorskip("vtk") + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + em = ElementManager.__new__(ElementManager) + (em.lat0, em.lon0) = self.SEAM + points = [(self.SEAM[0], 179.998), (self.SEAM[0], -179.998)] + samples = list(em._terrain_samples(points, closed=False)) + assert len(samples) > 2 + for (lat, lon) in samples: + assert 0.0 <= mp_util.wrap_180(lon - 179.998) <= 0.004 + 1e-9 + + def test_mavexplorer_looks_at_a_flight_across_the_antimeridian(self): + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + import os + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + # starting just east of it, and going further west than east + flight = [(-16.5, -179.9995, 30.0), (-16.5, 179.99, 20.0), + (-16.49, -179.995, 25.0)] + (lat, lon, ground, span) = module.path_view(flight) + assert lat == pytest.approx(-16.49667, abs=1e-4) + assert lon == pytest.approx(179.9985, abs=1e-4) + assert ground == 20.0 + east_west = math.radians(0.015) * mp_util.radius_of_earth * math.cos( + math.radians(-16.49)) + assert span == pytest.approx(east_west, rel=0.01) + + class TestRhumbHelpers(object): def test_distance_takes_the_short_way_around(self): From 0200ea4cd533701bccff1add2f9873d7e7bc17b3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 12:35:33 +1000 Subject: [PATCH 03/22] map3d: resolve altitude frames before measuring a loiter climb "map: plot the loiter path a vehicle actually flies" works out how many turns a NAV_LOITER_TO_ALT is drawn with from how much climb is left when the vehicle arrives. The live module measured that by subtracting the raw altitude of the item before from the loiter's own, whatever frames the two were in, so an AMSL item before a home-relative loiter read as a dive of the whole height of the field, and the spiral was drawn with the turns of that dive. Resolve both to AMSL before subtracting. An altitude that cannot be resolved yet is unknown rather than guessed: a relative item before home is known, or a terrain-frame item before the terrain under it has been fetched, has nothing to measure against, and its loiter is drawn as a single turn. MAVExplorer already resolved its items before measuring, so it was not affected. The test goes through send_mission() itself rather than only the helper, and fails against the raw subtraction. --- MAVProxy/modules/mavproxy_map3d/__init__.py | 29 ++++++- tests/test_mission_geometry.py | 94 +++++++++++++++++++++ 2 files changed, 119 insertions(+), 4 deletions(-) diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index 4a06165d6d..d87400e5a2 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -277,6 +277,23 @@ def mission_home(self, wploader): return None return (home.x, home.y) + def item_amsl(self, alt, frame): + '''the AMSL altitude of a mission item, or None if we cannot tell. + + The viewer resolves the frames itself; this is only so that the climb + between two items can be measured with both of them in the one frame + ''' + if frame in (0, 5): # already AMSL + return alt + if frame in (10, 11): + # still above terrain: send_mission() turns these into AMSL once + # it has the terrain height, and until then nobody knows + return None + if self.home_amsl is None: + # relative to a home we have not been told about yet + return None + return self.home_amsl + alt + def send_mission(self): if self.map is None: return @@ -307,6 +324,9 @@ def send_mission(self): if terr is not None: z = terr + w.z frame = 0 + # the items are drawn in whatever frames they carry, so resolve + # the altitude before measuring anything against the item before + amsl = self.item_amsl(z, frame) circle_radius = mp_util.mission_circle_radius( w.command, (w.param1, w.param2, w.param3, w.param4), @@ -317,13 +337,14 @@ def send_mission(self): # this one circles until it reaches its altitude, so what is # left to climb on arrival decides how many turns to draw approach = None + alt_change = None if previous is not None: approach = mp_util.gps_distance(previous[0], previous[1], lat, lon) + if amsl is not None and previous[2] is not None: + alt_change = amsl - previous[2] circle_turns = mp_util.loiter_to_alt_turns( - circle_radius, - z - previous[2] if previous is not None else None, - self.mav_param, approach) + circle_radius, alt_change, self.mav_param, approach) exit_converge = None if circle_radius is not None and not (w.param4 > 0): # param4 == 0 asks for the next leg to be crosstracked from @@ -332,7 +353,7 @@ def send_mission(self): items.append(MissionItem(lat, lon, z, frame, w.command, w.seq, w.param1, circle_radius, circle_turns, exit_converge)) - previous = (lat, lon, z) + previous = (lat, lon, amsl) self.map.set_mission(items) def set_icon_type(self, name): diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 55ef2a2d68..bc17021710 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -24,6 +24,39 @@ def swept_angle(centre, points): for i in range(1, len(bearings))) +def waypoint(seq, command, x, y, z, frame=3, params=(0, 0, 0, 0)): + '''a mission item the way the wp module's loader holds one''' + from types import SimpleNamespace + return SimpleNamespace(seq=seq, command=command, x=x, y=y, z=z, + frame=frame, param1=params[0], param2=params[1], + param3=params[2], param4=params[3]) + + +def live_mission_items(wpoints, home_amsl=584.0, params=None, + vehicle='plane', default_radius=60.0): + '''the MissionItems the live map3d module sends for a mission. + + The module is built without the MAVProxy around it: only what + send_mission() reads is supplied, and terrain is never available + ''' + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import Map3DModule + module = Map3DModule.__new__(Map3DModule) + loader = SimpleNamespace(wpoints=wpoints, wp=lambda i: wpoints[i]) + module.mpstate = SimpleNamespace( + mav_param=params or {}, vehicle_type=vehicle, + module=lambda name: (SimpleNamespace(wploader=loader) + if name == 'wp' else None)) + sent = [] + module.map = SimpleNamespace(set_mission=sent.extend) + module.home_amsl = home_amsl + module.home_position = None + module.default_circle_radius = lambda: default_radius + module.terrain_alt = lambda lat, lon: None + module.send_mission() + return sent + + class TestProjection(object): """the local frame everything in the 3D map is drawn in""" @@ -312,6 +345,67 @@ def test_turn_counts(self): m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0)) is None +class TestLiveMissionAltitudes(object): + """the map3d module measures the climb into a loiter itself, to work out + how many turns it takes, so it has to resolve the frames first""" + + def module(self, home_amsl): + from MAVProxy.modules.mavproxy_map3d import Map3DModule + # the module talks to a live MAVProxy, which is not what is under + # test here: only the altitude it hands the turn count + module = Map3DModule.__new__(Map3DModule) + module.home_amsl = home_amsl + return module + + def test_frames_resolve_to_amsl(self): + module = self.module(584.0) + for frame in (0, 5): + assert module.item_amsl(700.0, frame) == 700.0 + for frame in (3, 6): + assert module.item_amsl(100.0, frame) == 684.0 + + def test_a_terrain_altitude_without_terrain_is_unknown(self): + # send_mission() turns a terrain-frame item into AMSL when it has the + # terrain height; one still in frame 10 or 11 has no known height, + # and home is not a stand-in for the ground under it + module = self.module(584.0) + for frame in (10, 11): + assert module.item_amsl(100.0, frame) is None + + def test_a_climb_across_two_frames(self): + module = self.module(584.0) + # 600m AMSL to 100m above a home at 584m is a climb of 84m, not the + # 500m descent the raw item altitudes look like + first = module.item_amsl(600.0, 0) + second = module.item_amsl(100.0, 3) + assert second - first == pytest.approx(84.0) + + def test_send_mission_measures_the_climb_in_one_frame(self): + from pymavlink import mavutil + m = mavutil.mavlink + params = {'AIRSPEED_CRUISE': 20.0, 'TECS_CLMB_MAX': 5.0, + 'TECS_SINK_MIN': 2.0} + loiter_at = mp_util.gps_newpos(HERE[0], HERE[1], 90, 300) + items = live_mission_items([ + waypoint(0, m.MAV_CMD_NAV_WAYPOINT, HERE[0], HERE[1], 600.0, + frame=0), + waypoint(1, m.MAV_CMD_NAV_LOITER_TO_ALT, loiter_at[0], + loiter_at[1], 400.0, frame=3, params=(0, 60, 0, 0)), + ], home_amsl=584.0, params=params) + # 600m AMSL up to 400m above a 584m home is a climb of 384m; the raw + # altitudes would have it a 200m descent, which takes more turns + climb = mp_util.loiter_to_alt_turns(60, 384.0, params, 300.0) + descent = mp_util.loiter_to_alt_turns(60, -200.0, params, 300.0) + assert abs(climb - descent) > 0.5 + assert items[1].circle_turns == pytest.approx(climb, rel=0.01) + + def test_without_a_home_a_relative_altitude_is_unknown(self): + module = self.module(None) + assert module.item_amsl(100.0, 3) is None + # an AMSL item still stands on its own + assert module.item_amsl(700.0, 0) == 700.0 + + class TestHoveringVehicles(object): def setup_method(self): From 97aaec609c6022660ddb637bdbececfbb98efcef Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 12:36:15 +1000 Subject: [PATCH 04/22] mp_util: only read param4 as a crosstrack choice where it is one "map3d: join and leave loiter circles on a tangent" draws the vehicle pulling back onto a track out of a loiter's centre whenever a circling item's param4 was not positive. That is what param4 means for three of the circling commands and nothing like it for the other two: LOITER_UNLIM's param4 is a yaw angle, and it is never left anyway, and DO_ORBIT counts its turns there. ArduPilot agrees: AP_Mission turns param4 into loiter_xtrack for LOITER_TURNS, LOITER_TIME and LOITER_TO_ALT and for no other command, and ArduPlane only restarts the next leg from the exit point when that flag is set. mission_crosstracks_from_centre() says the same, and both the live map and MAVExplorer ask it rather than testing param4 themselves; each has a test which fails if it goes back to reading param4 directly. --- MAVProxy/modules/lib/mp_util.py | 23 ++++++++ MAVProxy/modules/mavproxy_map3d/__init__.py | 13 +++-- MAVProxy/tools/MAVExplorer.py | 7 +-- tests/test_mission_geometry.py | 60 +++++++++++++++++++++ 4 files changed, 93 insertions(+), 10 deletions(-) diff --git a/MAVProxy/modules/lib/mp_util.py b/MAVProxy/modules/lib/mp_util.py index f358e01b0a..f843cfd2c9 100644 --- a/MAVProxy/modules/lib/mp_util.py +++ b/MAVProxy/modules/lib/mp_util.py @@ -314,6 +314,29 @@ def mission_circle_turns(command, params): return turns +def mission_crosstracks_from_centre(command, params): + """whether the leg out of a circling mission item is flown against a + track starting at the item's own location rather than at the point the + vehicle leaves the circle. + + ArduPlane crosstracks from the centre unless param4 asks for the exit + location instead, and only the loiter items which are flown until + something lets the vehicle go carry that choice there. LOITER_UNLIM + never leaves, and DO_ORBIT counts its turns in param4, so neither is + asked + """ + from pymavlink import mavutil + mavlink = mavutil.mavlink + if command not in (mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavlink.MAV_CMD_NAV_LOITER_TIME, + mavlink.MAV_CMD_NAV_LOITER_TO_ALT): + return False + xtrack = params[3] + if xtrack is None or math.isnan(xtrack): + return True + return not xtrack > 0 + + def param_value(params, name): """look one parameter up in params, which may be a mapping (a live vehicle's mav_param, or a log's params) or a callable taking a name. diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index d87400e5a2..8de07f17fb 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -327,11 +327,9 @@ def send_mission(self): # the items are drawn in whatever frames they carry, so resolve # the altitude before measuring anything against the item before amsl = self.item_amsl(z, frame) + params = (w.param1, w.param2, w.param3, w.param4) circle_radius = mp_util.mission_circle_radius( - w.command, - (w.param1, w.param2, w.param3, w.param4), - default_radius, - self.vehicle_type) + w.command, params, default_radius, self.vehicle_type) circle_turns = None if w.command == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: # this one circles until it reaches its altitude, so what is @@ -346,9 +344,10 @@ def send_mission(self): circle_turns = mp_util.loiter_to_alt_turns( circle_radius, alt_change, self.mav_param, approach) exit_converge = None - if circle_radius is not None and not (w.param4 > 0): - # param4 == 0 asks for the next leg to be crosstracked from - # the loiter centre rather than from where it was left + if (circle_radius is not None and + mp_util.mission_crosstracks_from_centre(w.command, params)): + # the next leg is flown against a track from the loiter + # centre, so the vehicle pulls back onto it after leaving exit_converge = mp_util.vehicle_track_convergence(self.mav_param) items.append(MissionItem(lat, lon, z, frame, w.command, w.seq, w.param1, circle_radius, circle_turns, diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index 544085753b..e086c2e481 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -800,9 +800,10 @@ def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): amsl - previous[2] if previous is not None else None, params, approach) converge = None - if radius is not None and not (prm[3] > 0): - # param4 == 0 asks for the next leg to be crosstracked from the - # loiter centre rather than from where it was left + if (radius is not None and + mp_util.mission_crosstracks_from_centre(cid, prm)): + # the next leg is flown against a track from the loiter centre, + # so the vehicle pulls back onto it after leaving the circle converge = mp_util.vehicle_track_convergence(params) out.append(MissionItem(la, lo, amsl, 0, cid, seq, prm[0], radius, turns, converge)) diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index bc17021710..c2da9fb8c2 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -344,6 +344,33 @@ def test_turn_counts(self): assert mp_util.mission_circle_turns( m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0)) is None + def test_which_items_crosstrack_from_their_centre(self): + m = self.mavlink + # param4 of these picks the track the next leg is flown against: 0 + # asks for one out of the loiter centre, 1 for the exit location + for command in (m.MAV_CMD_NAV_LOITER_TURNS, + m.MAV_CMD_NAV_LOITER_TIME, + m.MAV_CMD_NAV_LOITER_TO_ALT): + assert mp_util.mission_crosstracks_from_centre( + command, (1, 0, 60, 0)) + assert not mp_util.mission_crosstracks_from_centre( + command, (1, 0, 60, 1)) + # an item which does not say gets what the vehicle does by default + assert mp_util.mission_crosstracks_from_centre( + command, (1, 0, 60, float('nan'))) + + def test_items_whose_param4_is_not_a_crosstrack_choice(self): + m = self.mavlink + # LOITER_UNLIM is never left, and its param4 is a yaw angle; DO_ORBIT + # counts its turns there. Neither is asking for a track out + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, mp_util.MAV_CMD_DO_ORBIT): + for param4 in (0, 1, 90, math.radians(270)): + assert not mp_util.mission_crosstracks_from_centre( + command, (80, 5, 60, param4)) + # nor is an item which does not circle at all + assert not mp_util.mission_crosstracks_from_centre( + m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)) + class TestLiveMissionAltitudes(object): """the map3d module measures the climb into a loiter itself, to work out @@ -911,6 +938,39 @@ def test_crosstracking_from_the_exit_flies_straight_there(self): assert len(errors) == 1 +class TestProducersCrosstrack(object): + """what each producer of MissionItems makes of param4. Only the loiters + ArduPlane lets go of carry a crosstrack choice there; LOITER_UNLIM + never leaves, and DO_ORBIT counts its turns in param4, so either + producer reading param4 as a crosstrack choice for those would draw a + pull-back after a loiter the vehicle never leaves""" + + PARAMS = {'NAVL1_PERIOD': 17.0, 'AIRSPEED_CRUISE': 20.0} + + def cases(self): + from pymavlink import mavutil + m = mavutil.mavlink + # (command, params, whether the leg out is pulled back to the centre) + return [ + (m.MAV_CMD_NAV_LOITER_TURNS, (1, 0, 60, 0), True), + (m.MAV_CMD_NAV_LOITER_TURNS, (1, 0, 60, 1), False), + (m.MAV_CMD_NAV_LOITER_UNLIM, (0, 0, 60, 0), False), + ] + + def test_the_live_module(self): + from pymavlink import mavutil + m = mavutil.mavlink + at = [mp_util.gps_newpos(HERE[0], HERE[1], 90, 400 * i) + for i in range(3)] + for (command, params, pulled_back) in self.cases(): + items = live_mission_items([ + waypoint(0, m.MAV_CMD_NAV_WAYPOINT, at[0][0], at[0][1], 100.0), + waypoint(1, command, at[1][0], at[1][1], 100.0, params=params), + waypoint(2, m.MAV_CMD_NAV_WAYPOINT, at[2][0], at[2][1], 100.0), + ], params=self.PARAMS) + assert (items[1].exit_converge is not None) == pulled_back, command + + class TestPolygonBounds(object): def test_bounds_cover_the_arc_and_not_just_the_chord(self): From 2608e9753b18c3da32518ef909e3de5c8e565379 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 12:36:53 +1000 Subject: [PATCH 05/22] map3d: draw the turns LOITER_TURNS and DO_ORBIT ask for "map: plot loiter and orbit commands using their radius" added mission_circle_turns() to read the turn count a LOITER_TURNS or a finite DO_ORBIT carries, but nothing called it: both producers only ever filled circle_turns in for NAV_LOITER_TO_ALT, which works its own count out from the climb. A three-turn loiter climbing while it circled was drawn as a single turn. Hand the count through from the live module and from MAVExplorer, with LOITER_TO_ALT still working its own out. A circling item at a level altitude is still drawn as one turn whatever it asks for, since the rest would be drawn exactly on top of the first. The drawing tests build their MissionItems by hand, which is how this went unnoticed, so the new tests go through the producers instead: the live module's send_mission() and MAVExplorer's resolve_mission_amsl(). --- MAVProxy/modules/mavproxy_map3d/__init__.py | 4 +- MAVProxy/tools/MAVExplorer.py | 4 +- tests/test_mission_geometry.py | 87 +++++++++++++++++++++ 3 files changed, 93 insertions(+), 2 deletions(-) diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index 8de07f17fb..c3bd5aa979 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -330,7 +330,9 @@ def send_mission(self): params = (w.param1, w.param2, w.param3, w.param4) circle_radius = mp_util.mission_circle_radius( w.command, params, default_radius, self.vehicle_type) - circle_turns = None + # items which say how many turns they fly say so themselves; + # None is an item which circles until something else stops it + circle_turns = mp_util.mission_circle_turns(w.command, params) if w.command == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: # this one circles until it reaches its altitude, so what is # left to climb on arrival decides how many turns to draw diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index e086c2e481..cbac712da1 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -790,7 +790,9 @@ def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): amsl = home_amsl + z radius = mp_util.mission_circle_radius(cid, prm, default_radius, mav_type) - turns = None + # items which say how many turns they fly say so themselves; None is + # an item which circles until something else stops it + turns = mp_util.mission_circle_turns(cid, prm) if cid == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: approach = None if previous is not None: diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index c2da9fb8c2..a8a3f8f155 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -372,6 +372,81 @@ def test_items_whose_param4_is_not_a_crosstrack_choice(self): m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)) +class TestLogMissionItems(object): + """what MAVExplorer hands the 3D map for the mission items in a log. + + The drawing tests below build their MissionItems by hand, so they say + nothing about whether anything fills them in: this covers the other end + """ + + def module(self): + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + import os + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + # MAVProxy/tools is not a package, so the tool is loaded by path + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + def resolve(self, mission, params=None): + return self.module().resolve_mission_amsl(mission, 584.0, params or {}) + + def item(self, seq, command, params, alt=100.0): + return (HERE[0], HERE[1] + seq * 0.01, alt, 3, command, seq, params) + + def test_the_turns_an_item_asks_for_are_carried_through(self): + from pymavlink import mavutil + m = mavutil.mavlink + out = self.resolve([ + self.item(0, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)), + self.item(1, m.MAV_CMD_NAV_LOITER_TURNS, (3, 0, 60, 0)), + self.item(2, mp_util.MAV_CMD_DO_ORBIT, (60, 5, 0, math.radians(270))), + self.item(3, m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 60, 0)), + ]) + assert out[1].circle_turns == 3 + assert out[2].circle_turns == pytest.approx(0.75) + # circling until its time is up is not a number of turns + assert out[3].circle_turns is None + + def test_a_loiter_to_alt_works_its_own_turns_out(self): + from pymavlink import mavutil + m = mavutil.mavlink + params = {'AIRSPEED_CRUISE': 20.0, 'TECS_CLMB_MAX': 5.0} + out = self.resolve([ + self.item(0, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0), alt=100.0), + self.item(1, m.MAV_CMD_NAV_LOITER_TO_ALT, (0, 60, 0, 0), + alt=700.0), + ], params) + # this one has no turn count of its own: what is left to climb on + # arrival decides it. 600m at 5m/s is 120s of climbing, of which + # the 900m approach does 45s, leaving four turns of a 60m circle + assert out[1].circle_turns == pytest.approx(4.0, abs=0.2) + + +class TestLiveMissionTurns(object): + """the turns the live map3d module hands the viewer""" + + def test_the_turns_an_item_asks_for_are_carried_through(self): + from pymavlink import mavutil + m = mavutil.mavlink + at = [mp_util.gps_newpos(HERE[0], HERE[1], 90, 400 * i) + for i in range(3)] + items = live_mission_items([ + waypoint(0, m.MAV_CMD_NAV_WAYPOINT, at[0][0], at[0][1], 100.0), + waypoint(1, m.MAV_CMD_NAV_LOITER_TURNS, at[1][0], at[1][1], + 300.0, params=(3, 0, 60, 0)), + waypoint(2, m.MAV_CMD_NAV_LOITER_TIME, at[2][0], at[2][1], + 300.0, params=(30, 0, 60, 0)), + ]) + assert items[1].circle_turns == 3 + # circling until its time is up is not a number of turns + assert items[2].circle_turns is None + + class TestLiveMissionAltitudes(object): """the map3d module measures the climb into a loiter itself, to work out how many turns it takes, so it has to resolve the frames first""" @@ -970,6 +1045,18 @@ def test_the_live_module(self): ], params=self.PARAMS) assert (items[1].exit_converge is not None) == pulled_back, command + def test_mavexplorer(self): + from pymavlink import mavutil + m = mavutil.mavlink + log = TestLogMissionItems() + for (command, params, pulled_back) in self.cases(): + items = log.resolve([ + log.item(0, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)), + log.item(1, command, params), + log.item(2, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)), + ], self.PARAMS) + assert (items[1].exit_converge is not None) == pulled_back, command + class TestPolygonBounds(object): From e6a78bc7a24cc93b49b3eea84e4f5bf22773f5a4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 12:37:25 +1000 Subject: [PATCH 06/22] mp_util: take loiter radii the way ArduPlane's update_loiter() does The MAVLink spec gives NAV_LOITER_TIME a radius in param3, but ArduPilot has nowhere to keep one: the loiter time takes all sixteen bits of the only spare field a location-carrying command has, so AP_Mission keeps param3 as the loiter_ccw bit alone and hands it back as -1 when the item asked for counter-clockwise and +1 otherwise. ArduPlane's verify_loiter_time() calls update_loiter(0) whatever the item was uploaded with, which flies WP_LOITER_RAD: counter-clockwise if the item asked for it, and otherwise whichever way WP_LOITER_RAD's own sign says. mission_circle_radius() read the +-1 as a radius, so a LOITER_TIME from a live vehicle or a log was drawn as a circle a metre across -- nothing anyone can see -- and one from a mission file, or read from an upload in a tlog, at a radius the vehicle never flies. Take the size from the vehicle's own radius for every LOITER_TIME, and follow its direction too: a negative param3 turns the circle counter-clockwise, and anything else leaves it going the vehicle's way. With no vehicle radius to take, nothing is drawn. update_loiter() treats a radius of a metre or less on the other loiter items the same way, and flies LOITER_RADIUS_DEFAULT, 60m, when WP_LOITER_RAD is itself a metre or less. Do both of those as well, rather than drawing a circle a metre across or none at all. Found flying a plane mission in SITL: the vehicle handed back p3=1.0 for a LOITER_TIME uploaded with none. --- MAVProxy/modules/lib/mp_util.py | 30 ++++++++++++-- tests/test_mission_geometry.py | 71 +++++++++++++++++++++++++++++++-- 2 files changed, 94 insertions(+), 7 deletions(-) diff --git a/MAVProxy/modules/lib/mp_util.py b/MAVProxy/modules/lib/mp_util.py index f843cfd2c9..feed97a9f6 100644 --- a/MAVProxy/modules/lib/mp_util.py +++ b/MAVProxy/modules/lib/mp_util.py @@ -290,10 +290,32 @@ def mission_circle_radius(command, params, default_radius=None, vehicle=None): # circles, so those are left alone return None radius = params[index] - if radius is None or math.isnan(radius) or radius == 0: - radius = default_radius - if radius is None or math.isnan(radius) or radius == 0: - return None + if command == MAV_CMD_DO_ORBIT: + if radius is None or math.isnan(radius) or radius == 0: + radius = default_radius + if radius is None or math.isnan(radius) or radius == 0: + return None + return radius + if radius is None or math.isnan(radius): + radius = 0.0 + if (command == mavlink.MAV_CMD_NAV_LOITER_TIME or abs(radius) <= 1): + # the vehicle's own radius. ArduPilot has nowhere to keep a radius + # for LOITER_TIME -- the loiter time takes all of the storage -- so + # it keeps only a bit saying the item asked for counter-clockwise, + # and hands that back as -1 (or +1 when it did not); whatever radius + # the item was uploaded with, ArduPlane's update_loiter(0) flies + # WP_LOITER_RAD. It takes any radius of a metre or less the same + # way, and flies counter-clockwise if the item asked for it, and + # otherwise the way WP_LOITER_RAD's sign says + if default_radius is None or math.isnan(default_radius): + return None + size = abs(default_radius) + if size <= 1: + # ArduPlane's LOITER_RADIUS_DEFAULT + size = 60.0 + if radius < 0 or default_radius < 0: + return -size + return size return radius diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index a8a3f8f155..48ccd187b0 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -307,7 +307,8 @@ def test_radius_parameter_per_command(self): cases = [ (m.MAV_CMD_NAV_LOITER_UNLIM, (0, 0, 70, 0), 70), (m.MAV_CMD_NAV_LOITER_TURNS, (3, 0, -55, 0), -55), - (m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0), 90), + # LOITER_TIME has no radius of its own: see below + (m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0), None), (m.MAV_CMD_NAV_LOITER_TO_ALT, (1, -65, 0, 0), -65), (mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, 0), 80), ] @@ -331,6 +332,70 @@ def test_unset_radius_uses_the_vehicle_default(self): assert mp_util.mission_circle_radius( m.MAV_CMD_NAV_LOITER_UNLIM, (0, 0, 0, 0)) is None + def test_a_loiter_time_radius_of_one_is_a_direction(self): + m = self.mavlink + # ArduPilot cannot store a radius for LOITER_TIME and hands back +-1 + # to say which way round it flies, so the vehicle's own radius is the + # size and param3 only picks the direction + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 1, 0), 60) == 60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -1, 0), 60) == -60 + # and one uploaded with a real radius is flown at the vehicle's all + # the same: ArduPlane's verify_loiter_time() calls update_loiter(0) + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -90, 0), 60) == -60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0), 60) == 60 + + def test_a_radius_of_a_metre_or_less_is_the_vehicles(self): + m = self.mavlink + # update_loiter() takes a radius of a metre or less as unset, for + # every loiter item, keeping the direction the item asked for + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TURNS): + assert mp_util.mission_circle_radius( + command, (1, 0, 1, 0), 80) == 80 + assert mp_util.mission_circle_radius( + command, (1, 0, -1, 0), 80) == -80 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TO_ALT, (0, -0.5, 0, 0), 80) == -80 + # and a radius which is really a radius is left alone + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TURNS, (1, 0, -90, 0), 80) == -90 + + def test_a_vehicle_radius_of_a_metre_or_less_is_arduplanes_default(self): + m = self.mavlink + # with WP_LOITER_RAD a metre or less, update_loiter() flies + # LOITER_RADIUS_DEFAULT, 60m, the way WP_LOITER_RAD's sign says + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TIME): + assert mp_util.mission_circle_radius( + command, (30, 0, 0, 0), 0) == 60 + assert mp_util.mission_circle_radius( + command, (30, 0, 0, 0), -1) == -60 + assert mp_util.mission_circle_radius( + command, (30, 0, -1, 0), 1) == -60 + + def test_a_loiter_time_goes_the_vehicles_way_unless_told_otherwise(self): + m = self.mavlink + # ArduPlane's update_loiter() flies counter-clockwise when the item + # asked for it, and otherwise the way WP_LOITER_RAD's sign says: so + # a vehicle whose own radius is negative circles counter-clockwise + # for an item handing back +1, or one uploaded with no radius at all + for param3 in (0, 1): + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, param3, 0), -60) == -60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -1, 0), -60) == -60 + # and a clockwise vehicle is only turned round by the item asking + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 0, 0), 60) == 60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -1, 0), 60) == -60 + # and with no vehicle radius to take, nothing is drawn rather than a + # circle a metre across + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 1, 0)) is None + def test_turn_counts(self): m = self.mavlink assert mp_util.mission_circle_turns( @@ -539,10 +604,10 @@ def test_a_hovering_vehicle_only_circles_for_some_items(self): (m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0)), (m.MAV_CMD_NAV_LOITER_TO_ALT, (1, -65, 0, 0))): assert mp_util.mission_circle_radius( - command, params, vehicle=m.MAV_TYPE_QUADROTOR) is None + command, params, 60, vehicle=m.MAV_TYPE_QUADROTOR) is None # a forward-flight vehicle circles for all of them assert mp_util.mission_circle_radius( - command, params, vehicle=m.MAV_TYPE_FIXED_WING) is not None + command, params, 60, vehicle=m.MAV_TYPE_FIXED_WING) is not None def test_unknown_vehicle_keeps_the_old_behaviour(self): m = self.mavlink From f5e8dd38a331e86d1d80c427523d809f9d6b9a73 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 12:37:55 +1000 Subject: [PATCH 07/22] MAVExplorer: draw only the last mission a log holds cmd_map3d appended every CMD message it found to the mission it drew. A log carries the whole mission once at its start and again every time one is uploaded, so a flight which had its mission changed drew every mission it had ever held, joined into one line by the legs between the end of one and the start of the next. Keep the items by sequence number instead, and start again at the logger's "New mission", which it writes before every mission it logs -- including a cleared one, which has no items after it at all. A shorter mission replacing a longer one therefore leaves nothing of the longer behind, and a cleared mission draws nothing. Logs from before the logger wrote the message still start again at a mission's first item. The reading moves out of cmd_map3d into mission_from_log(), so the tests can hand it a log. Found in a SITL plane log where the vehicle booted holding one mission and was then sent another: MAVExplorer drew 31 items, both missions end to end, for a flight of 16. --- MAVProxy/tools/MAVExplorer.py | 49 ++++++++++++++++++++--------- tests/test_mission_geometry.py | 56 ++++++++++++++++++++++++++++++++++ 2 files changed, 91 insertions(+), 14 deletions(-) diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index cbac712da1..766a697490 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -661,6 +661,40 @@ def cmd_map(args): map3d_views = [] +def mission_from_log(mlog, condition=None): + '''the flight path in a log, and the mission the log ends with. + + A log carries the whole mission again every time one is uploaded, and + once more when it starts, so the items are kept by sequence number and + the logger's "New mission" -- written before every one of them, even a + mission with no items at all -- starts them again. Drawing every mission + the flight ever held would join them into one line + ''' + path = [] + items = {} + while True: + m = mlog.recv_match(type=['POS', 'CMD', 'MSG'], condition=condition) + if m is None: + break + mtype = m.get_type() + if mtype == 'POS': + path.append((m.Lat, m.Lng, m.Alt, + grapher.timestamp_to_days(m._timestamp))) + elif mtype == 'MSG': + if m.Message == 'New mission': + items = {} + elif mtype == 'CMD': + if m.CNum == 0 and len(items) > 1: + # a log from before the logger said so: the first item of a + # mission still means whatever is held is stale + items = {} + if m.Lat != 0 or m.Lng != 0: + params = tuple(getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)) + items[m.CNum] = (m.Lat, m.Lng, m.Alt, getattr(m, 'Frame', 3), + m.CId, m.CNum, params) + return (path, [items[seq] for seq in sorted(items)]) + + def path_view(path): '''where a 3D map of a flight path looks from: the middle of the path, its lowest altitude and how far across it is, as (lat, lon, amsl, span). @@ -710,20 +744,7 @@ def cmd_map3d(args): return mlog = mestate.mlog - path = [] - mission = [] - while True: - m = mlog.recv_match(type=['POS', 'CMD'], condition=mestate.settings.condition) - if m is None: - break - mtype = m.get_type() - if mtype == 'POS': - path.append((m.Lat, m.Lng, m.Alt, - grapher.timestamp_to_days(m._timestamp))) - elif mtype == 'CMD' and (m.Lat != 0 or m.Lng != 0): - params = tuple(getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)) - mission.append((m.Lat, m.Lng, m.Alt, getattr(m, 'Frame', 3), - m.CId, m.CNum, params)) + (path, mission) = mission_from_log(mlog, mestate.settings.condition) mlog.rewind() if len(path) == 0: diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 48ccd187b0..803ac1619c 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -491,6 +491,62 @@ def test_a_loiter_to_alt_works_its_own_turns_out(self): # the 900m approach does 45s, leaving four turns of a 60m circle assert out[1].circle_turns == pytest.approx(4.0, abs=0.2) + def log(self, *messages): + '''a log which hands back these messages in order''' + class Log(object): + def __init__(self, messages): + self.messages = list(messages) + + def recv_match(self, type=None, condition=None): + while self.messages: + m = self.messages.pop(0) + if type is None or m.get_type() in type: + return m + return None + return Log(messages) + + def message(self, kind, **fields): + from types import SimpleNamespace + m = SimpleNamespace(_timestamp=0, **fields) + m.get_type = lambda: kind + return m + + def dump(self, where, count, command=16): + '''what the logger writes for a mission of count items at where''' + out = [self.message('MSG', Message='New mission')] + for seq in range(count): + p = mp_util.gps_newpos(where[0], where[1], 90, 100 * seq) + out.append(self.message('CMD', CNum=seq, CId=command, Lat=p[0], + Lng=p[1], Alt=100.0, Frame=3, Prm1=0, + Prm2=0, Prm3=0, Prm4=0)) + return out + + def test_a_log_draws_the_last_mission_it_holds(self): + first = HERE + second = mp_util.gps_newpos(HERE[0], HERE[1], 0, 5000) + (path, mission) = self.module().mission_from_log( + self.log(*(self.dump(first, 4) + self.dump(second, 2)))) + # just the second mission, even though it is the shorter of the two + assert len(mission) == 2 + assert mission[0][0] == pytest.approx(second[0]) + + def test_a_cleared_mission_draws_nothing(self): + # clearing the mission writes the message and then no items at all + (path, mission) = self.module().mission_from_log( + self.log(*(self.dump(HERE, 4) + + [self.message('MSG', Message='New mission')]))) + assert mission == [] + + def test_a_log_from_before_the_logger_said_new_mission(self): + # without the message, a mission's first item still starts it again + first = [m for m in self.dump(HERE, 4) if m.get_type() == 'CMD'] + second = [m for m in self.dump( + mp_util.gps_newpos(HERE[0], HERE[1], 0, 5000), 2) + if m.get_type() == 'CMD'] + (path, mission) = self.module().mission_from_log( + self.log(*(first + second))) + assert len(mission) == 2 + class TestLiveMissionTurns(object): """the turns the live map3d module hands the viewer""" From 4675a6b4303406e9bdfe76d0b8fb7b17961913e6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 12:38:52 +1000 Subject: [PATCH 08/22] MAVExplorer: draw a takeoff which has no position of its own A takeoff item normally carries an altitude and no position: the vehicle climbs from wherever it is when the item runs. The live 3D map draws one as the climb from home, but cmd_map3d dropped every CMD without a position, so from a log a mission started at its first waypoint and the line ran diagonally up to it instead of showing the climb. A log can say where the climb really began. The autopilot announces "Mission: " as it starts each item, so the takeoff is drawn from where the vehicle was at that moment. A mission never flown far enough to get there falls back on home, the mission's own first item, which ArduPilot sets where it arms; and a mission uploaded before the vehicle had a home leaves that item empty too -- as happens in SITL -- so where the flight itself began stands in for it then. Anything else without a position still has nowhere to be drawn and is left out. TAKEOFF_COMMANDS moves to mp_util, so the module and MAVExplorer read it from the one place. Found flying a copter mission with a positionless takeoff in SITL. --- MAVProxy/modules/lib/mp_util.py | 6 ++ MAVProxy/modules/mavproxy_map3d/__init__.py | 5 +- MAVProxy/tools/MAVExplorer.py | 52 ++++++++++++-- tests/test_mission_geometry.py | 78 +++++++++++++++++++++ 4 files changed, 132 insertions(+), 9 deletions(-) diff --git a/MAVProxy/modules/lib/mp_util.py b/MAVProxy/modules/lib/mp_util.py index feed97a9f6..35de9f807a 100644 --- a/MAVProxy/modules/lib/mp_util.py +++ b/MAVProxy/modules/lib/mp_util.py @@ -193,6 +193,12 @@ def arc_points(latlon1, latlon2, arc_angle, steps=None): return points +# takeoff items carry an altitude and often no position of their own: the +# vehicle climbs from where it already is, which for a mission is home. +# Numeric because mp_util does not import pymavlink at module scope +TAKEOFF_COMMANDS = (22, 84) # NAV_TAKEOFF, NAV_VTOL_TAKEOFF + + # vehicles which hold position at a loiter point rather than flying a circle # around it. MAVProxy's own vehicle type names are accepted as well as the # MAV_TYPEs, since that is what a live module has to hand diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index c3bd5aa979..baf8119bca 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -27,9 +27,6 @@ FENCE_RETURN_BGR = (255, 127, 127) FENCE_RETURN_RADIUS = 10.0 -# takeoff items normally carry an altitude and no position -TAKEOFF_COMMANDS = (22, 84) # NAV_TAKEOFF, NAV_VTOL_TAKEOFF - def bgr_to_rgb(bgr, default=(1.0, 0.0, 1.0)): '''SlipMap/OpenCV colours are BGR; VTK wants RGB floats''' @@ -309,7 +306,7 @@ def send_mission(self): for w in wploader.wpoints: frame = getattr(w, 'frame', 0) (lat, lon) = (w.x, w.y) - if lat == 0 and lon == 0 and w.command in TAKEOFF_COMMANDS: + if lat == 0 and lon == 0 and w.command in mp_util.TAKEOFF_COMMANDS: # draw the climb from home, otherwise the takeoff altitude is # dropped and the mission appears to start at the first waypoint home = self.mission_home(wploader) diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index 766a697490..dd9f11ff26 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -661,6 +661,37 @@ def cmd_map(args): map3d_views = [] +def mission_items_from_cmds(items, started_at=None, flown_from=None): + '''the mission items to draw, from a log's CMD entries keyed by sequence. + + An item with no position of its own is a takeoff, which climbs from + wherever the vehicle is when it runs. flown_from maps an item's + sequence to where the vehicle was when the log says the item began, and + that is used when there is one. Otherwise the best guess is home, the + mission's own first item, since ArduPilot sets home where it arms; and a + mission uploaded before the vehicle had a home leaves that item empty + too, so started_at -- where the flight itself began -- stands in for it. + Anything else without a position is not something the map can place, + and is left out + ''' + flown_from = flown_from or {} + home = items.get(0) + if home is not None and (home[0] != 0 or home[1] != 0): + started_at = (home[0], home[1]) + mission = [] + for seq in sorted(items): + item = items[seq] + if item[0] == 0 and item[1] == 0: + if item[4] not in mp_util.TAKEOFF_COMMANDS: + continue + where = flown_from.get(seq, started_at) + if where is None: + continue + item = (where[0], where[1]) + item[2:] + mission.append(item) + return mission + + def mission_from_log(mlog, condition=None): '''the flight path in a log, and the mission the log ends with. @@ -672,6 +703,9 @@ def mission_from_log(mlog, condition=None): ''' path = [] items = {} + # where the vehicle was when each item began, from the "Mission: " + # the autopilot announces as it starts one + flown_from = {} while True: m = mlog.recv_match(type=['POS', 'CMD', 'MSG'], condition=condition) if m is None: @@ -681,18 +715,26 @@ def mission_from_log(mlog, condition=None): path.append((m.Lat, m.Lng, m.Alt, grapher.timestamp_to_days(m._timestamp))) elif mtype == 'MSG': + fields = m.Message.split() if m.Message == 'New mission': items = {} + flown_from = {} + elif (len(fields) > 1 and fields[0] == 'Mission:' and + fields[1].isdigit() and path): + flown_from.setdefault(int(fields[1]), (path[-1][0], path[-1][1])) elif mtype == 'CMD': if m.CNum == 0 and len(items) > 1: # a log from before the logger said so: the first item of a # mission still means whatever is held is stale items = {} - if m.Lat != 0 or m.Lng != 0: - params = tuple(getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)) - items[m.CNum] = (m.Lat, m.Lng, m.Alt, getattr(m, 'Frame', 3), - m.CId, m.CNum, params) - return (path, [items[seq] for seq in sorted(items)]) + flown_from = {} + params = tuple(getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)) + items[m.CNum] = (m.Lat, m.Lng, m.Alt, getattr(m, 'Frame', 3), + m.CId, m.CNum, params) + mission = mission_items_from_cmds( + items, started_at=(path[0][0], path[0][1]) if path else None, + flown_from=flown_from) + return (path, mission) def path_view(path): diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 803ac1619c..450ab9f1a4 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -463,6 +463,63 @@ def resolve(self, mission, params=None): def item(self, seq, command, params, alt=100.0): return (HERE[0], HERE[1] + seq * 0.01, alt, 3, command, seq, params) + def test_a_takeoff_with_no_position_climbs_from_home(self): + from pymavlink import mavutil + m = mavutil.mavlink.MAV_CMD_NAV_TAKEOFF + home = (HERE[0], HERE[1], 584.0, 0, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)) + takeoff = (0.0, 0.0, 30.0, 3, m, 1, (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds({0: home, 1: takeoff}) + assert len(out) == 2 + # the takeoff is drawn as the climb from home it is + assert out[1][0] == HERE[0] + assert out[1][1] == HERE[1] + assert out[1][2] == 30.0 + assert out[1][4] == m + + def test_an_item_with_nowhere_to_go_is_left_out(self): + from pymavlink import mavutil + home = (HERE[0], HERE[1], 584.0, 0, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)) + # a jump carries no position and is not a takeoff, so it is not drawn + jump = (0.0, 0.0, 0.0, 3, mavutil.mavlink.MAV_CMD_DO_JUMP, 1, + (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds({0: home, 1: jump}) + assert [i[5] for i in out] == [0] + # and with nowhere to climb from, nor is a takeoff + takeoff = (0.0, 0.0, 30.0, 3, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 1, + (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds({1: takeoff}) + assert out == [] + + def test_a_mission_uploaded_before_home_was_known(self): + from pymavlink import mavutil + # the autopilot keeps its own home in item 0, so a mission uploaded + # before it had one leaves that item empty as well + home = (0.0, 0.0, 0.0, 0, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, + (0, 0, 0, 0)) + takeoff = (0.0, 0.0, 30.0, 3, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 1, + (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds( + {0: home, 1: takeoff}, started_at=HERE) + # the empty home is not drawn, but the takeoff climbs from where the + # flight started + assert [i[5] for i in out] == [1] + assert (out[0][0], out[0][1]) == HERE + + def test_a_takeoff_climbs_from_where_it_was_flown(self): + from pymavlink import mavutil + # a vehicle need not take off from the home a mission was uploaded + # with; where the log says the takeoff began wins over home + home = (HERE[0], HERE[1], 584.0, 0, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)) + takeoff = (0.0, 0.0, 30.0, 3, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 1, + (0, 0, 0, 0)) + elsewhere = mp_util.gps_newpos(HERE[0], HERE[1], 45, 2000) + out = self.module().mission_items_from_cmds( + {0: home, 1: takeoff}, flown_from={1: elsewhere}) + assert (out[1][0], out[1][1]) == elsewhere + def test_the_turns_an_item_asks_for_are_carried_through(self): from pymavlink import mavutil m = mavutil.mavlink @@ -537,6 +594,27 @@ def test_a_cleared_mission_draws_nothing(self): [self.message('MSG', Message='New mission')]))) assert mission == [] + def test_the_log_says_where_a_takeoff_began(self): + from pymavlink import mavutil + m = mavutil.mavlink + moved = mp_util.gps_newpos(HERE[0], HERE[1], 45, 2000) + + def cmd(seq, command, lat, lng, alt): + return self.message('CMD', CNum=seq, CId=command, Lat=lat, + Lng=lng, Alt=alt, Frame=3, Prm1=0, Prm2=0, + Prm3=0, Prm4=0) + (path, mission) = self.module().mission_from_log(self.log( + self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), + self.message('MSG', Message='New mission'), + cmd(0, m.MAV_CMD_NAV_WAYPOINT, HERE[0], HERE[1], 584.0), + cmd(1, m.MAV_CMD_NAV_TAKEOFF, 0.0, 0.0, 30.0), + # the vehicle is carried somewhere else before it flies + self.message('POS', Lat=moved[0], Lng=moved[1], Alt=600.0), + self.message('MSG', Message='Mission: 1 Takeoff'), + self.message('POS', Lat=moved[0], Lng=moved[1], Alt=630.0), + )) + assert (mission[1][0], mission[1][1]) == moved + def test_a_log_from_before_the_logger_said_new_mission(self): # without the message, a mission's first item still starts it again first = [m for m in self.dump(HERE, 4) if m.get_type() == 'CMD'] From c7454c200a02f2bf8c14e7c87c9b3efaeaed40ef Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 12:39:26 +1000 Subject: [PATCH 09/22] map: draw loiter circles and direction arrows by default The 2D map's loitercircle and showdirection settings were both off, so a mission with loiters in it was drawn as a line through their centres with nothing to say the vehicle circles there, and nothing to say which way round the mission runs. mavflightview and MAVExplorer draw the circles whatever the settings say, so the same mission looked different depending on which one it was opened in. Now that the circles are the ones the vehicle flies, turn both on, and turn on the 3D map's direction arrows in the map3d module and MAVExplorer with them, so the live maps and MAVExplorer's 3D map show a mission's circles and its direction alike. mavflightview has no direction setting and still draws its mission without arrows. "map set loitercircle 0", "map set showdirection 0" and their map3d and MAVExplorer equivalents put them back. --- MAVProxy/modules/mavproxy_map/__init__.py | 4 ++-- MAVProxy/modules/mavproxy_map3d/__init__.py | 2 +- MAVProxy/tools/MAVExplorer.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 8a42a23fe6..3832f98c00 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -69,11 +69,11 @@ def __init__(self, mpstate): ('showahrs3pos', int, 0), ('brightness', float, 1), ('rallycircle', bool, False), - ('loitercircle', bool, False), + ('loitercircle', bool, True), ('showclicktime', int, 2), ('showwpnum', bool, True), ('circle_linewidth', int, 1), - ('showdirection', bool, False), + ('showdirection', bool, True), ('setpos_accuracy', float, 50), ('mission_color', str, "white"), ('font_size', float, 0.5), diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index baf8119bca..f9db04fd68 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -52,7 +52,7 @@ def __init__(self, mpstate): range=(0.25, 2.0)), ('terrainshading', bool, True), ('terrainwireframe', bool, False), - ('showdirection', bool, False), + ('showdirection', bool, True), ]) self.add_command('map3d', self.cmd_map3d, "3D map control", ['', diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index dd9f11ff26..7a1f60b838 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -120,7 +120,7 @@ def __init__(self): MPSetting('paramdocs', bool, True, 'show param docs'), MPSetting('max_rate', float, 0, 'maximum display rate of graphs in Hz'), MPSetting('vehicle_type', str, 'Auto', 'force vehicle type for mode handling'), - MPSetting('showdirection', bool, False, + MPSetting('showdirection', bool, True, 'show direction of travel on the 3D map mission'), ] ) From a5544b639e3824a3f7a0880165f6548a8ec7a049 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 3 Oct 2026 12:29:39 +1000 Subject: [PATCH 10/22] tests: add missions which exercise the loiter drawing plane-mission-geometry.txt and copter-mission-geometry.txt are SITL-flyable missions which exercise the drawing this fixes: climbing and descending spirals, crossing and non-crossing tangents between loiters, both param4 settings, mixed altitude frames, the arc waypoints only a multirotor flies, and the loiters it holds position for. README.md describes each one and what to look for. Co-Authored-By: Claude Opus 5 (1M context) --- tests/missions/README.md | 59 ++++++++++++++++++ tests/missions/copter-mission-geometry.txt | 27 ++++++++ tests/missions/plane-mission-geometry.txt | 33 ++++++++++ tests/test_mission_geometry.py | 71 ++++++++++++++++++++++ 4 files changed, 190 insertions(+) create mode 100644 tests/missions/README.md create mode 100644 tests/missions/copter-mission-geometry.txt create mode 100644 tests/missions/plane-mission-geometry.txt diff --git a/tests/missions/README.md b/tests/missions/README.md new file mode 100644 index 0000000000..4b324dbdc1 --- /dev/null +++ b/tests/missions/README.md @@ -0,0 +1,59 @@ +# missions for checking the map drawing + +Missions built to put every kind of item the maps draw specially in front +of the drawing code at once, so a SITL flight can be compared against what +was drawn: the live 2D and 3D maps while it flies, and mavflightview or +MAVExplorer's `map3d` over the dataflash log afterwards. + +Each item carries a comment saying what it is there for. They are laid out +around SITL's default CMAC home, `-35.363261 149.165230`, and every one of +them can be flown by the vehicle it is named for. + + wp load tests/missions/plane-mission-geometry.txt + +## plane-mission-geometry.txt + +Sixteen items covering the loiter drawing: + +- a loiter to altitude climbing 400m over a short approach, drawn as a + spiral of about two turns, and one descending 200m, drawn as about three +- three turns counter-clockwise and two clockwise, close enough together + that the leg between them is the tangent which crosses between the two + circles, and a loiter for time after them turning the same way, whose leg + is the tangent which does not cross +- a loiter with no radius of its own, which takes `WP_LOITER_RAD` +- param4 both ways: 0 leaves the next leg crosstracked from the loiter + centre, so the drawing pulls the vehicle back onto that track after it + leaves the circle; 1 crosstracks from the exit, and no pull-back is drawn +- AMSL, home-relative and terrain-frame altitudes, and five changes of + frame between one item and the next +- a loiter forever at the end. The mission parks there: switch out of AUTO + once there is nothing left to watch + +ArduPlane has no `NAV_ARC_WAYPOINT`, so the arc waypoints are in the copter +mission instead. Nothing here flies `MAV_CMD_DO_ORBIT` either: the maps +draw its circle, but no ArduPilot vehicle implements the command, so it +cannot go in a mission which is meant to be flown. + +## copter-mission-geometry.txt + +Thirteen items over a few hundred metres, covering what a multirotor draws +differently: + +- arc waypoints, one sweeping 90 degrees clockwise and one 270 + counter-clockwise. The second bulges a long way outside its chord, which + is what the 2D map has to bound it over to keep it on screen +- loiter turns both ways round, near enough that the leg between them is + the tangent which crosses between the two circles +- the loiters a multirotor holds position for instead of circling -- + unlimited, for time, and to altitude -- which are drawn as the points + they are even where the item carries a radius +- turns with no radius of its own: a multirotor has no `WP_LOITER_RAD` to + fall back on, so nothing is drawn (the vehicle flies its + `CIRCLE_RADIUS_M`, or `CIRCLE_RADIUS` in centimetres on older firmware) +- a takeoff carrying no position, drawn as the climb from where it was + flown +- a multirotor has no L1 controller, so nothing is drawn pulling it back + onto a track out of a loiter centre + +As with the plane mission, it parks at a loiter forever at the end. diff --git a/tests/missions/copter-mission-geometry.txt b/tests/missions/copter-mission-geometry.txt new file mode 100644 index 0000000000..a8c1a93aa5 --- /dev/null +++ b/tests/missions/copter-mission-geometry.txt @@ -0,0 +1,27 @@ +QGC WPL 110 +# a copter mission for the map's arcs, loiters and spirals +0 1 0 16 0 0 0 0 -35.3632610 149.1652300 584.000000 1 +# straight up. This one carries no position of its own, which the map draws as the climb from where it is flown +1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.0000000 0.0000000 30.000000 1 +# a plain leg, to give the first arc something to leave from +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3619135 149.1652300 30.000000 1 +# arc waypoint, 90 degrees clockwise (positive sweep) +3 0 3 36 90.000000 0.000000 0.000000 0.000000 -35.3619135 149.1674331 30.000000 1 +# arc waypoint sweeping more than half a turn counter-clockwise: the arc bulges a long way outside its chord +4 0 3 36 -270.000000 0.000000 0.000000 0.000000 -35.3635015 149.1693804 40.000000 1 +# two turns, 50m clockwise. A multirotor does fly this one as a circle, so the map draws it +5 0 3 18 2.000000 0.000000 50.000000 0.000000 -35.3654072 149.1670436 40.000000 1 +# one turn the other way round, near enough that the leg between the two circles is the tangent which crosses between them +6 0 3 18 1.000000 0.000000 -50.000000 1.000000 -35.3638191 149.1650962 40.000000 1 +# turns with no radius of its own. A multirotor has no WP_LOITER_RAD to fall back on, so there is no circle to draw: the item is drawn as the point it is (the vehicle flies its CIRCLE_RADIUS_M, or CIRCLE_RADIUS in centimetres on older firmware) +7 0 3 18 1.000000 0.000000 0.000000 0.000000 -35.3621676 149.1671214 40.000000 1 +# a multirotor holds position for LOITER_TIME, so no circle is drawn here either. It carries no radius: the autopilot has nowhere to keep one for this command, and hands back 0 whatever is uploaded +8 0 3 19 20.000000 0.000000 0.000000 0.000000 -35.3628435 149.1693987 40.000000 1 +# a multirotor climbs straight up for LOITER_TO_ALT rather than spiralling, so again no circle +9 0 3 31 0.000000 40.000000 0.000000 0.000000 -35.3649995 149.1693987 70.000000 1 +# an AMSL altitude, to mix the frames +10 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.3656754 149.1671213 654.000000 1 +# a terrain-frame altitude, 50m above the ground +11 0 10 16 0.000000 0.000000 0.000000 0.000000 -35.3642991 149.1657052 50.000000 1 +# hold here forever. A multirotor sits on the point, so no circle. The mission parks here: switch out of AUTO when done +12 0 3 17 0.000000 0.000000 40.000000 0.000000 -35.3627797 149.1663833 50.000000 1 diff --git a/tests/missions/plane-mission-geometry.txt b/tests/missions/plane-mission-geometry.txt new file mode 100644 index 0000000000..88ee54b50e --- /dev/null +++ b/tests/missions/plane-mission-geometry.txt @@ -0,0 +1,33 @@ +QGC WPL 110 +# a plane mission for the map's loiters, spirals and tangents +0 1 0 16 0 0 0 0 -35.3632610 149.1652300 584.000000 1 +# climb out to the north +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.3597223 149.1659951 100.000000 1 +# a plain leg, to settle onto a track before anything interesting +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3491062 149.1682903 120.000000 1 +# plain legs out to the west. ArduPlane has no NAV_ARC_WAYPOINT, so the arcs are in the copter mission instead +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3477023 149.1780519 120.000000 1 +# turning back south +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3552996 149.1814422 120.000000 1 +# and out towards the loiters +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3583721 149.1917926 130.000000 1 +# switch to an AMSL altitude: the climb into the next loiter is measured across the frame change +6 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.3529821 149.2032393 750.000000 1 +# loiter to altitude, 100m clockwise, climbing 400m over a short approach: most of that is left to do on arrival, so it is drawn as a spiral of about two turns. param4 0 leaves the next leg crosstracked from this centre, so the vehicle pulls back onto it +7 0 0 31 0.000000 100.000000 0.000000 0.000000 -35.3483143 149.2065435 1150.000000 1 +# a long leg, so there is room to see the vehicle rejoin the track out of the loiter centre +8 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.3288651 149.1927782 1150.000000 1 +# three turns, 100m counter-clockwise (negative radius). param4 1 crosstracks the next leg from where the circle is left instead +9 0 0 18 3.000000 0.000000 -100.000000 1.000000 -35.3288651 149.1762618 1150.000000 1 +# two turns the other way round, close enough that the leg between the two circles is the tangent which crosses between them +10 0 0 18 2.000000 0.000000 100.000000 0.000000 -35.3350889 149.1718573 1150.000000 1 +# loiter for 45s with no radius of its own, so it takes WP_LOITER_RAD, clockwise. The autopilot has nowhere to keep a radius for this command and hands back +-1 for the direction alone. Turning the same way as the loiter before it, the leg between them is the tangent which does not cross. Back to a home-relative altitude, and a descent +11 0 3 19 45.000000 0.000000 0.000000 0.000000 -35.3413126 149.1674524 300.000000 1 +# a terrain-frame altitude, 300m above the ground +12 0 10 16 0.000000 0.000000 0.000000 0.000000 -35.3391287 149.1522692 300.000000 1 +# loiter to altitude descending, 100m counter-clockwise: a plane comes down at TECS_SINK_MIN, so 200m of descent is several turns of spiral. param4 1, so no pull-back onto a track from here +13 0 3 31 0.000000 -100.000000 0.000000 1.000000 -35.3362416 149.1480513 90.000000 1 +# a plain leg out of the descending spiral +14 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3286443 149.1446618 90.000000 1 +# circle here forever, 80m clockwise. The mission parks here: switch out of AUTO when there is nothing left to look at +15 0 3 17 0.000000 0.000000 80.000000 0.000000 -35.3232543 149.1446618 90.000000 1 diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 450ab9f1a4..deff24f70d 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -1281,3 +1281,74 @@ def test_orbit_geometry_without_enum(monkeypatch): monkeypatch.delattr(mavutil.mavlink, 'MAV_CMD_DO_ORBIT', raising=False) assert mp_util.mission_circle_radius(34, (80, 5, 0, 0)) == 80 assert mp_util.mission_circle_turns(34, (80, 5, 0, math.pi)) == pytest.approx(.5) + + +class TestMissionFiles(object): + """the missions under tests/missions are flown by hand in SITL, so check + here that they still load and are still the missions their README says""" + + def load(self, name): + '''(seq, frame, command, params, lat, lon, alt) for each item''' + import os + path = os.path.join(os.path.dirname(__file__), 'missions', name) + with open(path) as f: + lines = f.read().splitlines() + assert lines[0] == 'QGC WPL 110' + items = [] + for line in lines[1:]: + if not line.strip() or line.startswith('#'): + continue + fields = line.split() + assert len(fields) == 12, line + items.append((int(fields[0]), int(fields[2]), int(fields[3]), + tuple(float(v) for v in fields[4:8]), + float(fields[8]), float(fields[9]), + float(fields[10]))) + assert [i[0] for i in items] == list(range(len(items))) + return items + + def test_the_plane_mission(self): + from pymavlink import mavutil + m = mavutil.mavlink + items = self.load('plane-mission-geometry.txt') + assert len(items) == 16 + commands = set(i[2] for i in items) + # ArduPlane flies neither of these, so a flyable mission has neither + assert mp_util.MAV_CMD_NAV_ARC_WAYPOINT not in commands + assert mp_util.MAV_CMD_DO_ORBIT not in commands + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TURNS, + m.MAV_CMD_NAV_LOITER_TIME, m.MAV_CMD_NAV_LOITER_TO_ALT): + assert command in commands + # both ways round, and param4 both ways + radii = [mp_util.mission_circle_radius(i[2], i[3], 60) for i in items] + assert any(r is not None and r > 0 for r in radii) + assert any(r is not None and r < 0 for r in radii) + xtrack = [mp_util.mission_crosstracks_from_centre(i[2], i[3]) + for i in items if i[2] in (m.MAV_CMD_NAV_LOITER_TURNS, + m.MAV_CMD_NAV_LOITER_TO_ALT)] + assert True in xtrack and False in xtrack + # AMSL, home-relative and terrain altitudes + assert set(i[1] for i in items) >= {0, 3, 10} + + def test_the_copter_mission(self): + from pymavlink import mavutil + m = mavutil.mavlink + items = self.load('copter-mission-geometry.txt') + assert len(items) == 13 + commands = [i[2] for i in items] + # ArduCopter flies arc waypoints, and no vehicle flies DO_ORBIT + sweeps = [i[3][0] for i in items if i[2] == mp_util.MAV_CMD_NAV_ARC_WAYPOINT] + assert any(s > 0 for s in sweeps) + assert any(s < -180 for s in sweeps) + assert mp_util.MAV_CMD_DO_ORBIT not in commands + # a takeoff with no position of its own + takeoff = items[commands.index(m.MAV_CMD_NAV_TAKEOFF)] + assert (takeoff[4], takeoff[5]) == (0.0, 0.0) + # circles only where a multirotor flies them, and only with a radius + drawn = [i[0] for i in items + if mp_util.mission_circle_radius(i[2], i[3], None, + 'copter') is not None] + assert [commands[s] for s in drawn] == [m.MAV_CMD_NAV_LOITER_TURNS] * 2 + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TIME, + m.MAV_CMD_NAV_LOITER_TO_ALT): + assert command in commands From 7825b04063e684f3c54c936a6048bf9e64288171 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 3 Oct 2026 12:29:39 +1000 Subject: [PATCH 11/22] mavflightview: read the mission out of telemetry logs again A telemetry log carries the missions downloaded from the vehicle or uploaded to it, each a MISSION_COUNT and then its items, in whatever order they were asked for and perhaps never all of them; a dataflash log carries a CMD for each item whenever the vehicle writes the whole mission out. LogMission, in the lib so that any of the tools can read a log's mission with it, reads either and keeps only the last mission to arrive whole: the map draws the mission the vehicle was flying rather than every one the log holds joined end to end, a transfer the log ends part way through leaves the one before it as it was, and a mission cleared away is cleared. MISSION_CLEAR_ALL, the float items a ground station may upload instead of MISSION_ITEM_INT, and the frame a dataflash CMD logs are taken with it. WP_LOITER_RAD, which the loiter circles are drawn at, is looked up once the log has been read, since a telemetry log carries the parameters as it goes. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/modules/lib/log_mission.py | 156 ++++++++++++ MAVProxy/tools/mavflightview.py | 148 +++++------ tests/conftest.py | 20 ++ tests/test_mavflightview.py | 365 ++++++++++++++++++++++++++++ 4 files changed, 607 insertions(+), 82 deletions(-) create mode 100644 MAVProxy/modules/lib/log_mission.py create mode 100644 tests/conftest.py create mode 100644 tests/test_mavflightview.py diff --git a/MAVProxy/modules/lib/log_mission.py b/MAVProxy/modules/lib/log_mission.py new file mode 100644 index 0000000000..8fce9ab617 --- /dev/null +++ b/MAVProxy/modules/lib/log_mission.py @@ -0,0 +1,156 @@ +'''the mission a log holds, as it was downloaded, uploaded or written out + +AP_FLAKE8_CLEAN +''' + +from pymavlink import mavutil + + +# the messages a log carries its mission in +MISSION_TYPES = ['MISSION_COUNT', 'MISSION_CLEAR_ALL', + 'MISSION_ITEM', 'MISSION_ITEM_INT', 'CMD', 'MSG'] +# a MAVLink1 message has no mission_type, and is only ever about the mission +DEFAULT_MISSION_TYPE = mavutil.mavlink.MAV_MISSION_TYPE_MISSION + + +class LogMission(object): + '''the last whole mission a log holds. A telemetry log carries the + missions downloaded from or uploaded to the vehicle, each a MISSION_COUNT + and then its items, in whatever order they were asked for, and perhaps + never all of them. A dataflash log carries a CMD for each item of the + mission whenever the vehicle writes the whole mission out: at the start + of the log, and after it changes. + + mission_type is which of the tables the mission protocol carries to + keep: the mission itself, or the rally points or fence, which travel in + the same messages. Only the mission is written out as CMD, so a table + of another kind is whatever a telemetry log carries of it''' + + def __init__(self, mission_type=mavutil.mavlink.MAV_MISSION_TYPE_MISSION): + self.mission_type = mission_type + # seq -> MISSION_ITEM, for the last mission to arrive whole + self.items = {} + # (count, {seq: MISSION_ITEM}) for one arriving, while it does. The + # count is None where the log does not say how many items to expect + self.transfer = None + # how many times a table has arrived whole, or been cleared away: + # the vehicle has said what it holds, even where that is the table + # held already + self.arrivals = 0 + + def start(self, count): + '''a mission of count items starts to arrive''' + if self.transfer is not None and self.transfer[0] is None: + # with no count to finish it, the last one ended where this starts + self.items = self.transfer[1] + self.arrivals += 1 + self.transfer = (count, {}) + if count == 0: + self.clear() + + def clear(self): + self.items = {} + self.transfer = None + self.arrivals += 1 + + def add(self, item): + if self.transfer is None: + # an item which is not part of a mission seen to start arriving: + # a log which starts part way through, or a single item changed + self.items[item.seq] = item + self.arrivals += 1 + return + (count, items) = self.transfer + if count is not None and item.seq >= count: + return + items[item.seq] = item + if count is not None and len(items) == count: + self.items = items + self.transfer = None + self.arrivals += 1 + + def read(self, m, type=None): + '''take in one of the MISSION_TYPES messages. type is what to take + it as, for a caller which has made one message stand in for another + -- a telemetry log's STATUSTEXT for a dataflash MSG, say''' + if type is None: + type = m.get_type() + if self.mission_type != mavutil.mavlink.MAV_MISSION_TYPE_MISSION \ + and type in ('MSG', 'CMD'): + # a dataflash log writes out the mission this way, and nothing + # else the mission protocol carries + return + if type == 'MSG': + if getattr(m, 'Message', None) == 'New mission': + # the logger says so before it writes the mission out, and + # a mission cleared away is written out as nothing more + self.start(0) + return + if type == 'CMD': + if m.CNum == 0: + # the vehicle writes out the whole mission from home onwards + self.start(getattr(m, 'CTot', None)) + # a log from before 2014 carries neither the frame nor the + # parameters of an item + params = [getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)] + self.add(mavutil.mavlink.MAVLink_mission_item_message( + 0, 0, m.CNum, + getattr(m, 'Frame', + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT), + m.CId, 0, 1, params[0], params[1], params[2], params[3], + m.Lat, m.Lng, m.Alt)) + return + # fence and rally points travel in the same messages + mission_type = getattr(m, 'mission_type', DEFAULT_MISSION_TYPE) + if type == 'MISSION_CLEAR_ALL': + if mission_type in (self.mission_type, + mavutil.mavlink.MAV_MISSION_TYPE_ALL): + self.clear() + return + if mission_type != self.mission_type: + return + if type == 'MISSION_COUNT': + self.start(m.count) + elif type == 'MISSION_ITEM_INT': + self.add(mavutil.mavlink.MAVLink_mission_item_message( + 0, 0, m.seq, m.frame, m.command, m.current, m.autocontinue, + m.param1, m.param2, m.param3, m.param4, + m.x / 1.0e7, m.y / 1.0e7, m.z)) + else: + self.add(m) + + def whole(self): + '''the mission by sequence, from its first item as far as it runs + without a gap. Nothing is made up to stand in for an item never + seen, and a mission which never arrived whole is not one: a log + which starts part way through one carries no mission at all, since + there is no flying, numbering or jumping through a mission whose + first items are missing''' + items = self.items + if self.transfer is not None: + (count, arriving) = self.transfer + if count is None or not items: + # the last one ran to the end of the log, or it never arrived + # whole and nothing before it did + items = arriving + out = {} + seq = 0 + while seq in items: + out[seq] = items[seq] + seq += 1 + return out + + def partial(self): + '''whether what whole() gives is less than what arrived whole: the + log ends part way through the only transfer it carries. What did + arrive is where the vehicle was told to go, and is worth drawing, + but not worth flying: what would have followed it is not known''' + if self.transfer is None: + return False + (count, arriving) = self.transfer + return count is not None and not self.items and len(arriving) < count + + def fill(self, wp): + '''add the mission to the MAVWPLoader wp''' + for (_, item) in sorted(self.whole().items()): + wp.add(item) diff --git a/MAVProxy/tools/mavflightview.py b/MAVProxy/tools/mavflightview.py index f164e37461..8f4cd6c122 100755 --- a/MAVProxy/tools/mavflightview.py +++ b/MAVProxy/tools/mavflightview.py @@ -24,6 +24,7 @@ from MAVProxy.modules.lib import multiproc from MAVProxy.modules.lib import grapher from MAVProxy.modules.lib import kmlread +from MAVProxy.modules.lib.log_mission import LogMission, MISSION_TYPES def create_map(title): @@ -202,8 +203,30 @@ def colourmap_for_mav_type(mav_type): return map +def drawable_mission(wploader): + '''wploader, or a copy of it pymavlink can draw. A DO_JUMP to an item + which is not there -- or to home -- ends the mission in ArduPilot, but + MAVWPLoader follows it and fails on the missing item. In the copy such + a jump points at itself, which ends the line drawn there instead''' + import copy + count = wploader.count() + bad = [i for i in range(count) + if wploader.wp(i).command == mavutil.mavlink.MAV_CMD_DO_JUMP and + not 0 < int(wploader.wp(i).param1) < count] + if not bad: + return wploader + ret = mavwp.MAVWPLoader() + for i in range(count): + w = copy.copy(wploader.wp(i)) + if i in bad: + w.param1 = i + ret.add(w) + return ret + + def display_waypoints(wploader, map, default_radius=None, mav_type=None): '''display the waypoints''' + wploader = drawable_mission(wploader) mission_list = wploader.view_list() polygons = wploader.polygon_list() map.add_object(mp_slipmap.SlipClearLayer('Mission')) @@ -389,10 +412,6 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): wp = mavwp.MAVWPLoader() if options.mission is not None: wp.load(options.mission) - # the radius a loiter item without one of its own will be flown at, from - # the parameters the log carries - options.default_circle_radius = mp_util.param_value( - getattr(mlog, 'params', None), 'WP_LOITER_RAD') fen = mavwp.MAVFenceLoader() if options.fence is not None: fen.load(options.fence) @@ -423,8 +442,9 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): if options.ahr2: expressions.extend(pos_expressions(['AHR2', 'AHRS2', 'GPS'])) - # find the union of message types we need from the log for all expressions - recv_match_types = set() + # find the union of message types we need from the log for all + # expressions, and the ones the mission is read from + recv_match_types = set(MISSION_TYPES) for e in expressions: recv_match_types.update(set(e.recv_match_types)) @@ -443,6 +463,7 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): mlog.rewind() + mission = LogMission() while True: try: m = mlog.recv_match(type=recv_match_types) @@ -453,60 +474,10 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): type = m.get_type() - if type in ['MISSION_ITEM', 'MISSION_ITEM_INT']: - try: - new_m = m - if type == 'MISSION_ITEM_INT': - # create a MISSION_ITEM from MISSION_ITEM_INT - new_m = mavutil.mavlink.MAVLink_mission_item_message( - 0, - 0, - m.seq, - m.frame, - m.command, - m.current, - m.autocontinue, - m.param1, - m.param2, - m.param3, - m.param4, - m.x / 1.0e7, - m.y / 1.0e7, - m.z - ) - while new_m.seq > wp.count(): - print("Adding dummy WP %u" % wp.count()) - wp.set(new_m, wp.count()) - wp.set(new_m, m.seq) - except Exception as e: - print("Exception: %s" % str(e)) - pass - continue - elif type == 'CMD': + if type in MISSION_TYPES: + # a mission given on the command line is drawn instead if options.mission is None: - m = mavutil.mavlink.MAVLink_mission_item_message( - 0, - 0, - m.CNum, - mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, - m.CId, - 0, # current - 1, # autocontinue - m.Prm1, - m.Prm2, - m.Prm3, - m.Prm4, - m.Lat, - m.Lng, - m.Alt - ) - try: - while m.seq > wp.count(): - print("Adding dummy WP %u" % wp.count()) - wp.set(m, wp.count()) - wp.set(m, m.seq) - except Exception: - pass + mission.read(m) continue if not mlog.check_condition(options.condition): @@ -573,6 +544,14 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): path = paths2 + mission.fill(wp) + + # the radius a loiter item without one of its own will be flown at, from + # the parameters the log carries. A telemetry log only has those once it + # has been read through + options.default_circle_radius = mp_util.param_value( + getattr(mlog, 'params', None), 'WP_LOITER_RAD') + if len(path) == 0: print("No points to plot") return None @@ -580,6 +559,33 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): return [path, wp, fen, used_flightmodes, getattr(mlog, 'mav_type', None), instances] +def mission_objects(wp, options, mav_type, title): + '''the map objects the mission in wp is drawn with: its legs and arcs, + and the circles of its loiters. None if there is nothing to draw''' + if not options.show_waypoints: + return None + wp = drawable_mission(wp) + plist = wp.polygon_list() + vlist = wp.view_list() + if len(plist) == 0: + return None + mission_obj = [] + for i in range(len(plist)): + mission_obj.append(mp_slipmap.SlipPolygon( + 'Mission-%s-%u' % (title, i), + plist[i], + layer='Mission', + linewidth=2, + colour=(255, 255, 255), + arcs=mp_slipmap.mission_arcs(wp, vlist[i]), + )) + mission_obj.extend(mp_slipmap.mission_circles( + 'Loiter-%s' % title, 'Mission', wp, vlist[i], plist[i], + default_radius=getattr(options, 'default_circle_radius', None), + vehicle=mav_type)) + return mission_obj + + def mavflightview_show(path, wp, fen, @@ -634,29 +640,7 @@ def mavflightview_show(path, linewidth=2, showlines=(not getattr(options, "no_show_lines", False)), colour=(255, 0, 180))) - plist = [] - vlist = [] - if options.show_waypoints: - plist = wp.polygon_list() - vlist = wp.view_list() - mission_obj = None - if len(plist) > 0: - mission_obj = [] - for i in range(len(plist)): - mission_obj.append(mp_slipmap.SlipPolygon( - 'Mission-%s-%u' % (title, i), - plist[i], - layer='Mission', - linewidth=2, - colour=(255, 255, 255), - arcs=mp_slipmap.mission_arcs(wp, vlist[i]), - )) - mission_obj.extend(mp_slipmap.mission_circles( - 'Loiter-%s' % title, 'Mission', wp, vlist[i], plist[i], - default_radius=getattr(options, 'default_circle_radius', None), - vehicle=mav_type)) - else: - mission_obj = None + mission_obj = mission_objects(wp, options, mav_type, title) if len(fence) > 1: fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence', diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000000..f5a292d43a --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,20 @@ +'''shared test setup''' + +import os + +import pytest + + +@pytest.fixture(autouse=True) +def restore_mavlink20(): + """put MAVLINK20 back as it was after each test. MAVExplorer.py sets it + when it is loaded, and pymavlink's mission loader reads it to choose the + messages it builds, but the dialect pymavlink has already loaded does not + change with it: tests which run after one loading MAVExplorer.py would + otherwise build messages the loaded dialect cannot take""" + before = os.environ.get('MAVLINK20') + yield + if before is None: + os.environ.pop('MAVLINK20', None) + else: + os.environ['MAVLINK20'] = before diff --git a/tests/test_mavflightview.py b/tests/test_mavflightview.py new file mode 100644 index 0000000000..bd2e419bb7 --- /dev/null +++ b/tests/test_mavflightview.py @@ -0,0 +1,365 @@ +'''what mavflightview reads out of a log''' + +import struct + +import pytest + +HERE = (-35.363262, 149.165238) + + +def mavflightview(): + pytest.importorskip("cv2") + pytest.importorskip("wx") + import importlib.util + import os + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'mavflightview.py') + # MAVProxy/tools is not a package, so the tool is loaded by path + spec = importlib.util.spec_from_file_location('mavflightview', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def write_tlog(path, messages): + '''write messages to a telemetry log, a second apart''' + from pymavlink import mavutil + mav = mavutil.mavlink.MAVLink(None, srcSystem=1, srcComponent=1) + usec = 1700000000 * 1000000 + with open(path, 'wb') as f: + for m in messages: + usec += 1000000 + f.write(struct.pack('>Q', usec) + m.pack(mav)) + + +def mission_item_int(seq, lat, lon, alt, command=None, params=(0, 0, 0, 0)): + from pymavlink import mavutil + mavlink = mavutil.mavlink + if command is None: + command = mavlink.MAV_CMD_NAV_WAYPOINT + return mavlink.MAVLink_mission_item_int_message( + 255, 0, seq, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + command, 0, 1, params[0], params[1], params[2], params[3], + int(lat * 1e7), int(lon * 1e7), alt) + + +def mission_count(count): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_mission_count_message(255, 0, count) + + +def mission_clear_all(): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_mission_clear_all_message(1, 1) + + +def heartbeat(): + from pymavlink import mavutil + mavlink = mavutil.mavlink + return mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, + 0, 0, 0, 3) + + +def param_value(name, value, count=1, index=0): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_param_value_message( + name.encode(), value, mavutil.mavlink.MAV_PARAM_TYPE_REAL32, + count, index) + + +def global_position_int(lat, lon): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_global_position_int_message( + 0, int(lat * 1e7), int(lon * 1e7), 100000, 100000, 0, 0, 0, 0) + + +def view(tmp_path, messages, types=None, mission=None): + '''run mavflightview over a telemetry log of messages. Returns what + mavflightview_mav does, and the options it ran with''' + from pymavlink import mavutil + tool = mavflightview() + path = str(tmp_path / 'flight.tlog') + write_tlog(path, messages) + options = tool.mavflightview_options() + options.types = types + options.mission = mission + mlog = mavutil.mavlink_connection(path) + return (tool.mavflightview_mav(mlog, options), options) + + +class Message(object): + '''a stand-in for a message from a log, with only the fields given''' + + def __init__(self, type, **fields): + self._type = type + self.__dict__.update(fields) + + def get_type(self): + return self._type + + +class TestTlogMission(object): + """a telemetry log carries its mission only as the MISSION_ITEM_INTs of + a download, with none of the CMD messages a dataflash log has""" + + def mission(self, tmp_path, messages, types=None, mission=None): + return view(tmp_path, messages, types, mission)[0][1] + + def items(self, n=3, lon=0.0, alt=50): + '''the n items of a mission''' + ret = [mission_item_int(0, HERE[0], HERE[1], 584)] + for seq in range(1, n): + ret.append(mission_item_int( + seq, HERE[0] + seq * 0.001, HERE[1] + lon, alt + seq)) + return ret + + def log(self, mission): + return ([global_position_int(HERE[0], HERE[1])] + mission + + [global_position_int(HERE[0] + 0.001, HERE[1])]) + + def altitudes(self, wp): + return [wp.wp(i).z for i in range(wp.count())] + + def test_mission_items_are_read(self, tmp_path): + wp = self.mission(tmp_path, self.log(self.items())) + assert wp.count() == 3 + assert wp.wp(2).x == pytest.approx(HERE[0] + 0.002) + assert wp.wp(2).y == pytest.approx(HERE[1]) + assert self.altitudes(wp) == [584, 51, 52] + + def test_mission_items_are_read_whatever_types_are_plotted(self, tmp_path): + wp = self.mission(tmp_path, self.log(self.items()), + types='GLOBAL_POSITION_INT') + assert wp.count() == 3 + + def test_a_download_is_read(self, tmp_path): + wp = self.mission(tmp_path, self.log([mission_count(3)] + self.items())) + assert self.altitudes(wp) == [584, 51, 52] + + def test_items_arriving_out_of_order_are_all_kept(self, tmp_path): + # a download where some items had to be asked for again + items = self.items(4) + items = [mission_count(4), items[3], items[1], items[0], items[2]] + wp = self.mission(tmp_path, self.log(items)) + assert [wp.wp(i).seq for i in range(wp.count())] == [0, 1, 2, 3] + assert self.altitudes(wp) == [584, 51, 52, 53] + + def test_nothing_is_made_up_for_items_never_seen(self, tmp_path): + # a download which never finished: item 2 did not arrive + items = self.items(4) + items = [mission_count(4), items[0], items[1], items[3]] + wp = self.mission(tmp_path, self.log(items)) + assert self.altitudes(wp) == [584, 51] + + def test_a_download_which_never_finished_leaves_the_last_whole_one( + self, tmp_path): + items = ([mission_count(3)] + self.items() + + [mission_count(3)] + self.items(alt=100)[:2]) + wp = self.mission(tmp_path, self.log(items)) + assert self.altitudes(wp) == [584, 51, 52] + + def test_a_shorter_mission_later_leaves_nothing_of_the_longer_one( + self, tmp_path): + items = ([mission_count(4)] + self.items(4) + + [mission_count(2)] + self.items(2, alt=100)) + wp = self.mission(tmp_path, self.log(items)) + assert self.altitudes(wp) == [584, 101] + + def test_a_cleared_mission_is_not_drawn(self, tmp_path): + items = [mission_count(3)] + self.items() + [mission_clear_all()] + wp = self.mission(tmp_path, self.log(items)) + assert wp.count() == 0 + + def test_a_mission_given_to_draw_is_left_alone(self, tmp_path): + path = tmp_path / 'mission.txt' + path.write_text( + 'QGC WPL 110\n' + '0\t1\t0\t16\t0\t0\t0\t0\t%f\t%f\t584\t1\n' + '1\t0\t3\t16\t0\t0\t0\t0\t%f\t%f\t77\t1\n' % + (HERE[0], HERE[1], HERE[0] - 0.01, HERE[1])) + items = [mission_count(3)] + self.items() + wp = self.mission(tmp_path, self.log(items), mission=str(path)) + assert self.altitudes(wp) == [584, 77] + + def test_a_jump_to_nowhere_ends_the_mission_drawn(self, tmp_path): + from pymavlink import mavutil + tool = mavflightview() + jump = mavutil.mavlink.MAVLink_mission_item_int_message( + 255, 0, 2, 0, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, + 20, 1, 0, 0, 0, 0, 0) + log = [heartbeat(), global_position_int(HERE[0], HERE[1]), + mission_item_int(0, HERE[0], HERE[1], 584), + mission_item_int(1, HERE[0] + 0.01, HERE[1], 100), jump, + mission_item_int(3, HERE[0] + 0.01, HERE[1] + 0.01, 100), + global_position_int(HERE[0] + 0.01, HERE[1])] + (ret, options) = view(tmp_path, log) + (wp, mav_type) = (ret[1], ret[4]) + assert wp.count() == 4 + objects = tool.mission_objects(wp, options, mav_type, 'T') + lines = [o for o in objects if type(o).__name__ == 'SlipPolygon'] + # the line runs to the jump and stops; the item after it, which + # the vehicle never reaches, is drawn apart from it + assert [len(line.points) for line in lines] == [2, 1] + # and the live map is given the same mission to draw + added = [] + + class Map(object): + def add_object(self, obj): + added.append(obj) + tool.display_waypoints(wp, Map()) + assert [o for o in added if type(o).__name__ == 'SlipPolygon'] + # the mission itself is left as it was + assert wp.wp(2).param1 == 20 + + +class TestLogMission(object): + """the messages a log's mission is read from, one at a time: those which + need MAVLink2 fields or a dataflash log to make""" + + def mission(self, messages): + from pymavlink import mavwp + mission = mavflightview().LogMission() + for m in messages: + mission.read(m) + wp = mavwp.MAVWPLoader() + mission.fill(wp) + return [wp.wp(i).z for i in range(wp.count())] + + def item(self, seq, alt, mission_type=0): + return Message('MISSION_ITEM', seq=seq, x=HERE[0], y=HERE[1], z=alt, + mission_type=mission_type) + + def count(self, count, mission_type=0): + return Message('MISSION_COUNT', count=count, + mission_type=mission_type) + + def cmd(self, num, alt, total=None): + fields = dict(CNum=num, CId=16, Prm1=0, Prm2=0, Prm3=0, Prm4=0, + Lat=HERE[0], Lng=HERE[1], Alt=alt) + if total is not None: + fields['CTot'] = total + return Message('CMD', **fields) + + def test_fence_and_rally_points_are_not_the_mission(self): + from pymavlink import mavutil + mission = [self.count(2), self.item(0, 584), self.item(1, 50)] + for mission_type in (mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY): + mission += [self.count(3, mission_type), + self.item(0, 1000, mission_type), + self.item(1, 1001, mission_type), + self.item(2, 1002, mission_type)] + assert self.mission(mission) == [584, 50] + + def test_clearing_the_fence_leaves_the_mission(self): + from pymavlink import mavutil + fence = mavutil.mavlink.MAV_MISSION_TYPE_FENCE + mission = [self.count(2), self.item(0, 584), self.item(1, 50), + Message('MISSION_CLEAR_ALL', mission_type=fence)] + assert self.mission(mission) == [584, 50] + everything = mavutil.mavlink.MAV_MISSION_TYPE_ALL + mission[-1] = Message('MISSION_CLEAR_ALL', mission_type=everything) + assert self.mission(mission) == [] + + def test_a_mavlink1_item_is_a_mission_item(self): + item = Message('MISSION_ITEM', seq=0, x=HERE[0], y=HERE[1], z=584) + assert self.mission([item]) == [584] + + def test_the_last_mission_a_dataflash_log_writes_out(self): + dumps = ([self.cmd(n, 50 + n, 4) for n in range(4)] + + [self.cmd(n, 100 + n, 2) for n in range(2)]) + assert self.mission(dumps) == [100, 101] + + def test_dataflash_mission_with_no_count_of_its_items(self): + # older logs have no CTot: each dump ends where the next starts + dumps = ([self.cmd(n, 50 + n) for n in range(4)] + + [self.cmd(n, 100 + n) for n in range(2)]) + assert self.mission(dumps) == [100, 101] + assert self.mission(dumps[:4]) == [50, 51, 52, 53] + + def test_a_cleared_dataflash_mission_is_not_drawn(self): + # the logger writes "New mission" before each mission it writes + # out, and nothing after it for one which has been cleared + new_mission = Message('MSG', Message='New mission') + dumps = ([new_mission] + [self.cmd(n, 50 + n, 4) for n in range(4)] + + [new_mission]) + assert self.mission(dumps) == [] + # other messages leave it alone, and the next mission is drawn + dumps += [Message('MSG', Message='Mission: 1 WP')] + dumps += [new_mission] + [self.cmd(n, 100 + n, 2) for n in range(2)] + assert self.mission(dumps) == [100, 101] + + def test_a_cleared_dataflash_mission_is_not_drawn_from_a_log(self, tmp_path): + # a dataflash log through mavflightview itself, which has to ask the + # log for the logger's messages as well as the mission + tool = mavflightview() + messages = ( + [Message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), + Message('MSG', Message='New mission')] + + [self.cmd(n, 50 + n, 3) for n in range(3)] + + [Message('MSG', Message='New mission'), + Message('POS', Lat=HERE[0] + 0.01, Lng=HERE[1], Alt=600.0)]) + + class Log(object): + flightmode = 'AUTO' + messages = {} + + def __init__(self): + self.queue = [] + + def rewind(self): + self.queue = list(messages) + + def recv_match(self, type=None): + while self.queue: + m = self.queue.pop(0) + m._timestamp = 1.0 + if type is None or m.get_type() in type: + return m + return None + + def check_condition(self, condition): + return True + options = tool.mavflightview_options() + options.colour_source = None + ret = tool.mavflightview_mav(Log(), options) + assert ret[1].count() == 0 + + +class TestTlogParameters(object): + """a loiter with no radius of its own is drawn at WP_LOITER_RAD, which a + telemetry log only carries in its PARAM_VALUEs""" + + def view(self, tmp_path, messages): + return view(tmp_path, messages) + + def radius(self, tmp_path, messages): + return self.view(tmp_path, messages)[1].default_circle_radius + + def log(self, params): + from pymavlink import mavutil + loiter = mission_item_int( + 1, HERE[0] + 0.01, HERE[1], 100, + command=mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, + params=(3, 0, 0, 0)) + return ([heartbeat(), global_position_int(HERE[0], HERE[1])] + + params + + [mission_item_int(0, HERE[0], HERE[1], 584), loiter, + global_position_int(HERE[0] + 0.01, HERE[1])]) + + def test_parameters_sent_by_the_autopilot(self, tmp_path): + # opening a log reads the first message of each type in it, so + # WP_LOITER_RAD is not the first parameter, as in any real download + log = self.log([param_value('AHRS_EKF_TYPE', 3, 2, 0), + param_value('WP_LOITER_RAD', 150, 2, 1)]) + assert self.radius(tmp_path, log) == 150 + + def test_the_loiter_is_drawn_at_that_radius(self, tmp_path): + log = self.log([param_value('AHRS_EKF_TYPE', 3, 2, 0), + param_value('WP_LOITER_RAD', 150, 2, 1)]) + (ret, options) = self.view(tmp_path, log) + (wp, mav_type) = (ret[1], ret[4]) + objects = mavflightview().mission_objects(wp, options, mav_type, 'T') + circles = [o for o in objects if type(o).__name__ == 'SlipCircle'] + assert [c.radius for c in circles] == [150] From 8c9933abfe28435fb0aea7898f62d0a07a427440 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 16:15:34 +1000 Subject: [PATCH 12/22] mp_util: take the parameters MAVProxy fetched over FTP from a log When MAVProxy fetches a vehicle's parameters over FTP it logs them to the tlog itself, as PARAM_VALUEs from MAV_COMP_ID_MISSIONPLANNER so they can be told apart from ones the autopilot sent. A log's params only offer the autopilot's own, so a log from a vehicle whose parameters were fetched that way has just the few it sent as they changed -- 35 of 1479 in the log this was found in, and not WP_LOITER_RAD, so mavflightview still drew no loiter circles. Add log_params(), which takes both, the autopilot's own values over the fetched ones, and use it where mavflightview and MAVExplorer's 3D map read parameters to draw a mission. A log picks the vehicle whose parameters it offers from its first heartbeat, and one with no heartbeat in it -- cut out of a longer log, say -- offers none at all. Take the parameters of the one vehicle they came from there, and none where several vehicles sent some, rather than guess between them. --- MAVProxy/modules/lib/mp_util.py | 34 +++++++++++++++++ MAVProxy/tools/MAVExplorer.py | 3 +- MAVProxy/tools/mavflightview.py | 2 +- tests/test_mavflightview.py | 66 ++++++++++++++++++++++++++++++--- 4 files changed, 98 insertions(+), 7 deletions(-) diff --git a/MAVProxy/modules/lib/mp_util.py b/MAVProxy/modules/lib/mp_util.py index 35de9f807a..43c3a3a618 100644 --- a/MAVProxy/modules/lib/mp_util.py +++ b/MAVProxy/modules/lib/mp_util.py @@ -365,6 +365,40 @@ def mission_crosstracks_from_centre(command, params): return not xtrack > 0 +def log_params(mlog): + """the parameters a log carries. A telemetry log's params are only the + PARAM_VALUEs the autopilot sent. Where MAVProxy fetched them over FTP + it logged them itself, as PARAM_VALUEs from MAV_COMP_ID_MISSIONPLANNER so + they can be told apart, and those are left out: a log from a vehicle + whose parameters were fetched that way offers only the few the autopilot + sent as they changed. Take both, the autopilot's own over the fetched + ones""" + params = getattr(mlog, 'params', None) + param_state = getattr(mlog, 'param_state', None) + if not param_state: + return params + from pymavlink import mavutil + sysid = getattr(mlog, 'sysid', 0) + if not sysid: + # the log picks the vehicle whose params it offers from its first + # heartbeat, and a log with none in it never picks one. Take the + # vehicle the parameters came from, so long as only one did + systems = set(k[0] for (k, state) in param_state.items() + if k[0] != 0 and state.params) + if len(systems) != 1: + return params + sysid = systems.pop() + own = param_state.get((sysid, 1)) + params = own.params if own is not None else {} + fetched = param_state.get((sysid, + mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER)) + if fetched is None or not fetched.params: + return params + ret = dict(fetched.params) + ret.update(params or {}) + return ret + + def param_value(params, name): """look one parameter up in params, which may be a mapping (a live vehicle's mav_param, or a log's params) or a callable taking a name. diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index 7a1f60b838..32cc2c10da 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -799,7 +799,8 @@ def cmd_map3d(args): # waypoints (MAV_FRAME_GLOBAL_TERRAIN_ALT = 10/11) are "z above terrain", so # they need the terrain elevation at the waypoint, not home + z. if mission: - mission = resolve_mission_amsl(mission, ground0, mlog.params, + mission = resolve_mission_amsl(mission, ground0, + mp_util.log_params(mlog), getattr(mlog, 'mav_type', None)) # drop views the user has already closed, so their child processes are reaped diff --git a/MAVProxy/tools/mavflightview.py b/MAVProxy/tools/mavflightview.py index 8f4cd6c122..c5f942aa78 100755 --- a/MAVProxy/tools/mavflightview.py +++ b/MAVProxy/tools/mavflightview.py @@ -550,7 +550,7 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): # the parameters the log carries. A telemetry log only has those once it # has been read through options.default_circle_radius = mp_util.param_value( - getattr(mlog, 'params', None), 'WP_LOITER_RAD') + mp_util.log_params(mlog), 'WP_LOITER_RAD') if len(path) == 0: print("No points to plot") diff --git a/tests/test_mavflightview.py b/tests/test_mavflightview.py index bd2e419bb7..2ed42ea2c8 100644 --- a/tests/test_mavflightview.py +++ b/tests/test_mavflightview.py @@ -22,12 +22,20 @@ def mavflightview(): def write_tlog(path, messages): - '''write messages to a telemetry log, a second apart''' + '''write messages to a telemetry log, a second apart. A message is + sent by vehicle 1's autopilot unless it is given as (component, message) + or (system, component, message)''' from pymavlink import mavutil - mav = mavutil.mavlink.MAVLink(None, srcSystem=1, srcComponent=1) usec = 1700000000 * 1000000 with open(path, 'wb') as f: for m in messages: + (system, component) = (1, 1) + if isinstance(m, tuple) and len(m) == 2: + (component, m) = m + elif isinstance(m, tuple): + (system, component, m) = m + mav = mavutil.mavlink.MAVLink(None, srcSystem=system, + srcComponent=component) usec += 1000000 f.write(struct.pack('>Q', usec) + m.pack(mav)) @@ -337,14 +345,14 @@ def view(self, tmp_path, messages): def radius(self, tmp_path, messages): return self.view(tmp_path, messages)[1].default_circle_radius - def log(self, params): + def log(self, params, with_heartbeat=True): from pymavlink import mavutil loiter = mission_item_int( 1, HERE[0] + 0.01, HERE[1], 100, command=mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, params=(3, 0, 0, 0)) - return ([heartbeat(), global_position_int(HERE[0], HERE[1])] + - params + + start = [heartbeat()] if with_heartbeat else [] + return (start + [global_position_int(HERE[0], HERE[1])] + params + [mission_item_int(0, HERE[0], HERE[1], 584), loiter, global_position_int(HERE[0] + 0.01, HERE[1])]) @@ -363,3 +371,51 @@ def test_the_loiter_is_drawn_at_that_radius(self, tmp_path): objects = mavflightview().mission_objects(wp, options, mav_type, 'T') circles = [o for o in objects if type(o).__name__ == 'SlipCircle'] assert [c.radius for c in circles] == [150] + + def test_parameters_mavproxy_fetched_over_ftp(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + # the autopilot sends a few of its own too, as they change + log = self.log([(ftp, param_value('WP_RADIUS', 90, 2, 0)), + (ftp, param_value('WP_LOITER_RAD', 150, 2, 1)), + param_value('STAT_RUNTIME', 1000)]) + assert self.radius(tmp_path, log) == 150 + + def test_the_autopilots_own_value_wins(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(ftp, param_value('WP_LOITER_RAD', 150)), + param_value('WP_LOITER_RAD', 80)]) + assert self.radius(tmp_path, log) == 80 + + def test_a_log_with_no_heartbeat(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(ftp, param_value('WP_RADIUS', 90, 2, 0)), + (ftp, param_value('WP_LOITER_RAD', 150, 2, 1)), + param_value('STAT_RUNTIME', 1000)], + with_heartbeat=False) + assert self.radius(tmp_path, log) == 150 + log = self.log([param_value('AHRS_EKF_TYPE', 3, 2, 0), + param_value('WP_LOITER_RAD', 120, 2, 1)], + with_heartbeat=False) + assert self.radius(tmp_path, log) == 120 + + def test_no_guessing_between_vehicles_with_no_heartbeat(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(1, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (1, ftp, param_value('WP_LOITER_RAD', 150, 2, 1)), + (2, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (2, ftp, param_value('WP_LOITER_RAD', 60, 2, 1))], + with_heartbeat=False) + assert self.radius(tmp_path, log) is None + + def test_the_vehicle_the_log_follows(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(2, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (2, ftp, param_value('WP_LOITER_RAD', 60, 2, 1)), + (1, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (1, ftp, param_value('WP_LOITER_RAD', 150, 2, 1))]) + assert self.radius(tmp_path, log) == 150 From c5e2740b24c4c22d40156d160c68afb2b02d900d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Sep 2026 18:37:38 +1000 Subject: [PATCH 13/22] MAVExplorer: take home from the log where the mission has none A mission uploaded before the vehicle has a home logs its home item as nothing at all, and resolve_mission_amsl() then measures every home-relative altitude from the lowest position in the log instead. The first positions are logged before the EKF has an origin, at nothing too, so a QuadPlane log from SITL drew its relative-altitude items 165m low. The vehicle logs its home in ORGN once it has one. Where the mission's home item is empty, fill it from the last one logged. --- MAVProxy/tools/MAVExplorer.py | 13 ++++++++++++- tests/test_mission_geometry.py | 27 +++++++++++++++++++++++++++ 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index 32cc2c10da..b6a03a9cc0 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -706,14 +706,20 @@ def mission_from_log(mlog, condition=None): # where the vehicle was when each item began, from the "Mission: " # the autopilot announces as it starts one flown_from = {} + # home, as the vehicle last logged it + home = None while True: - m = mlog.recv_match(type=['POS', 'CMD', 'MSG'], condition=condition) + m = mlog.recv_match(type=['POS', 'CMD', 'MSG', 'ORGN'], + condition=condition) if m is None: break mtype = m.get_type() if mtype == 'POS': path.append((m.Lat, m.Lng, m.Alt, grapher.timestamp_to_days(m._timestamp))) + elif mtype == 'ORGN': + if m.Type == 1 and (m.Lat != 0 or m.Lng != 0): + home = (m.Lat, m.Lng, m.Alt) elif mtype == 'MSG': fields = m.Message.split() if m.Message == 'New mission': @@ -731,6 +737,11 @@ def mission_from_log(mlog, condition=None): params = tuple(getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)) items[m.CNum] = (m.Lat, m.Lng, m.Alt, getattr(m, 'Frame', 3), m.CId, m.CNum, params) + if (home is not None and 0 in items and + items[0][0] == 0 and items[0][1] == 0): + # a mission uploaded before the vehicle had a home carries an empty + # one, which everything relative to home would be measured from + items[0] = (home[0], home[1], home[2], 0) + items[0][4:] mission = mission_items_from_cmds( items, started_at=(path[0][0], path[0][1]) if path else None, flown_from=flown_from) diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index deff24f70d..1b54cef7e1 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -615,6 +615,33 @@ def cmd(seq, command, lat, lng, alt): )) assert (mission[1][0], mission[1][1]) == moved + def test_an_empty_home_is_the_one_the_log_recorded(self): + # a mission uploaded before the vehicle had a home logs home as + # nothing at all; the vehicle logs its home when it gets one. The + # first positions are logged before the origin is, at nothing too + from pymavlink import mavutil + m = mavutil.mavlink + up = mp_util.gps_newpos(HERE[0], HERE[1], 0, 1000) + + def cmd(seq, command, lat, lng, alt, frame): + return self.message('CMD', CNum=seq, CId=command, Lat=lat, + Lng=lng, Alt=alt, Frame=frame, Prm1=0, + Prm2=0, Prm3=0, Prm4=0) + (path, mission) = self.module().mission_from_log(self.log( + self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=0.1), + self.message('MSG', Message='New mission'), + cmd(0, m.MAV_CMD_NAV_WAYPOINT, 0.0, 0.0, 0.0, 0), + cmd(1, m.MAV_CMD_NAV_WAYPOINT, up[0], up[1], 70.0, 3), + self.message('ORGN', Type=0, Lat=HERE[0], Lng=HERE[1], Alt=500.0), + self.message('ORGN', Type=1, Lat=HERE[0], Lng=HERE[1], Alt=584.0), + self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), + )) + assert (mission[0][0], mission[0][1], mission[0][2]) == HERE + (584.0,) + ground0 = min(p[2] for p in path) + out = self.module().resolve_mission_amsl(mission, ground0, {}) + # measured from the home the vehicle had, not the lowest position + assert out[1].alt == pytest.approx(654.0) + def test_a_log_from_before_the_logger_said_new_mission(self): # without the message, a mission's first item still starts it again first = [m for m in self.dump(HERE, 4) if m.get_type() == 'CMD'] From 79c17702ba8649f58a83fe6168da7feab75c192a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 3 Oct 2026 12:34:19 +1000 Subject: [PATCH 14/22] plane_track: fly a mission through ArduPlane's navigation Work out the path a plane or QuadPlane flies a mission along, rather than the lines between its items: a port of enough of ArduPlane's navigation to fly the mission with its L1 controller, at the aircraft's own airspeeds, roll limits and climb rates. It flies every loiter, with the radius and direction the item and WP_LOITER_RAD give it and the exit ArduPlane takes -- lined up on the next item as the mission stores it, or crosstracking from where the circle is left; waypoints with their acceptance and pass-by radii, and WP_MAX_RADIUS; a fixed-wing takeoff at the item's pitch, or faster where TECS has an airspeed sensor to fly by, and a VTOL one straight up and away the way the aircraft points; the return to launch a RETURN_TO_LAUNCH item or a mission running out makes, to home or the rally point nearest it, with the circle a plane ends in and each Q_RTL_MODE a QuadPlane lands by; a VTOL landing straight in or on the fixed-wing spiral approach param1 or Q_OPTIONS asks for, into the wind; and jumps, jump tags, speed changes and altitude frames along the way. Where the path cannot be worked out -- a command it does not know, an item the aircraft never finishes, or a flight longer than it is worth following -- it says so, and the caller draws the mission's geometry instead. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/modules/lib/plane_track.py | 1406 +++++++++++++++++++++++++++ tests/test_plane_track.py | 1204 +++++++++++++++++++++++ 2 files changed, 2610 insertions(+) create mode 100644 MAVProxy/modules/lib/plane_track.py create mode 100644 tests/test_plane_track.py diff --git a/MAVProxy/modules/lib/plane_track.py b/MAVProxy/modules/lib/plane_track.py new file mode 100644 index 0000000000..4aa2194b7f --- /dev/null +++ b/MAVProxy/modules/lib/plane_track.py @@ -0,0 +1,1406 @@ +''' +the path an ArduPlane flies along a mission + +ArduPlane does not fly the lines drawn between its mission items. Its L1 +controller tracks each leg from wherever set_next_WP() says the leg starts, +which is the item before, the centre of a loiter just left, the point the +loiter was left at, or wherever the vehicle happened to be; it flies at a +loiter's centre until it is close enough to capture the circle, and it +leaves only once it is lined up on whatever comes next. How the aircraft +then turns onto each of those tracks is the controller's own dynamics, so +rather than approximate them with geometry this ports the controller and +the mission logic around it, and flies a simple aircraft through them: a +fixed airspeed, turning with a coordinated bank that lags the controller's +demand, and climbing and descending within the rates TECS allows. + +The ports follow AP_L1_Control and ArduPlane's commands_logic.cpp, +navigation.cpp, altitude.cpp and mode_loiter.cpp. + +AP_FLAKE8_CLEAN +''' + +import math + +from pymavlink import mavutil + +from MAVProxy.modules.lib import mp_util + +GRAVITY = 9.80665 +# ArduPilot's Location maths: metres per 1e-7 degree of latitude +LOCATION_SCALING_FACTOR = 0.011131884502145034 + +# ArduPlane's own defaults, for parameters a vehicle or log does not carry. +# Older firmware names some of them differently, in other units +PARAMETERS = { + 'NAVL1_PERIOD': ([('NAVL1_PERIOD', 1.0)], 17.0), + 'NAVL1_DAMPING': ([('NAVL1_DAMPING', 1.0)], 0.75), + 'NAVL1_XTRACK_I': ([('NAVL1_XTRACK_I', 1.0)], 0.02), + 'NAVL1_LIM_BANK': ([('NAVL1_LIM_BANK', 1.0)], 0.0), + 'ROLL_LIMIT': ([('ROLL_LIMIT_DEG', 1.0), ('LIM_ROLL_CD', 0.01)], 45.0), + 'RLL2SRV_TCONST': ([('RLL2SRV_TCONST', 1.0)], 0.5), + 'WP_RADIUS': ([('WP_RADIUS', 1.0)], 90.0), + 'WP_MAX_RADIUS': ([('WP_MAX_RADIUS', 1.0)], 0.0), + 'WP_LOITER_RAD': ([('WP_LOITER_RAD', 1.0)], 60.0), + 'AIRSPEED_CRUISE': ([('AIRSPEED_CRUISE', 1.0), ('TRIM_ARSPD_CM', 0.01)], + 12.0), + 'AIRSPEED_MIN': ([('AIRSPEED_MIN', 1.0), ('ARSPD_FBW_MIN', 1.0)], 9.0), + 'AIRSPEED_MAX': ([('AIRSPEED_MAX', 1.0), ('ARSPD_FBW_MAX', 1.0)], 22.0), + 'TECS_CLMB_MAX': ([('TECS_CLMB_MAX', 1.0)], 5.0), + 'TECS_SINK_MAX': ([('TECS_SINK_MAX', 1.0)], 5.0), + 'TECS_TIME_CONST': ([('TECS_TIME_CONST', 1.0)], 5.0), + 'ALT_SLOPE_MIN': ([('ALT_SLOPE_MIN', 1.0)], 15.0), + 'CLIMB_SLOPE_HGT': ([('CLIMB_SLOPE_HGT', 1.0)], 25.0), + 'RTL_ALTITUDE': ([('RTL_ALTITUDE', 1.0), ('ALT_HOLD_RTL', 0.01)], 100.0), + 'RTL_RADIUS': ([('RTL_RADIUS', 1.0)], 0.0), + 'RALLY_LIMIT_KM': ([('RALLY_LIMIT_KM', 1.0)], 5.0), + 'RALLY_INCL_HOME': ([('RALLY_INCL_HOME', 1.0)], 0.0), + 'Q_ENABLE': ([('Q_ENABLE', 1.0)], 0.0), + 'Q_OPTIONS': ([('Q_OPTIONS', 1.0)], 0.0), + 'Q_RTL_MODE': ([('Q_RTL_MODE', 1.0)], 0.0), + 'Q_RTL_ALT': ([('Q_RTL_ALT', 1.0)], 15.0), + 'Q_FW_LND_APR_RAD': ([('Q_FW_LND_APR_RAD', 1.0)], 0.0), + 'Q_TRANS_DECEL': ([('Q_TRANS_DECEL', 1.0)], 2.0), + 'FLIGHT_OPTIONS': ([('FLIGHT_OPTIONS', 1.0)], 0.0), + 'ARSPD_TYPE': ([('ARSPD_TYPE', 1.0)], 0.0), + 'ARSPD_USE': ([('ARSPD_USE', 1.0)], 0.0), +} +# FLIGHT_OPTIONS bit for climbing to a higher waypoint at once in AUTO, +# rather than on a slope along the leg +FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO = 1 << 14 +# the pitch in degrees a fixed-wing takeoff climbs at where its item gives +# none (Plane::do_takeoff) +DEFAULT_TAKEOFF_PITCH = 4.0 +# QuadPlane's Q_OPTIONS bit for flying NAV_TAKEOFF as a fixed-wing takeoff +Q_OPTION_ALLOW_FW_TAKEOFF = 1 << 1 +# QuadPlane's Q_OPTIONS bit for landing a mission's NAV_VTOL_LAND after a +# fixed-wing approach, which the item's param1 can ask for too +Q_OPTION_MISSION_LAND_FW_APPROACH = 1 << 4 +NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH = 1 +# the course of a VTOL landing approach where no wind is known: ArduPlane +# takes it as into the wind, atan2(-wind.y, -wind.x), which a wind of zero +# makes due south +STILL_AIR_APPROACH = -180.0 +# QuadPlane::RTL_MODE, what Q_RTL_MODE makes of a return to launch +Q_RTL_DISABLED = 0 +Q_RTL_SWITCH_QRTL = 1 +Q_RTL_VTOL_APPROACH = 2 +Q_RTL_QRTL_ALWAYS = 3 + +# how often ArduPlane navigates, and how finely the aircraft is flown +NAV_PERIOD = 0.1 +# the drawn path gets a point at least this often, and whenever the aircraft +# has turned this far since the last one +POINT_SPACING = 20.0 +POINT_TURN = math.radians(4.0) +# a mission is not flown for longer than this, and one item not for longer +# than it takes to fly its leg this many times over, plus the time its +# loiter asks for -- the turns or the time, or the climb or descent -- and +# MAX_LAPS laps more: past that the aircraft is taken to be stuck, circling +# a waypoint it cannot turn tightly enough to reach. Flying a mission takes +# a second or so of work for every hour of it, so one which plainly takes +# longer than this is not attempted +MAX_FLIGHT_TIME = 4 * 3600.0 +LEG_ALLOWANCE = 4.0 +MAX_LAPS = 20 +LOITER_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, +) +WAYPOINT_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + mavutil.mavlink.MAV_CMD_NAV_LAND, + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, + mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, +) +TAKEOFF_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, +) +# items a flight ends at, rather than running out of mission and returning +# to launch (MissionFlight.run) +ENDS_THE_FLIGHT = ( + mavutil.mavlink.MAV_CMD_NAV_LAND, + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, +) +# navigation commands numbered past MAV_CMD_NAV_LAST +LATE_NAV_COMMANDS = (42702, 42703) # NAV_SCRIPT_TIME, NAV_ATTITUDE_TIME +MAV_CMD_JUMP_TAG = 600 +MAV_CMD_DO_JUMP_TAG = 601 +# ArduPlane rejects these, and moves straight on to the item after +SKIPPED_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT, + mp_util.MAV_CMD_NAV_ARC_WAYPOINT, +) + + +def parameter(params, name): + '''a parameter the flight depends on, from params or ArduPlane's + default''' + (names, default) = PARAMETERS[name] + for (param, scale) in names: + value = mp_util.param_value(params, param) + if value is not None and not math.isnan(value): + return float(value) * scale + return default + + +def eas2tas(amsl): + '''the true airspeed of each metre per second of equivalent airspeed at + an altitude, in the standard atmosphere''' + temperature = max(288.15 - 0.0065 * amsl, 150.0) + return 1.0 / math.sqrt((temperature / 288.15) ** 4.2559) + + +def wrap_pi(angle): + return (angle + math.pi) % (2 * math.pi) - math.pi + + +def wrap_longitude(dlon): + '''a difference in longitude, the short way round: Location's + diff_longitude()''' + return (dlon + 180.0) % 360.0 - 180.0 + + +def cross(a, b): + return a[0] * b[1] - a[1] * b[0] + + +def dot(a, b): + return a[0] * b[0] + a[1] * b[1] + + +def minus(a, b): + return (a[0] - b[0], a[1] - b[1]) + + +def norm(a): + return math.hypot(a[0], a[1]) + + +def unit(a): + length = norm(a) + return (a[0] / length, a[1] / length) + + +def path_proportion(point, start, end): + '''Location::line_path_proportion: how far along start->end point is, + more than 1 once past end''' + leg = minus(end, start) + squared = dot(leg, leg) + if squared < 0.001: + return 1.0 + return dot(leg, minus(point, start)) / squared + + +class L1Control(object): + '''AP_L1_Control, working in metres north and east''' + + def __init__(self, period, damping, xtrack_i_gain, bank_limit): + self.period = period + self.damping = damping + self.xtrack_i_gain = xtrack_i_gain + self.bank_limit = bank_limit + self.xtrack_i = 0.0 + self.last_nu = 0.0 + self.lateral_acceleration = 0.0 + self.circling = False + self.distance = 0.0 + self.target_bearing = 0.0 + + def loiter_radius(self, radius, amsl, cruise): + '''the radius actually flown for a loiter radius at an altitude''' + scale = eas2tas(amsl) ** 2 + bank = min(max(self.bank_limit, 0.0), 89.0) + if bank <= 0 or cruise <= 0: + return radius * scale + sea_level = cruise ** 2 / (math.tan(math.radians(bank)) * GRAVITY) + if sea_level > radius: + return radius * scale + return max(sea_level * scale, radius) + + def turn_distance(self, wp_radius, amsl, turn_angle): + distance = min(wp_radius * eas2tas(amsl) ** 2, self.distance) + turn_angle = abs(turn_angle) + if turn_angle >= 90: + return distance + return distance * turn_angle / 90.0 + + def _prevent_indecision(self, nu, yaw): + limit = 0.9 * math.pi + if (abs(nu) > limit and abs(self.last_nu) > limit and + abs(wrap_pi(self.target_bearing - yaw)) > math.radians(120) and + nu * self.last_nu < 0): + return self.last_nu + return nu + + def update_waypoint(self, position, velocity, yaw, start, end, dt): + k_l1 = 4.0 * self.damping ** 2 + speed = max(norm(velocity), 0.1) + self.target_bearing = math.atan2(end[1] - position[1], + end[0] - position[0]) + self.distance = 0.3183099 * self.damping * self.period * speed + track = minus(end, start) + track_length = norm(track) + if track_length < 1.0e-6: + track = minus(end, position) + if norm(track) < 1.0e-6: + track = (math.cos(yaw), math.sin(yaw)) + track = unit(track) + from_start = minus(position, start) + crosstrack = cross(from_start, track) + start_distance = norm(from_start) + along = dot(from_start, track) + if (start_distance > self.distance and + along / max(start_distance, 1.0) < -0.7071): + # behind the start of the leg: fly to its start + towards = unit(from_start) + towards = (-towards[0], -towards[1]) + nu = math.atan2(cross(velocity, towards), dot(velocity, towards)) + elif along > track_length + speed * 3: + # three seconds past the end of it: fly back to the end + towards = unit(minus(end, position)) + nu = math.atan2(cross(velocity, towards), dot(velocity, towards)) + else: + nu2 = math.atan2(cross(velocity, track), dot(velocity, track)) + sine_nu1 = crosstrack / max(self.distance, 0.1) + nu1 = math.asin(min(max(sine_nu1, -0.7071), 0.7071)) + if self.xtrack_i_gain <= 0: + self.xtrack_i = 0.0 + elif abs(nu1) < math.radians(5): + self.xtrack_i += nu1 * self.xtrack_i_gain * dt + self.xtrack_i = min(max(self.xtrack_i, -0.1), 0.1) + nu = nu1 + self.xtrack_i + nu2 + nu = self._prevent_indecision(nu, yaw) + self.last_nu = nu + nu = min(max(nu, -1.5708), 1.5708) + self.lateral_acceleration = k_l1 * speed * speed / self.distance * math.sin(nu) + self.circling = False + + def update_loiter(self, position, velocity, yaw, centre, radius, direction): + omega = 2 * math.pi / self.period + kx = omega * omega + kv = 2.0 * self.damping * omega + k_l1 = 4.0 * self.damping ** 2 + speed = max(norm(velocity), 1.0) + self.target_bearing = math.atan2(centre[1] - position[1], + centre[0] - position[0]) + self.distance = 0.3183099 * self.damping * self.period * speed + from_centre = minus(position, centre) + if norm(from_centre) > 0.1: + outwards = unit(from_centre) + else: + outwards = unit(velocity) + # capturing the centre, as a waypoint + xtrack_velocity = cross(outwards, velocity) + ltrack_velocity = -dot(velocity, outwards) + nu = self._prevent_indecision( + math.atan2(xtrack_velocity, ltrack_velocity), yaw) + self.last_nu = nu + nu = min(max(nu, -math.pi / 2), math.pi / 2) + capture = k_l1 * speed * speed / self.distance * math.sin(nu) + # tracking the circle + radial_velocity = -ltrack_velocity + radial_error = norm(from_centre) - radius + pd = radial_error * kx + radial_velocity * kv + tangential = xtrack_velocity * direction + centripetal = tangential ** 2 / max(0.5 * radius, radius + radial_error) + if ltrack_velocity < 0: + if tangential < 0: + pd = max(pd, 0.0) + elif radial_error < 0: + pd = max(pd, -centripetal) + circle = direction * (pd + centripetal) + # the switch from capture to circling happens where the two demands + # cross, and only outside the circle + if radial_error > 0 and direction * capture < direction * circle: + self.lateral_acceleration = capture + self.circling = False + else: + self.lateral_acceleration = circle + self.circling = True + + +class MissionFlight(object): + '''ArduPlane flying a mission in AUTO, from home''' + + def __init__(self, origin, home, items, params, heading=None, start=None, + rally=None, approach=None, takeoff_course=None): + self.origin = origin + self.lon_scale = math.cos(math.radians(origin[0])) + self.params = params + self.l1 = L1Control(parameter(params, 'NAVL1_PERIOD'), + parameter(params, 'NAVL1_DAMPING'), + parameter(params, 'NAVL1_XTRACK_I'), + parameter(params, 'NAVL1_LIM_BANK')) + self.cruise = parameter(params, 'AIRSPEED_CRUISE') + self.airspeed = self.cruise + self.airspeed_min = parameter(params, 'AIRSPEED_MIN') + self.airspeed_max = parameter(params, 'AIRSPEED_MAX') + self.roll_limit = math.radians(parameter(params, 'ROLL_LIMIT')) + self.roll_tconst = max(parameter(params, 'RLL2SRV_TCONST'), 0.05) + self.wp_radius = parameter(params, 'WP_RADIUS') + self.wp_max_radius = parameter(params, 'WP_MAX_RADIUS') + self.loiter_radius = parameter(params, 'WP_LOITER_RAD') + self.climb = parameter(params, 'TECS_CLMB_MAX') + self.sink = parameter(params, 'TECS_SINK_MAX') + self.alt_tconst = max(parameter(params, 'TECS_TIME_CONST'), 0.5) + self.alt_slope_min = parameter(params, 'ALT_SLOPE_MIN') + self.climb_slope_height = parameter(params, 'CLIMB_SLOPE_HGT') + self.rtl_altitude = parameter(params, 'RTL_ALTITUDE') + self.rtl_radius = parameter(params, 'RTL_RADIUS') + self.rally_limit = parameter(params, 'RALLY_LIMIT_KM') * 1000.0 + self.rally_incl_home = parameter(params, 'RALLY_INCL_HOME') > 0 + self.quadplane = parameter(params, 'Q_ENABLE') > 0 + self.q_options = int(parameter(params, 'Q_OPTIONS')) + self.immediate_climb = bool(int(parameter(params, 'FLIGHT_OPTIONS')) & + FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO) + # AP_AHRS::using_airspeed_sensor, so far as the parameters say. + # ARSPD_USE 2, for a glider with its sensor behind the propeller, is + # not used while the throttle is running (AP_Airspeed::use): a + # takeoff is taken to run it, as all but a glider's tow or winch + # launch does, so such a sensor is not one to fly the climb out by + self.airspeed_sensor = (parameter(params, 'ARSPD_TYPE') != 0 and + parameter(params, 'ARSPD_USE') == 1) + # whether the aircraft is returning to launch, rather than in AUTO + self.rtl = False + self.q_rtl_mode = int(parameter(params, 'Q_RTL_MODE')) + self.q_rtl_alt = parameter(params, 'Q_RTL_ALT') + self.q_approach_radius = parameter(params, 'Q_FW_LND_APR_RAD') + self.q_trans_decel = parameter(params, 'Q_TRANS_DECEL') + # the course a VTOL landing approach is flown on + if approach is None: + approach = STILL_AIR_APPROACH + self.approach = math.radians(approach) + # the jumps followed so far, by index + self.jumps_taken = set() + # whether the mission has come round to a jump repeated for ever, + # which it never gets past + self.looping = False + self.home = self.local(home[0], home[1]) + self.home_amsl = home[2] + self.rally = [(self.local(lat, lon), amsl) + for (lat, lon, amsl) in (rally or [])] + self.items = items + # the course a VTOL takeoff transitions on, where there is + # something to go on: the way the aircraft points + self.takeoff_heading = heading + # the course a fixed-wing takeoff was flown on, where a flight of it + # has been measured; nothing says it beforehand + self.takeoff_course = takeoff_course + # where the aircraft is when the mission starts: home, unless it + # started somewhere else + self.position = self.home + self.amsl = home[2] + if start is not None: + self.position = self.local(start[0], start[1]) + if start[2] is not None: + self.amsl = start[2] + self.yaw = 0.0 + self.roll = 0.0 + self.time = 0.0 + self.points = [] + self.last_point = None + # what set_next_WP() keeps + self.prev_wp = (self.position, self.amsl) + self.next_wp = (self.position, self.amsl) + self.crosstrack = False + self.next_wp_crosstrack = False + self.offset_altitude = 0.0 + self.target_amsl = self.amsl + # QRTL's approach: how far above where it returns to the aircraft + # was, and how far from it, when the approach began, once it has + self.qrtl = False + self.qrtl_start = None + + def local(self, lat, lon): + return ((lat - self.origin[0]) * 1.0e7 * LOCATION_SCALING_FACTOR, + wrap_longitude(lon - self.origin[1]) * 1.0e7 * + LOCATION_SCALING_FACTOR * self.lon_scale) + + def latlon(self, point): + '''back from local(). The longitude is left unwrapped, so a path + across the antimeridian stays continuous, as the 3D map's rings do''' + return (self.origin[0] + point[0] / (1.0e7 * LOCATION_SCALING_FACTOR), + self.origin[1] + point[1] / (1.0e7 * LOCATION_SCALING_FACTOR * + self.lon_scale)) + + def location(self, item): + '''an item's location and altitude. An item with no position of its + own is flown where the aircraft is, and one with no altitude at the + one it has''' + (command, lat, lon, amsl, params) = item + if lat == 0 and lon == 0: + point = self.position + else: + point = self.local(lat, lon) + if amsl is None: + amsl = self.amsl + return (point, amsl) + + def jump_target(self, index): + '''the index a DO_JUMP or DO_JUMP_TAG at index goes to, or None if it + goes nowhere there is''' + (command, lat, lon, amsl, params) = self.items[index] + target = int(params[0] or 0) + if command == MAV_CMD_DO_JUMP_TAG: + for (i, item) in enumerate(self.items): + if item[0] == MAV_CMD_JUMP_TAG and int(item[4][0] or 0) == target: + return i + return None + # items are numbered from home, which is not among them + if target < 1 or target > len(self.items): + return None + return target - 1 + + def jumps(self, index): + '''whether the jump at index is followed. AP_Mission follows one + its repeat count, or for ever with a count of -1; drawn, each is + followed once, so the path goes round a loop the once''' + repeats = int(self.items[index][4][1] or 0) + return repeats != 0 and index not in self.jumps_taken + + def loops_forever(self, index): + '''whether the jump at index has already been followed and is + followed for ever, so that the mission never gets past it''' + repeats = int(self.items[index][4][1] or 0) + return repeats < 0 and index in self.jumps_taken + + def next_nav_index(self, index, take=False, passed=None): + '''AP_Mission::get_next_nav_cmd: the index of the next item after + index which ArduPlane navigates to, following jumps, or None. take + counts the jumps as followed, and passed collects the indexes of the + other commands on the way''' + i = index + 1 + followed = set() + while i < len(self.items): + command = self.items[i][0] + if command in (mavutil.mavlink.MAV_CMD_DO_JUMP, MAV_CMD_DO_JUMP_TAG): + if i in followed: + # round in a circle without navigating anywhere + return None + if self.loops_forever(i): + # the aircraft goes round this loop for ever, and has + # been drawn going round it once + self.looping = True + return None + if self.jumps(i): + target = self.jump_target(i) + if target is None: + return None + followed.add(i) + if take: + self.jumps_taken.add(i) + i = target + continue + i += 1 + continue + if command in SKIPPED_COMMANDS: + i += 1 + continue + if (command in LOITER_COMMANDS or command in WAYPOINT_COMMANDS or + command in TAKEOFF_COMMANDS or + command < mavutil.mavlink.MAV_CMD_NAV_LAST or + command in LATE_NAV_COMMANDS): + # including a navigation command this does not know how to fly + return i + if passed is not None: + passed.append(i) + i += 1 + return None + + def add_point(self, force=False): + if self.last_point is not None and not force: + (point, yaw) = self.last_point + if (norm(minus(self.position, point)) < POINT_SPACING and + abs(wrap_pi(self.yaw - yaw)) < POINT_TURN): + return + self.last_point = (self.position, self.yaw) + (lat, lon) = self.latlon(self.position) + self.points.append((lat, lon, self.amsl)) + + def set_next_wp(self, location): + '''Plane::set_next_WP''' + if self.next_wp_crosstrack: + self.prev_wp = self.next_wp + self.crosstrack = True + else: + self.prev_wp = (self.position, self.amsl) + self.next_wp_crosstrack = True + self.crosstrack = False + self.next_wp = location + if path_proportion(self.position, self.prev_wp[0], self.next_wp[0]) >= 1: + self.prev_wp = (self.position, self.amsl) + # Plane::setup_alt_slope: RTL only slopes down to where it is going, + # climbing to it at once, as AUTO does with IMMEDIATE_CLIMB_IN_AUTO + above = self.amsl > self.next_wp[1] + if not above and (self.rtl or self.immediate_climb): + self.offset_altitude = 0.0 + return + # Plane::set_offset_altitude_location + self.offset_altitude = self.next_wp[1] - self.prev_wp[1] + if (self.alt_slope_min <= 0 or + abs(self.offset_altitude) < self.alt_slope_min): + self.offset_altitude = 0.0 + + def update_target_altitude(self): + '''Mode::update_target_altitude, for AUTO away from landing''' + if self.qrtl: + self.update_qrtl_target_altitude() + return + (end, end_amsl) = self.next_wp + (start, start_amsl) = self.prev_wp + if self.l1.circling: + self.target_amsl = end_amsl + elif (self.offset_altitude != 0 and + path_proportion(self.position, start, end) < 1): + if (self.offset_altitude > 0 and + self.amsl - self.home_amsl < self.climb_slope_height): + # a full rate climb up to CLIMB_SLOPE_HGT + self.target_amsl = end_amsl + return + proportion = path_proportion(self.position, start, end) + proportion = min(max(1.0 - proportion, 0.0), 1.0) + target = end_amsl - self.offset_altitude * proportion + self.target_amsl = min(max(target, min(start_amsl, end_amsl)), + max(start_amsl, end_amsl)) + else: + self.target_amsl = end_amsl + + def update_qrtl_target_altitude(self): + '''ModeQRTL::update_target_altitude, for an aircraft which entered + QRTL flying as a plane, whose position controller is then on its + approach: it comes in at RTL_ALTITUDE, or down from where it started + towards that where the difference is worth a slope, and drops to + Q_RTL_ALT over the last stretch, which is as long as descending at + 0.6 of TECS_SINK_MAX at AIRSPEED_CRUISE takes. + + This is ArduPilot master's, since bd9c0285fc. Plane 4.7 and + before have no slope: they go to RTL_ALTITUDE at once, so a return + entered well above it is flown lower than drawn here until the + last stretch''' + (end, end_amsl) = self.next_wp + radius = max(abs(self.loiter_radius), abs(self.rtl_radius)) + rtl_alt_delta = max(0.0, self.rtl_altitude - self.q_rtl_alt) + sink_time = rtl_alt_delta / max(0.6 * self.sink, 1.0) + sink_dist = self.cruise * sink_time + dist = norm(minus(end, self.position)) + rad_min = 2 * radius + rad_max = 20 * radius + reached = max(rad_min, min(rad_max, rad_min + sink_dist)) + if self.qrtl_start is None: + self.qrtl_start = (self.amsl - end_amsl, dist) + (start_delta, start_dist) = self.qrtl_start + if dist > reached: + if (self.alt_slope_min > 0 and + start_delta - rtl_alt_delta >= self.alt_slope_min and + start_dist > reached): + alt = linear_interpolate(rtl_alt_delta, start_delta, dist, + reached, start_dist) + # never back up to the slope, once below it + alt = min(alt, max(rtl_alt_delta, self.amsl - end_amsl)) + else: + alt = rtl_alt_delta + else: + alt = linear_interpolate(0.0, rtl_alt_delta, dist, rad_min, + reached) + self.target_amsl = end_amsl + alt + + def fly(self, lateral_acceleration, dt, climb=None): + '''move the aircraft on by dt, banking towards the controller's + demand, and climbing or descending towards the target altitude, or + at the climb rate given''' + demand = math.atan(lateral_acceleration / GRAVITY) + demand = min(max(demand, -self.roll_limit), self.roll_limit) + self.roll += (demand - self.roll) * (1.0 - math.exp(-dt / self.roll_tconst)) + speed = self.speed() + self.yaw = wrap_pi(self.yaw + GRAVITY * math.tan(self.roll) / speed * dt) + self.position = (self.position[0] + speed * math.cos(self.yaw) * dt, + self.position[1] + speed * math.sin(self.yaw) * dt) + if climb is None: + rate = (self.target_amsl - self.amsl) / self.alt_tconst + climb = min(max(rate, -self.sink), self.climb) + self.amsl += climb * dt + self.time += dt + self.add_point() + + def speed(self): + return self.airspeed * eas2tas(self.amsl) + + def velocity(self): + speed = self.speed() + return (speed * math.cos(self.yaw), speed * math.sin(self.yaw)) + + def turn_angle(self, index): + '''Plane::setup_turn_angle: the turn at the end of this leg''' + following = self.next_nav_index(index) + if following is None: + return 90.0 + (end, _) = self.next_wp + (after, _) = self.location(self.items[following]) + if norm(minus(after, end)) < 1.0e-6: + return 90.0 + leg = math.atan2(end[1] - self.prev_wp[0][1], end[0] - self.prev_wp[0][0]) + course = math.atan2(after[1] - end[1], after[0] - end[0]) + return math.degrees(wrap_pi(course - leg)) + + def time_limit(self, extra=0.0): + (end, _) = self.next_wp + leg = norm(minus(end, self.position)) + limit = self.time + 120.0 + LEG_ALLOWANCE * leg / self.speed() + extra + return min(limit, MAX_FLIGHT_TIME) + + def rtl_location(self, home_amsl=None): + '''AP_Rally::calc_best_rally_or_home_location: home, at home_amsl + where that is given, and otherwise RTL_ALTITUDE above it, or at the + altitude the aircraft is at if that is negative; or the rally point + nearest the aircraft, at its own altitude, where it is within + RALLY_LIMIT_KM and, with RALLY_INCL_HOME set, nearer than home. A + rally point whose altitude is not known raises, since nobody can say + how high the aircraft flies to it''' + if home_amsl is not None: + best = (self.home, home_amsl) + elif self.rtl_altitude < 0: + best = (self.home, self.amsl) + else: + best = (self.home, self.home_amsl + self.rtl_altitude) + nearest = None + for (point, amsl) in self.rally: + distance = norm(minus(point, self.position)) + if nearest is None or distance < nearest[0]: + nearest = (distance, point, amsl) + if nearest is None: + return best + (distance, point, amsl) = nearest + if self.rally_limit > 0 and distance > self.rally_limit: + return best + if (self.rally_incl_home and + distance >= norm(minus(self.home, self.position))): + return best + if amsl is None: + # the caller could not work out how high the point is -- terrain + # it does not have, most likely -- so nobody can say how high + # the aircraft flies its return, and it is not drawn flown + raise ValueError('rally point altitude unknown') + return (point, amsl) + + def fly_waypoint(self, index): + '''Plane::verify_nav_wp. False if the aircraft never gets there''' + (command, lat, lon, amsl, params) = self.items[index] + acceptance = 0.0 + passby = 0.0 + if command == mavutil.mavlink.MAV_CMD_NAV_WAYPOINT: + acceptance = min(max(params[1] or 0.0, 0.0), 255.0) + passby = min(max(params[2] or 0.0, 0.0), 255.0) + final = command != mavutil.mavlink.MAV_CMD_NAV_WAYPOINT + turn_angle = self.turn_angle(index) + limit = self.time_limit() + while self.time < limit: + (start, _) = self.prev_wp + (end, _) = self.next_wp + if passby > 0 and norm(minus(end, start)) > 0: + along = unit(minus(end, start)) + end = (end[0] + along[0] * passby, end[1] + along[1] * passby) + track_start = start if self.crosstrack else self.position + self.l1.update_waypoint(self.position, self.velocity(), self.yaw, + track_start, end, NAV_PERIOD) + self.update_target_altitude() + if (not final and self.wp_max_radius > 0 and + norm(minus(self.next_wp[0], self.position)) > + self.wp_max_radius): + # WP_MAX_RADIUS: not reached until this close, however far + # past it the aircraft has flown. Having flown past, it + # comes back round for it along a track from where it is + if (passby == 0 and + path_proportion(self.position, start, end) >= 1): + self.prev_wp = (self.position, self.amsl) + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + continue + if final: + accept = 0.0 + elif acceptance > 0: + accept = acceptance + elif passby == 0: + accept = self.l1.turn_distance(self.wp_radius, self.amsl, + turn_angle) + else: + accept = 0.0 + if (norm(minus(end, self.position)) <= max(accept, 1.0) or + path_proportion(self.position, start, end) >= 1): + return True + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + return False + + def fly_loiter(self, index): + '''Plane::verify_loiter_*, flying update_loiter() until the item is + done. False if it never is''' + mavlink = mavutil.mavlink + (command, lat, lon, amsl, params) = self.items[index] + if command == mavlink.MAV_CMD_NAV_LOITER_TIME: + requested = 0.0 + ccw = (params[2] or 0.0) < 0 + elif command == mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + requested = params[1] or 0.0 + ccw = requested < 0 + else: + requested = params[2] or 0.0 + ccw = requested < 0 + radius = abs(requested) + direction = -1 if ccw else 1 + if radius <= 1: + # Plane::update_loiter: WP_LOITER_RAD, turned the way its sign says + # unless the item asks for counter-clockwise + radius = default_loiter_radius(self.loiter_radius) + if not ccw: + direction = -1 if self.loiter_radius < 0 else 1 + if command == mavlink.MAV_CMD_NAV_LOITER_TURNS and radius > 255: + radius = min(255.0, radius * 0.1) * 10.0 + exit_from_here = not mp_util.mission_crosstracks_from_centre(command, + params) + # AP_Mission keeps whole turns in a byte, and a turn less than one + # in 256ths of one + turns = min(max(params[0] or 0.0, 0.0), 255.0) + if turns >= 1: + turns = float(int(turns)) + else: + turns = int(turns * 256) / 256.0 + loiter_time = max(params[0] or 0.0, 0.0) + started = False + start_time = None + swept = 0.0 + old_bearing = None + reached_alt = False + unable = False + lap_check = 3 * 360.0 + lap_alt = self.amsl + lining_up = False + following = self.next_nav_index(index) + seconds_per_lap = 2 * math.pi * radius * 1.5 / max(self.speed(), 1.0) + extra = MAX_LAPS * seconds_per_lap + if command == mavlink.MAV_CMD_NAV_LOITER_TIME: + extra += loiter_time + elif command == mavlink.MAV_CMD_NAV_LOITER_TURNS: + extra += turns * seconds_per_lap + elif command == mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + (_, centre_amsl) = self.next_wp + rate = self.climb if centre_amsl > self.amsl else self.sink + extra += 1.5 * abs(centre_amsl - self.amsl) / max(rate, 0.1) + limit = self.time_limit(extra) + while self.time < limit: + (centre, centre_amsl) = self.next_wp + (start, _) = self.prev_wp + flown = self.l1.loiter_radius(radius, self.amsl, self.airspeed) + if (not started and self.crosstrack and + norm(minus(centre, self.position)) > 3 * flown): + self.l1.update_waypoint(self.position, self.velocity(), self.yaw, + start, centre, NAV_PERIOD) + else: + self.l1.update_loiter(self.position, self.velocity(), self.yaw, + centre, flown, direction) + if not started and (self.l1.circling or + path_proportion(self.position, start, centre) > 1): + started = True + start_time = self.time + # Plane::loiter_angle_update + reached = self.l1.circling + if swept == 0 and not reached: + delta = 0.0 + elif swept == 0: + delta = 0.01 + lap_alt = self.amsl + lap_check = 3 * 360.0 + else: + delta = math.degrees(wrap_pi(self.l1.target_bearing - old_bearing)) + old_bearing = self.l1.target_bearing + swept += delta * direction + if reached and abs(self.amsl - centre_amsl) < 5.0: + reached_alt = True + elif not reached_alt and abs(swept) >= lap_check: + unable = abs(self.amsl - lap_alt) < 5.0 + lap_alt = self.amsl + lap_check += 3 * 360.0 + self.update_target_altitude() + if not lining_up: + if command == mavlink.MAV_CMD_NAV_LOITER_UNLIM: + if reached and swept >= 360.0: + return False + elif command == mavlink.MAV_CMD_NAV_LOITER_TURNS: + lining_up = reached and swept > turns * 360.0 and swept > 0.01 + elif command == mavlink.MAV_CMD_NAV_LOITER_TIME: + lining_up = (start_time is not None and swept > 0.01 and + self.time - start_time > loiter_time) + else: + lining_up = abs(swept) > 0.01 and (reached_alt or unable) + if lining_up: + swept = 0.0 + old_bearing = self.l1.target_bearing + if lining_up and (reached or + command != mavlink.MAV_CMD_NAV_LOITER_TURNS): + if following is None: + return True + if self.lined_up(centre, flown, direction, swept, + self.lineup_target(self.items[following], + centre)): + if exit_from_here: + self.next_wp = (self.position, centre_amsl) + return True + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + return False + + def lineup_target(self, item, centre): + '''where Plane::verify_loiter_heading lines a loiter's exit up with: + the next item's location as the mission holds it, which for one with + no position of its own is 0, 0 -- only starting it puts it where the + aircraft is -- as Location::get_distance_NE measures it from the + loiter's centre, the longitude scaled at the latitude between them''' + (command, lat, lon, amsl, params) = item + (clat, clon) = self.latlon(centre) + scale = max(math.cos(math.radians((lat + clat) / 2.0)), 0.01) + return (centre[0] + (lat - clat) * 1.0e7 * LOCATION_SCALING_FACTOR, + centre[1] + wrap_longitude(lon - clon) * 1.0e7 * + LOCATION_SCALING_FACTOR * scale) + + def lined_up(self, centre, radius, direction, swept, target): + '''ModeLoiter::isHeadingLinedUp''' + if radius <= 0: + return True + projected = minus(self.position, centre) + if norm(projected) < 1.0e-6: + projected = (math.cos(self.yaw), math.sin(self.yaw)) + projected = unit(projected) + projected = (projected[0] * radius, projected[1] * radius) + to_target = minus(target, centre) + target_distance = norm(to_target) + if target_distance <= 0: + return True + if target_distance >= radius: + bearing = math.atan2(to_target[1] - projected[1], + to_target[0] - projected[0]) + else: + chord = radius - target_distance + segment = 2.0 * math.asin(chord / (2.0 * radius)) + bearing = wrap_pi(math.atan2(to_target[1], to_target[0]) + + (math.pi / 2 - segment) * direction) + heading = wrap_pi(math.atan2(projected[1], projected[0]) + + math.pi / 2 * direction) + acceptance = 10.0 + 10.0 * int(abs(swept) / 360.0) + return abs(math.degrees(wrap_pi(bearing - heading))) <= acceptance + + def vtol_takeoff(self, command): + '''QuadPlane::is_vtol_takeoff''' + return vtol_takeoff(command, self.quadplane, self.q_options) + + def face(self, index): + '''turn to face the item at index, if it is somewhere else''' + if index is None: + return + (point, _) = self.location(self.items[index]) + if norm(minus(point, self.position)) > 1.0: + self.yaw = math.atan2(point[1] - self.position[1], + point[0] - self.position[0]) + + def fly_takeoff(self, index): + '''climb out to the takeoff altitude. A VTOL takeoff climbs straight + up where the aircraft is, and sets off the way it points: the heading + the caller has for it, and otherwise, as a guess, towards where the + mission goes next. A fixed-wing one climbs away at the item's pitch, + or faster where TECS has an airspeed sensor to fly by, until it is + past the takeoff altitude. It holds the ground course it had once + it got moving (Plane::verify_takeoff), which is wherever it was + launched or pointed down the runway: the course the caller measured + a flight of it on, where it has one, and otherwise towards where the + mission goes next, which is at least where it heads once it is up''' + (command, lat, lon, amsl, params) = self.items[index] + (point, target) = self.location(self.items[index]) + if self.vtol_takeoff(command): + # it transitions the way it is pointing, which is the way it + # was pointing on the ground, where that is known + self.amsl = max(self.amsl, target) + if self.takeoff_heading is not None: + self.yaw = wrap_pi(math.radians(self.takeoff_heading)) + else: + self.face(self.next_nav_index(index)) + self.add_point(force=True) + else: + if self.takeoff_course is not None: + self.yaw = wrap_pi(math.radians(self.takeoff_course)) + else: + self.face(self.next_nav_index(index)) + # Plane::takeoff_calc_pitch: no lower than the item's pitch, + # which is all it climbs at without an airspeed sensor for TECS + # to fly by + pitch = params[0] if params[0] > 0 else DEFAULT_TAKEOFF_PITCH + climb = self.speed() * math.tan(math.radians(pitch)) + if self.airspeed_sensor: + climb = max(climb, self.climb) + limit = min(self.time + 600.0, MAX_FLIGHT_TIME) + while self.amsl < target and self.time < limit: + self.target_amsl = target + self.fly(0.0, NAV_PERIOD, climb=climb) + # Plane::verify_takeoff: no crosstracking the leg after a takeoff + self.next_wp = self.prev_wp = (self.position, self.amsl) + self.next_wp_crosstrack = False + self.target_amsl = self.amsl + return True + + def change_speed(self, params): + '''Plane::do_change_speed, for the airspeed it flies at. Only an + airspeed change, of -2 for the cruise airspeed or one within + AIRSPEED_MIN and AIRSPEED_MAX, changes it: a groundspeed change sets + a minimum ground speed, which there is no wind here to call on''' + (speed_type, speed) = (params[0] or 0.0, params[1] or 0.0) + if int(speed_type) != 0: + return + if speed == -2: + self.airspeed = self.cruise + elif self.airspeed_min <= speed <= self.airspeed_max: + self.airspeed = speed + + def run(self): + '''fly the mission. Returns the path flown, or None where it cannot + be flown through to its end''' + mavlink = mavutil.mavlink + passed = [] + index = self.next_nav_index(-1, take=True, passed=passed) + if index is None: + return None + self.face(index) + self.add_point(force=True) + while index is not None: + if self.time > MAX_FLIGHT_TIME: + return None + for i in passed: + if self.items[i][0] == mavlink.MAV_CMD_DO_CHANGE_SPEED: + self.change_speed(self.items[i][4]) + passed = [] + command = self.items[index][0] + if command in TAKEOFF_COMMANDS: + self.fly_takeoff(index) + index = self.next_nav_index(index, take=True, passed=passed) + continue + if command == mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH: + return self.fly_rtl(index) + if command in WAYPOINT_COMMANDS or command in LOITER_COMMANDS: + location = self.location(self.items[index]) + else: + return None + self.set_next_wp(location) + if spiral_approach(command, self.items[index][4], self.quadplane, + self.q_options): + if not self.fly_vtol_approach(location): + return None + break + if command == mavlink.MAV_CMD_NAV_VTOL_LAND and self.quadplane: + # QuadPlane::do_vtol_land: flown in at the altitude the + # aircraft is at, which its position controller takes it + # down from once there. How far down is the ground's, so + # the path ends above the landing at that altitude + self.next_wp = (self.next_wp[0], self.amsl) + if command in LOITER_COMMANDS: + finished = self.fly_loiter(index) + if not finished and command == mavlink.MAV_CMD_NAV_LOITER_UNLIM: + break + else: + finished = self.fly_waypoint(index) + if not finished: + return None + if command in (mavlink.MAV_CMD_NAV_LAND, + mavlink.MAV_CMD_NAV_VTOL_LAND): + break + index = self.next_nav_index(index, take=True, passed=passed) + else: + if not self.looping: + # AP_Mission has no next command, so the mission is complete + # and Plane::exit_mission_callback changes mode to RTL + for i in passed: + if self.items[i][0] == mavlink.MAV_CMD_DO_CHANGE_SPEED: + self.change_speed(self.items[i][4]) + self.items.append((mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, 0, None, (0.0, 0.0, 0.0, 0.0))) + return self.fly_rtl(len(self.items) - 1) + self.add_point(force=True) + return self.points + + def loiter_step(self, centre, radius, direction): + '''fly on by a step, circling centre''' + flown = self.l1.loiter_radius(radius, self.amsl, self.airspeed) + self.l1.update_loiter(self.position, self.velocity(), self.yaw, + centre, flown, direction) + self.update_target_altitude() + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + + def stopping_distance(self): + '''QuadPlane::stopping_distance_m''' + return self.speed() ** 2 / (2 * max(self.q_trans_decel, 0.1)) + + def fly_vtol_approach(self, location, rtl=False): + '''Plane::verify_landing_vtol_approach: circle to the altitude of + location, at Q_FW_LND_APR_RAD -- WP_LOITER_RAD where that is zero -- + the way its sign says; break out of the circle square to the + approach course; fly the approach line through location, and land + there once near enough and lined up. A return to launch, with rtl, + first circles where it returns to until it is on the circle, and + then goes down to Q_RTL_ALT. The path ends above the landing, at + the altitude it was approached at. False if the aircraft never + gets there''' + mavlink = mavutil.mavlink + radius = self.q_approach_radius or self.loiter_radius + direction = -1 if radius < 0 else 1 + radius = abs(radius) + (centre, _) = location + lap = 2 * math.pi * max(radius, 1.0) * 1.5 / max(self.speed(), 1.0) + # there is no circling for ever before the approach + self.crosstrack = False + if rtl: + limit = self.time_limit(MAX_LAPS * lap) + while not self.l1.circling: + if self.time >= limit: + return False + self.loiter_step(centre, radius, direction) + # do_RTL(): on to where a return goes, at Q_RTL_ALT above home + self.next_wp_crosstrack = False + location = self.rtl_location(self.home_amsl + self.q_rtl_alt) + (centre, _) = location + self.set_next_wp(location) + self.crosstrack = False + # LOITER_TO_ALT, flown as a loiter to altitude which nothing follows + (lat, lon) = self.latlon(centre) + self.items.append((mavlink.MAV_CMD_NAV_LOITER_TO_ALT, + lat, lon, location[1], + (0.0, direction * radius, 0.0, 0.0))) + if not self.fly_loiter(len(self.items) - 1): + return False + limit = self.time_limit(MAX_LAPS * lap) + # ENSURE_RADIUS: out onto the circle, if inside it + while True: + distance = norm(minus(centre, self.position)) + if not (distance < radius and abs(distance - radius) > 5.0): + break + if self.time >= limit: + return False + self.loiter_step(centre, radius, direction) + # WAIT_FOR_BREAKOUT: round until heading square to the approach + breakout = wrap_pi(self.approach + + math.radians(270.0 if direction > 0 else 90.0)) + while abs(wrap_pi(self.yaw - breakout)) >= math.radians(5.0): + if self.time >= limit: + return False + self.loiter_step(centre, radius, direction) + # APPROACH_LINE: the line 1km either side of the landing + self.set_next_wp(location) + along = (math.cos(self.approach), math.sin(self.approach)) + + def back(distance): + return (centre[0] - along[0] * distance, + centre[1] - along[1] * distance) + (start, end) = (back(1000.0), back(-1000.0)) + breakout_point = back(radius) + limit = self.time_limit() + while True: + self.l1.update_waypoint(self.position, self.velocity(), self.yaw, + start, end, NAV_PERIOD) + self.update_target_altitude() + past = path_proportion(self.position, start, + back(self.stopping_distance())) >= 1 + half_radius = path_proportion(self.position, breakout_point, + centre) > 0.5 + to_centre = minus(centre, self.position) + velocity = self.velocity() + lined_up = True + if norm(to_centre) > 0: + cosine = (dot(velocity, to_centre) / + (norm(velocity) * norm(to_centre))) + lined_up = math.degrees( + math.acos(min(max(cosine, -1.0), 1.0))) < 30.0 + if past and (lined_up or half_radius): + break + if self.time >= limit: + return False + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + # VTOL_LANDING: QuadPlane::do_vtol_land, whose position controller + # takes the aircraft from here to where it lands, drawn straight + # there. How far down it goes is the ground's, which is not known + # here, so the path ends above the landing at the approach altitude + self.position = centre + self.amsl = location[1] + self.add_point(force=True) + return True + + def fly_rtl(self, index): + '''a return to launch, which ArduPlane flies in RTL mode, or QRTL + where Q_RTL_MODE has a QuadPlane do so. It ends the flight: the + path returned is the one flown''' + mavlink = mavutil.mavlink + # Plane::do_RTL: flown from wherever the aircraft is + self.rtl = True + self.next_wp_crosstrack = False + qrtl = self.quadplane and self.q_rtl_mode in (Q_RTL_SWITCH_QRTL, + Q_RTL_QRTL_ALWAYS) + if self.quadplane and self.q_rtl_mode == Q_RTL_QRTL_ALWAYS: + # ModeRTL::_enter goes straight to QRTL, which returns at + # Q_RTL_ALT + location = self.rtl_location(self.home_amsl + self.q_rtl_alt) + else: + location = self.rtl_location() + self.set_next_wp(location) + if self.quadplane and self.q_rtl_mode == Q_RTL_VTOL_APPROACH: + if not self.fly_vtol_approach(location, rtl=True): + return None + elif qrtl: + # with Q_RTL_MODE 1 the aircraft flies at the point in RTL, along + # a track from where it turned for it + # (Plane::update_loiter_update_nav), and QRTL takes it the rest + # of the way; QRTL, which 3 goes to at once, steers for it from + # wherever the aircraft is, do_RTL() having left no track + # (QuadPlane::vtol_position_controller) + self.crosstrack = self.q_rtl_mode == Q_RTL_SWITCH_QRTL + # QRTL comes in on its own altitude profile, which Q_RTL_MODE 1 + # only switches to near home, where the path ends + self.qrtl = self.q_rtl_mode == Q_RTL_QRTL_ALWAYS + if not self.fly_waypoint(index): + return None + else: + # ModeRTL::update: the aircraft circles where it returns to, at + # RTL_RADIUS -- WP_LOITER_RAD where that is zero -- the way its + # sign says. It is flown by update_loiter() from the start, + # with no track to follow there since do_RTL() leaves none, so + # it joins the circle rather than flying over its middle + self.items.append((mavlink.MAV_CMD_NAV_LOITER_UNLIM, + location[0][0], location[0][1], location[1], + (0.0, 0.0, self.rtl_radius, 0.0))) + self.fly_loiter(len(self.items) - 1) + self.add_point(force=True) + return self.points + + +def vtol_takeoff(command, quadplane, q_options): + '''QuadPlane::is_vtol_takeoff: whether command is flown as a VTOL + takeoff, straight up, rather than as a fixed-wing one''' + if command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF: + return True + return (command == mavutil.mavlink.MAV_CMD_NAV_TAKEOFF and quadplane and + not q_options & Q_OPTION_ALLOW_FW_TAKEOFF) + + +def takeoff_is_vtol(command, params): + '''whether command is flown as a VTOL takeoff by a vehicle with these + parameters''' + return vtol_takeoff(command, parameter(params, 'Q_ENABLE') > 0, + int(parameter(params, 'Q_OPTIONS'))) + + +def spiral_approach(command, params, quadplane, q_options): + '''QuadPlane::landing_with_fixed_wing_spiral_approach: whether command, + with params, is a VTOL landing flown in on a fixed-wing approach''' + return (quadplane and command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND and + (bool(q_options & Q_OPTION_MISSION_LAND_FW_APPROACH) or + params[0] == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH)) + + +def uses_vtol_approach(items, params): + '''whether flying items takes the course of a VTOL landing approach, + which is into the wind: a landing flown in on a fixed-wing approach, or + a return to launch which Q_RTL_MODE has approach its landing so -- the + one an item asks for, or the one a mission which runs out flies''' + quadplane = parameter(params, 'Q_ENABLE') > 0 + q_options = int(parameter(params, 'Q_OPTIONS')) + q_rtl_mode = int(parameter(params, 'Q_RTL_MODE')) + returning = quadplane and q_rtl_mode == Q_RTL_VTOL_APPROACH + runs_out = True + walk = MissionFlight((0.0, 0.0), (0.0, 0.0, 0.0), list(items), {}) + try: + index = walk.next_nav_index(-1, take=True) + while index is not None: + (command, _, _, _, item_params) = walk.items[index] + if spiral_approach(command, item_params, quadplane, q_options): + return True + if (returning and + command == mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH): + return True + if command in ENDS_THE_FLIGHT: + # the flight ends there rather than running out of mission + runs_out = False + break + index = walk.next_nav_index(index, take=True) + except (ValueError, ZeroDivisionError, OverflowError): + # an item nothing can make sense of, as mission_track() takes it: + # there is no path to draw for this mission, wind or no wind + return False + if walk.looping: + # round a loop for ever, so the mission never runs out either + runs_out = False + return returning and runs_out + + +def is_navigation_command(command): + '''whether ArduPlane flies command, rather than just doing it''' + return (command < mavutil.mavlink.MAV_CMD_NAV_LAST or + command in LATE_NAV_COMMANDS) + + +def positionless_amsl(alt, frame, home_amsl): + '''the AMSL altitude an item with no position of its own is flown at, + or None for the altitude the aircraft is at. Location::sanitize() puts + such an item where the aircraft is, but keeps its altitude unless that + is a relative 0; a terrain-relative one needs the terrain under wherever + that is, which is not known here''' + if alt is None: + return None + if frame in (0, 5): + return alt + if alt == 0 or frame not in (3, 6) or home_amsl is None: + return None + return home_amsl + alt + + +# Location::AltFrame, as a rally point's flags carry it +RALLY_ALT_ABSOLUTE = 0 +RALLY_ALT_ABOVE_HOME = 1 +RALLY_ALT_ABOVE_ORIGIN = 2 +RALLY_ALT_ABOVE_TERRAIN = 3 + + +def rally_alt_frame(flags): + '''the frame a rally point's altitude is in. + AP_Rally::rally_location_to_location(): above home, unless the flags say + the frame they carry is a valid one''' + if int(flags or 0) & (1 << 2): + return (int(flags) >> 3) & 3 + return RALLY_ALT_ABOVE_HOME + + +def rally_alt_is_fixed(flags): + '''whether a rally point is at an altitude of its own rather than one + which moves with home''' + return rally_alt_frame(flags) != RALLY_ALT_ABOVE_HOME + + +def rally_amsl(alt, flags, home_amsl, origin_amsl=None, terrain_amsl=None): + '''the AMSL altitude of a rally point, or None where it cannot be told. + + Location::get_alt_cm() measures the frame the flags carry from its own + datum: home, the EKF origin, or the terrain below the point, which the + caller looks up where it can''' + if alt is None: + return None + frame = rally_alt_frame(flags) + if frame == RALLY_ALT_ABSOLUTE: + return float(alt) + datum = {RALLY_ALT_ABOVE_HOME: home_amsl, + RALLY_ALT_ABOVE_ORIGIN: origin_amsl, + RALLY_ALT_ABOVE_TERRAIN: terrain_amsl}[frame] + if datum is None: + return None + return datum + alt + + +def default_loiter_radius(wp_loiter_rad): + '''the radius of a loiter which gives none of its own: WP_LOITER_RAD, + or LOITER_RADIUS_DEFAULT where that is no radius at all + (Plane::update_loiter)''' + if abs(wp_loiter_rad) <= 1: + return 60.0 + return abs(wp_loiter_rad) + + +def linear_interpolate(low_output, high_output, value, low, high): + '''AP_Math's linear_interpolate: output from low_output at low to + high_output at high, held at either end beyond them''' + if low > high: + (low, high) = (high, low) + (low_output, high_output) = (high_output, low_output) + if value <= low: + return low_output + if value >= high: + return high_output + p = (value - low) / (high - low) + return low_output + p * (high_output - low_output) + + +def least_flight_time(home, items, params, start=None): + '''the least time flying the mission could take: straight from each + item it navigates to on to the next, in the order the flight takes them + -- following jumps as it does -- at the fastest airspeed it may fly, and + loitering for the times asked. start is where the aircraft is when the + mission starts, where that is not home''' + mavlink = mavutil.mavlink + speed = max(parameter(params, 'AIRSPEED_CRUISE'), + parameter(params, 'AIRSPEED_MAX'), 1.0) + walk = MissionFlight((home[0], home[1]), home, list(items), {}) + seconds = 0.0 + (lat, lon) = (start[0], start[1]) if start else (home[0], home[1]) + index = walk.next_nav_index(-1, take=True) + while index is not None: + (command, item_lat, item_lon, amsl, item_params) = walk.items[index] + if command == mavlink.MAV_CMD_NAV_LOITER_TIME: + seconds += max(item_params[0] or 0.0, 0.0) + if not (item_lat == 0 and item_lon == 0): + seconds += mp_util.gps_distance(lat, lon, item_lat, item_lon) / speed + (lat, lon) = (item_lat, item_lon) + if command in (mavlink.MAV_CMD_NAV_LAND, mavlink.MAV_CMD_NAV_VTOL_LAND, + mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + mavlink.MAV_CMD_NAV_LOITER_UNLIM): + break + index = walk.next_nav_index(index, take=True) + return seconds + + +def first_navigation_item(items): + '''the number of the first item in items -- numbered from 1, as + mission_track() takes them -- which ArduPlane navigates to when the + mission starts, or None, which an item nothing can make sense of leaves + as well, as it leaves mission_track() with no path''' + flight = MissionFlight((0.0, 0.0), (0.0, 0.0, 0.0), list(items), {}) + try: + index = flight.next_nav_index(-1) + except (ValueError, ZeroDivisionError, OverflowError): + return None + return None if index is None else index + 1 + + +def mission_track(home, items, params, heading=None, start=None, rally=None, + approach=None, takeoff_course=None): + '''the path an ArduPlane flies a mission along, as (lat, lon, amsl) + points, or None where it cannot be worked out. + + home is (lat, lon, amsl). items are the mission items after home, in + order and numbered from 1, as (command, lat, lon, amsl, (param1, param2, + param3, param4)): every item, since the ones with no position of their + own change how the others are flown. amsl is None where it is not + known. params are the vehicle's parameters; ArduPlane's defaults stand + in for any not there. heading is the course in degrees a VTOL takeoff + transitions on, where there is something to go on: the way the aircraft + points. takeoff_course is the course in degrees a fixed-wing takeoff + was flown on, for a caller which has measured a flight of it: nothing + says it beforehand, since ArduPlane holds whatever course the launch or + the runway gave it, so without one such a takeoff is drawn towards the + next item. start is (lat, lon, amsl) + where the aircraft is when the mission starts, where that is not home. + rally is the vehicle's rally points, as (lat, lon, amsl), where it has + any, which a return to launch may go to instead of home. approach is + the course in degrees of a VTOL landing approach, which ArduPlane flies + into the wind it estimates, as MAVLink's WIND gives where it comes from; + STILL_AIR_APPROACH where none is given. + + The aircraft sets off from home, or start, towards the first item. + Each DO_JUMP is followed the once, so a loop is drawn once however many + times it is flown, and one repeated for ever ends the flight the second + time it is reached. A mission which runs out is complete, and the + aircraft returns to launch. A navigation command this does not know how to fly + -- NAV_DELAY, CONTINUE_AND_CHANGE_ALT and the like -- ends the attempt, + as does an item the aircraft never finishes, and a flight longer than + MAX_FLIGHT_TIME + ''' + if home is None or (home[0] == 0 and home[1] == 0) or home[2] is None: + return None + try: + if least_flight_time(home, items, params, start) > MAX_FLIGHT_TIME: + return None + flight = MissionFlight((home[0], home[1]), home, list(items), params, + heading, start, rally, approach, + takeoff_course) + return flight.run() + except (ValueError, ZeroDivisionError, OverflowError): + return None diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py new file mode 100644 index 0000000000..db4355e39d --- /dev/null +++ b/tests/test_plane_track.py @@ -0,0 +1,1204 @@ +'''the path plane_track flies a mission along, against ArduPlane's own +navigation and a flight flown through it''' + +import math + +import pytest + +from pymavlink import mavutil + +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import plane_track + +mavlink = mavutil.mavlink +HOME = (-35.363262, 149.165238, 584.0) +PARAMS = { + 'AIRSPEED_CRUISE': 22.0, 'AIRSPEED_MIN': 10.0, 'AIRSPEED_MAX': 40.0, + 'NAVL1_PERIOD': 15.0, 'NAVL1_DAMPING': 0.75, + 'ROLL_LIMIT_DEG': 50.0, 'WP_RADIUS': 60.0, 'WP_LOITER_RAD': 80.0, + 'TECS_CLMB_MAX': 5.0, 'TECS_SINK_MAX': 5.0, +} + + +# PARAMS for an aircraft whose TECS flies by an airspeed sensor +AIRSPEED_SENSOR = dict(PARAMS, ARSPD_TYPE=1, ARSPD_USE=1) + + +def offset(north, east, alt=100.0, base=HOME): + '''(lat, lon, amsl) north and east metres of base, alt above it''' + (lat, lon) = mp_util.gps_offset(base[0], base[1], east, north) + return (lat, lon, base[2] + alt) + + +def waypoint(north, east, alt=100.0): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, amsl, (0, 0, 0, 0)) + + +def loiter_to_alt(north, east, alt, radius=0.0, xtrack=0.0): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_LOITER_TO_ALT, lat, lon, amsl, + (0, radius, 0, xtrack)) + + +def loiter_turns(north, east, turns, radius, alt=100.0, xtrack=0.0): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_LOITER_TURNS, lat, lon, amsl, + (turns, 0, radius, xtrack)) + + +def change_speed(speed): + return (mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 0, None, (0, speed, -1, 0)) + + +def local(point): + '''metres (north, east) of HOME''' + return (mp_util.gps_distance(HOME[0], HOME[1], point[0], HOME[1]) * + (1 if point[0] >= HOME[0] else -1), + mp_util.gps_distance(HOME[0], HOME[1], HOME[0], point[1]) * + (1 if point[1] >= HOME[1] else -1)) + + +def fly(items, params=PARAMS, home=HOME): + track = plane_track.mission_track(home, items, params) + assert track is not None + return track + + +def up_to(track, north, east): + '''the track as far as where it comes closest to the point north and + east metres of HOME: the mission, without the return to launch which + follows it once it is complete''' + (lat, lon, _) = offset(north, east) + distances = [mp_util.gps_distance(p[0], p[1], lat, lon) for p in track] + return track[:distances.index(min(distances)) + 1] + + +def nearest(track, lat, lon): + '''the point on track nearest lat, lon''' + return min(track, key=lambda p: mp_util.gps_distance(p[0], p[1], lat, lon)) + + +def arrival(track, lat, lon, within=100.0): + '''the first point on track within so many metres of lat, lon: where it + gets there, before it turns for whatever comes next''' + return [p for p in track + if mp_util.gps_distance(p[0], p[1], lat, lon) < within][0] + + +def distance_to_line(point, start, end): + '''signed metres of point to the right of the line start->end''' + (n, e) = (point[0] - start[0], point[1] - start[1]) + (dn, de) = (end[0] - start[0], end[1] - start[1]) + length = math.hypot(dn, de) + return (n * de - e * dn) / length + + +class TestL1Control(object): + '''the controller port, one step at a time''' + + def control(self): + return plane_track.L1Control(15.0, 0.75, 0.0, 0.0) + + def test_on_the_track_it_holds_straight(self): + l1 = self.control() + l1.update_waypoint((0, 0), (20, 0), 0.0, (-500, 0), (500, 0), 0.1) + assert l1.lateral_acceleration == pytest.approx(0.0, abs=1e-9) + + def test_off_the_track_it_turns_back_towards_it(self): + l1 = self.control() + # east of a track running north, it turns left + l1.update_waypoint((0, 50), (20, 0), 0.0, (-500, 0), (500, 0), 0.1) + assert l1.lateral_acceleration < 0 + l1.update_waypoint((0, -50), (20, 0), 0.0, (-500, 0), (500, 0), 0.1) + assert l1.lateral_acceleration > 0 + + def test_far_from_a_loiter_it_heads_for_the_centre(self): + l1 = self.control() + l1.update_loiter((-1000, 0), (20, 0), 0.0, (0, 0), 100.0, 1) + assert not l1.circling + assert l1.lateral_acceleration == pytest.approx(0.0, abs=1e-6) + + def test_on_the_circle_it_turns_at_the_circles_rate(self): + l1 = self.control() + # due west of the centre, flying north: clockwise round it + l1.update_loiter((0, -100), (20, 0), 0.0, (0, 0), 100.0, 1) + assert l1.circling + assert l1.lateral_acceleration == pytest.approx(20.0 ** 2 / 100.0) + + def test_the_circle_flown_grows_with_altitude(self): + l1 = self.control() + assert l1.loiter_radius(100.0, 0.0, 20.0) == pytest.approx(100.0) + assert l1.loiter_radius(100.0, 3000.0, 20.0) > 130.0 + + +class TestWaypointMaxRadius(object): + """WP_MAX_RADIUS: a waypoint is not reached until the aircraft is that + close to it, however far past it has flown (Plane::verify_nav_wp)""" + + def waypoint(self, north, east, passby=0): + (lat, lon, amsl) = offset(north, east) + return (mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, amsl, (0, 0, passby, 0)) + + def corner(self, passby=0): + '''a right-angle turn at 1000 north, 600 east''' + return [self.waypoint(1000, 0), self.waypoint(1000, 600, passby), + self.waypoint(0, 600)] + + def closest(self, track, north, east): + (lat, lon, _) = offset(north, east) + distances = [mp_util.gps_distance(p[0], p[1], lat, lon) for p in track] + return (min(distances), distances.index(min(distances))) + + def test_the_corner_is_not_cut(self): + # turning early, the aircraft passes wide of the corner + (wide, _) = self.closest(fly(self.corner()), 1000, 600) + assert wide > 20.0 + track = fly(self.corner(), dict(PARAMS, WP_MAX_RADIUS=20)) + (near, _) = self.closest(track, 1000, 600) + assert near <= 20.0 + # and the mission goes on from there + assert self.closest(track, 0, 600)[0] <= PARAMS['WP_RADIUS'] + + def test_one_it_cannot_turn_tightly_enough_for_is_not_drawn(self): + # a waypoint 100m to the side of one the aircraft arrives at going + # the other way, with a turn some 85m across: held within 60m it + # is reached, and within 30m it is circled for ever, as ArduPlane + # warns, which leaves no path to draw + params = dict(PARAMS, ROLL_LIMIT_DEG=30.0) + items = [self.waypoint(1000, 0), self.waypoint(1000, 100), + self.waypoint(2000, 100)] + track = fly(items, dict(params, WP_MAX_RADIUS=60)) + assert self.closest(track, 1000, 100)[0] <= 60.0 + assert plane_track.mission_track( + HOME, items, dict(params, WP_MAX_RADIUS=30)) is None + + def test_a_pass_by_waypoint_is_overflown(self): + # the finish line is past the pass-by point, so the aircraft is + # never taken back to the waypoint: "it will overfly badly", as + # ArduPlane has it + params = dict(PARAMS, WP_MAX_RADIUS=30) + items = [self.waypoint(1000, 0), self.waypoint(1000, 600, passby=100), + self.waypoint(1000, 0)] + assert plane_track.mission_track(HOME, items, params) is None + assert plane_track.mission_track(HOME, items, PARAMS) is not None + + def test_only_waypoints_are_held_to_it(self): + # a landing passed wide of the radius is still done with + (lat, lon, amsl) = offset(1200, 0, 0) + items = [loiter_turns(1000, 0, 1, 80), + (mavlink.MAV_CMD_NAV_LAND, lat, lon, amsl, (0, 0, 0, 0))] + track = fly(items) + assert self.closest(track, 1200, 0)[0] > 1.0 + assert fly(items, dict(PARAMS, WP_MAX_RADIUS=1)) == track + # as are loiters + items = [loiter_turns(1000, 0, 1, 80), loiter_turns(1000, 600, 1, 80)] + assert fly(items, dict(PARAMS, WP_MAX_RADIUS=1)) == fly(items) + + +class TestVtolApproach(object): + """a QuadPlane's VTOL landing flown in on a fixed-wing approach: + Plane::verify_landing_vtol_approach""" + + QUADPLANE = dict(PARAMS, Q_ENABLE=1) + + def vtol_land(self, north, east, alt=60.0, option=1): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_VTOL_LAND, lat, lon, amsl, + (option, 0, 0, 0)) + + def items(self, **kwargs): + # out east and back to land at home + return [waypoint(0, 2000, 150), self.vtol_land(0, 0, **kwargs)] + + def landed(self, track): + '''where the fixed-wing flight hands over to the VTOL landing, from + the landing, where the path ends; the course it was on there; and + the path before, north and east of home''' + points = [local(p) for p in track] + end = points[-1] + pad = [i for (i, p) in enumerate(points) + if i > 10 and math.hypot(p[0] - end[0], p[1] - end[1]) < 0.5][0] + (a, b) = (points[pad - 2], points[pad - 1]) + course = math.degrees(math.atan2(b[1] - a[1], b[0] - a[0])) + handover = (b[0] - end[0], b[1] - end[1]) + bearing = math.degrees(math.atan2(handover[1], handover[0])) + return (handover, bearing, course, points[:pad]) + + def swept(self, points): + '''degrees swept clockwise about the pad''' + total = 0.0 + for (a, b) in zip(points, points[1:]): + total += mp_util.wrap_180(math.degrees(math.atan2(b[1], b[0])) - + math.degrees(math.atan2(a[1], a[0]))) + return total + + def circling(self, before, radius): + '''what was flown from coming back near the landing until the + approach''' + back = [i for (i, p) in enumerate(before) + if i > len(before) // 3 and math.hypot(*p) < radius * 1.3] + return before[back[0]:] + + def test_the_landing_is_circled_to_and_flown_in_to(self): + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=250) + track = plane_track.mission_track(HOME, self.items(), params, + approach=0.0) + (handover, bearing, course, before) = self.landed(track) + # the landing's altitude, come down to on the way in as ArduPlane + # does, and the circle round it at Q_FW_LND_APR_RAD, clockwise: + # from the east, a quarter of the way round to fly north + assert min(p[2] for p in track[len(track) // 2:]) >= HOME[2] + 55.0 + circling = self.circling(before, 250) + assert self.swept(circling) == pytest.approx(90.0, abs=30.0) + assert max(math.hypot(*p) for p in circling) > 250 + # into the wind: north, from south of the landing, a stopping + # distance or so out, and within the 30 degrees ArduPlane takes as + # lined up + assert mp_util.wrap_180(bearing - 180.0) == pytest.approx(0, abs=15) + assert course == pytest.approx(0.0, abs=30.0) + assert 100.0 < math.hypot(*handover) < 250.0 + # and it lands where the item is, at the altitude approached at + assert math.hypot(*local(track[-1])) < 0.5 + assert track[-1][2] == pytest.approx(HOME[2] + 60.0, abs=1.0) + + def test_it_is_flown_into_the_wind(self): + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=250) + for approach in (0.0, 90.0, -135.0): + track = plane_track.mission_track(HOME, self.items(), params, + approach=approach) + (_, bearing, course, _) = self.landed(track) + assert mp_util.wrap_180(course - approach) == pytest.approx( + 0.0, abs=30.0) + assert mp_util.wrap_180(bearing - (approach + 180)) == \ + pytest.approx(0.0, abs=20.0) + # with no wind to go on, ArduPlane's sum comes to due south + track = plane_track.mission_track(HOME, self.items(), params) + (_, bearing, course, _) = self.landed(track) + assert mp_util.wrap_180(course - 180.0) == pytest.approx(0, abs=30) + assert mp_util.wrap_180(bearing) == pytest.approx(0, abs=20) + + def test_the_circle_is_flown_the_way_its_radius_says(self): + # from the east to fly east: half way round, either way + for (radius, clockwise) in ((250, True), (-250, False)): + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=radius) + track = plane_track.mission_track(HOME, self.items(), params, + approach=90.0) + swept = self.swept(self.circling(self.landed(track)[3], 250)) + assert swept > 150 if clockwise else swept < -150 + # and with no radius of its own, WP_LOITER_RAD's + track = plane_track.mission_track(HOME, self.items(), + self.QUADPLANE, approach=90.0) + radius = PARAMS['WP_LOITER_RAD'] + circling = self.circling(self.landed(track)[3], radius) + assert min(math.hypot(*p) for p in circling) < radius * 1.3 + assert self.swept(circling) > 150 + + def test_what_asks_for_it(self): + def circles(items, params): + # coming from the east, only circling takes it west of the pad + track = plane_track.mission_track(HOME, items, params, + approach=0.0) + return any(local(p)[1] < -40 for p in track) + # param1, or Q_OPTIONS for every VTOL landing + assert circles(self.items(option=1), self.QUADPLANE) + assert circles(self.items(option=0), + dict(self.QUADPLANE, Q_OPTIONS=1 << 4)) + # but not a VTOL landing asked for neither way, which is flown + # straight in, nor anything but a QuadPlane + assert not circles(self.items(option=0), self.QUADPLANE) + assert not circles(self.items(option=1), PARAMS) + items = [waypoint(0, 2000, 150), self.vtol_land(0, 0, option=1)] + assert plane_track.uses_vtol_approach(items, self.QUADPLANE) + assert not plane_track.uses_vtol_approach(items, PARAMS) + items[-1] = self.vtol_land(0, 0, option=0) + assert not plane_track.uses_vtol_approach(items, self.QUADPLANE) + assert plane_track.uses_vtol_approach( + items, dict(self.QUADPLANE, Q_OPTIONS=1 << 4)) + + def test_a_landing_where_the_aircraft_is_goes_out_to_the_circle(self): + # a VTOL landing with no position of its own is where the aircraft + # is when it starts, so the circle is started from its middle, and + # the aircraft goes out to it before breaking out onto the approach + # -- even arriving, as here, on the course it breaks out on: east, + # for a clockwise circle and an approach south + here = (mavlink.MAV_CMD_NAV_VTOL_LAND, 0.0, 0.0, HOME[2] + 60, + (1, 0, 0, 0)) + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=250) + track = plane_track.mission_track( + HOME, [waypoint(0, 2000, 60), here], params, approach=180.0) + landing = local(track[-1]) + assert landing[1] > 1800 + points = [local(p) for p in track] + arrived = [i for (i, p) in enumerate(points) + if math.hypot(p[0] - landing[0], p[1] - landing[1]) < 30] + out = [math.hypot(p[0] - landing[0], p[1] - landing[1]) + for p in points[arrived[0]:]] + assert max(out) > 245.0 + # and comes in from the north, into the wind + last = points[-3] + assert last[0] > landing[0] + 50 + + def test_a_mission_which_runs_out_lands_on_the_approach_too(self): + """the return a mission which runs out flies is landed as + Q_RTL_MODE says, so such a mission is flown into the wind as + much as one which returns by an item of its own""" + params = dict(self.QUADPLANE, Q_RTL_MODE=2, Q_FW_LND_APR_RAD=200) + items = [waypoint(0, 2000, 150), waypoint(500, 2000, 150)] + assert plane_track.uses_vtol_approach(items, params) + # and the course it is drawn on is the wind's + into = [plane_track.mission_track(HOME, items, params, + approach=course)[-3] + for course in (45.0, 225.0)] + assert mp_util.gps_distance(into[0][0], into[0][1], + into[1][0], into[1][1]) > 100.0 + # where a mission the flight ends in does not return at all + for ending in (self.vtol_land(0, 0, option=0), + (mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, None, + (0, 0, 0, 0)), + (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (1, -1, 0, 0))): + assert not plane_track.uses_vtol_approach(items + [ending], params) + # and a landing the flight never reaches, a jump going past it, does + # not end anything: the mission still runs out + landing = (mavlink.MAV_CMD_NAV_LAND, HOME[0], HOME[1], HOME[2], + (0, 0, 0, 0)) + jumped = [items[0], (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, + (4, 1, 0, 0)), landing, items[1]] + assert plane_track.uses_vtol_approach(jumped, params) + # nor is an approach landing after one the flight ends at flown + assert not plane_track.uses_vtol_approach( + [items[0], landing, self.vtol_land(0, 0, option=1)], params) + # and a mission nothing can make sense of has no path at all, so + # asking it about the wind is answered rather than raising + nonsense = [items[0], (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, + (float('nan'), 1, 0, 0))] + assert not plane_track.uses_vtol_approach(nonsense, params) + assert plane_track.mission_track(HOME, nonsense, params) is None + + def test_a_return_to_launch_can_land_so(self): + """Q_RTL_MODE 2: circle home at RTL_ALTITUDE, go down to Q_RTL_ALT, + and come in on the approach""" + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + items = [waypoint(0, 2000, 150), rtl] + params = dict(self.QUADPLANE, Q_RTL_MODE=2, Q_FW_LND_APR_RAD=200) + assert plane_track.uses_vtol_approach(items, params) + assert not plane_track.uses_vtol_approach(items, self.QUADPLANE) + track = plane_track.mission_track(HOME, items, params, approach=45.0) + (_, bearing, course, before) = self.landed(track) + assert course == pytest.approx(45.0, abs=30.0) + assert mp_util.wrap_180(bearing - 225.0) == pytest.approx(0, abs=20) + # it was round home at RTL_ALTITUDE before coming down + assert any(150 < math.hypot(*local(p)) < 300 and + p[2] > HOME[2] + 90.0 for p in track[len(track) // 2:]) + assert track[-1][2] == pytest.approx(HOME[2] + 15.0, abs=1.0) + assert math.hypot(*local(track[-1])) < 0.5 + + +class TestMissionFlight(object): + '''ArduPlane's mission logic, flown''' + + def test_it_flies_the_legs_between_waypoints(self): + track = fly([waypoint(3000, 0), waypoint(3000, 3000)]) + points = [local(p) for p in up_to(track, 3000, 3000)] + # tracking the first leg north, well away from its ends + on_leg = [p for p in points if 800 < p[0] < 2200 and p[1] < 1500] + assert on_leg + assert max(abs(p[1]) for p in on_leg) < 3.0 + # and it ends at the last one, which is done with once it is within + # the distance it would start turning for whatever came next + assert math.hypot(points[-1][0] - 3000, points[-1][1] - 3000) < 100.0 + + def test_a_turn_is_cut_short_of_the_waypoint(self): + track = fly([waypoint(3000, 0), waypoint(3000, 3000)]) + points = [local(p) for p in track] + nearest = min(math.hypot(p[0] - 3000, p[1]) for p in points) + # a 90 degree turn is started WP_RADIUS or the L1 distance out + assert nearest > 20.0 + + def test_a_leg_into_a_loiter_tracks_towards_its_centre(self): + # out of one loiter and into another, the track is the line between + # their centres rather than a tangent from one circle to the other, + # until the aircraft is within three radii of the second centre: it + # leaves the first circle a radius to one side of that line, and + # pulls onto it + first = (0.0, 2000.0) + second = (6000.0, 2000.0) + track = fly([loiter_turns(first[0], first[1], 1, 300), + loiter_turns(second[0], second[1], 1, 150), + waypoint(9000, 2000)]) + points = [local(p) for p in track] + radius = plane_track.L1Control(15, 0.75, 0, 0).loiter_radius(150, 684, 22) + approach = [p for p in points + if 2500 < p[0] < second[0] - 3 * radius - 300 and + abs(p[1] - 2000) < 500] + assert approach + assert max(abs(distance_to_line(p, first, second)) for p in approach) < 5.0 + + def test_a_loiter_is_left_lined_up_on_what_comes_next(self): + centre = (0.0, 3000.0) + after = (3000.0, 3000.0) + track = fly([loiter_turns(centre[0], centre[1], 1, 200), + waypoint(*after)]) + points = [local(p) for p in up_to(track, *after)] + radius = plane_track.L1Control(15, 0.75, 0, 0).loiter_radius(200, 684, 22) + leaving = [p for p in points + if p[0] > 400 and math.hypot(p[0] - centre[0], p[1] - centre[1]) > radius + 50] + assert leaving + # the leg is flown against the track out of the loiter's centre, so it + # ends up on the line from the centre to the waypoint + late = [p for p in leaving if 1500 < p[0] < 2500] + assert max(abs(distance_to_line(p, centre, after)) for p in late) < 5.0 + + def test_a_loiter_lines_up_on_an_item_with_no_position_as_stored(self): + # Plane::verify_loiter_heading heads for the next item's location as + # the mission holds it, which is 0, 0 for one with no position: only + # starting that item puts it where the aircraft is. From here that + # is west-north-west, as Location::get_distance_NE has it + here = (mavlink.MAV_CMD_NAV_LOITER_TURNS, 0.0, 0.0, HOME[2] + 100, + (1, 0, 80, 0)) + (clat, clon) = offset(1000, 0)[:2] + scale = math.cos(math.radians(clat / 2.0)) + towards = math.degrees(math.atan2(-clon * scale, -clat)) + for radius in (100, -100): + flight = plane_track.MissionFlight( + (HOME[0], HOME[1]), HOME, + [loiter_turns(1000, 0, 1, radius, xtrack=1), here], PARAMS) + courses = [] + real = flight.fly_loiter + + def recording(index): + if index == 1: + courses.append(math.degrees(flight.yaw)) + return real(index) + flight.fly_loiter = recording + assert flight.run() is not None + (course,) = courses + # within ArduPlane's 10 degrees, and the aircraft's turn since + assert mp_util.wrap_180(course - towards) == pytest.approx( + 0, abs=15) + + def test_param4_crosstracks_from_where_the_loiter_is_left(self): + centre = (0.0, 3000.0) + after = (3000.0, 3000.0) + from_centre = [local(p) for p in up_to(fly( + [loiter_turns(centre[0], centre[1], 1, 200), waypoint(*after)]), + *after)] + from_exit = [local(p) for p in up_to(fly( + [loiter_turns(centre[0], centre[1], 1, 200, xtrack=1), waypoint(*after)]), + *after)] + # from the exit the aircraft does not pull back onto the centre line, + # so halfway along the leg it is still well to one side of it + halfway = [p for p in from_exit if 1400 < p[0] < 1600] + assert halfway + assert min(abs(distance_to_line(p, centre, after)) for p in halfway) > 50.0 + halfway = [p for p in from_centre if 1400 < p[0] < 1600] + assert max(abs(distance_to_line(p, centre, after)) for p in halfway) < 5.0 + + def test_loiters_turn_the_way_their_radius_says(self): + for (radius, clockwise) in ((200, True), (-200, False)): + points = [local(p) for p in fly( + [loiter_turns(0, 3000, 2, radius), waypoint(3000, 3000)])] + circling = [p for p in points + if math.hypot(p[0], p[1] - 3000) < 400] + swept = 0.0 + for (a, b) in zip(circling, circling[1:]): + swept += mp_util.wrap_180( + math.degrees(math.atan2(b[1] - 3000, b[0])) - + math.degrees(math.atan2(a[1] - 3000, a[0]))) + # a bearing from the centre measured east of north grows clockwise + if clockwise: + assert swept > 600 + else: + assert swept < -600 + + def test_a_loiter_to_altitude_circles_until_it_gets_there(self): + climb = 1000.0 + points = fly([loiter_to_alt(0, 3000, 100 + climb, 150), + waypoint(3000, 3000, 100 + climb)]) + centre = offset(0, 3000) + on_circle = [p for p in points + if mp_util.gps_distance(p[0], p[1], centre[0], centre[1]) < 260] + # climbing at 5 m/s it spends at least climb / 5 seconds up there + # (less what it climbs on the way), a circle every 2 pi r / v seconds + laps = len(on_circle) * plane_track.POINT_SPACING / (2 * math.pi * 170) + assert laps > 2.0 + assert max(p[2] for p in points) == pytest.approx(HOME[2] + 100 + climb, abs=6.0) + + def test_a_faster_airspeed_takes_a_wider_line_through_a_turn(self): + def overshoot(speed): + items = [waypoint(3000, 0), waypoint(3000, 3000)] + if speed is not None: + items.insert(0, change_speed(speed)) + points = [local(p) for p in fly(items)] + return max(p[0] for p in points) + assert overshoot(35.0) > overshoot(None) + 10.0 + + def test_only_an_airspeed_within_its_limits_changes_the_speed(self): + def overshoot(params, change=None): + items = [waypoint(3000, 0), waypoint(3000, 3000)] + if change is not None: + items.insert(0, (mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 0, None, + change)) + return max(local(p)[0] for p in fly(items, params)) + params = PARAMS + plain = overshoot(params) + assert overshoot(params, (0, 35, -1, 0)) > plain + 10.0 + # a groundspeed is only a minimum ground speed to ArduPlane + assert overshoot(params, (1, 35, -1, 0)) == pytest.approx(plain) + # and it will not take an airspeed outside AIRSPEED_MIN/MAX + assert overshoot(dict(params, AIRSPEED_MAX=30), + (0, 35, -1, 0)) == pytest.approx(plain) + assert overshoot(params, (0, 5, -1, 0)) == pytest.approx(plain) + # -2 goes back to AIRSPEED_CRUISE + assert overshoot(params, (0, -2, -1, 0)) == pytest.approx(plain) + + def test_a_loiter_unlimited_ends_the_flight_on_its_circle(self): + (lat, lon, amsl) = offset(0, 3000) + track = fly([(mavlink.MAV_CMD_NAV_LOITER_UNLIM, lat, lon, amsl, + (0, 0, 150, 0)), waypoint(3000, 3000)]) + last = track[-1] + radius = mp_util.gps_distance(last[0], last[1], lat, lon) + assert 120 < radius < 220 + + def test_what_it_cannot_fly_is_not_drawn(self): + delay = (mavlink.MAV_CMD_NAV_DELAY, 0, 0, None, (10, 0, 0, 0)) + assert plane_track.mission_track( + HOME, [waypoint(3000, 0), delay, waypoint(3000, 3000)], PARAMS) is None + assert plane_track.mission_track( + (0, 0, 0), [waypoint(3000, 0)], PARAMS) is None + + def test_a_waypoint_too_close_to_turn_for_is_passed(self): + # ArduPlane counts a waypoint done once the aircraft is past it along + # the leg, so one right beside the last does not leave it circling + track = fly([waypoint(3000, 0), waypoint(3000, 20), waypoint(3020, 20)], + dict(PARAMS, ROLL_LIMIT_DEG=10.0)) + assert len(track) > 10 + + def jump(self, target, repeats, command=mavlink.MAV_CMD_DO_JUMP): + return (command, 0, 0, None, (target, repeats, 0, 0)) + + def visits(self, track, north, east, within=80.0): + '''how many separate times the track passes near a point''' + (lat, lon, _) = offset(north, east) + count = 0 + near = False + for p in track: + close = mp_util.gps_distance(p[0], p[1], lat, lon) < within + if close and not near: + count += 1 + near = close + return count + + def test_a_jump_is_followed(self): + # 1 and 2 are a circuit's first two corners; 3 jumps past a + # waypoint which the mission only reaches by the jump back at 6 + items = [waypoint(2000, 0), waypoint(2000, 2000), + self.jump(5, -1), waypoint(-3000, -3000), + waypoint(0, 2000), self.jump(4, 1), waypoint(0, 4000)] + track = up_to(fly(items), 0, 4000) + # never flown straight from 2 to 4: 4 is reached once, by the jump + # back at 6, and 5 is flown before and after it + assert self.visits(track, -3000, -3000) == 1 + assert self.visits(track, 0, 2000) == 2 + assert self.visits(track, 0, 4000) == 1 + + def test_a_jump_is_drawn_once_however_often_it_repeats(self): + items = [waypoint(2000, 0), waypoint(2000, 2000), waypoint(0, 2000), + self.jump(1, 50), waypoint(0, 4000)] + track = fly(items) + assert self.visits(track, 2000, 2000) == 2 + assert self.visits(track, 0, 4000) == 1 + + def test_a_jump_repeated_for_ever_ends_the_flight(self): + # the aircraft never gets past it: the circuit is drawn going round + # the once, and nothing after the jump is drawn + items = [waypoint(2000, 0), waypoint(2000, 2000), waypoint(0, 2000), + self.jump(1, -1), waypoint(0, 4000)] + track = fly(items) + assert self.visits(track, 2000, 2000) == 2 + assert self.visits(track, 0, 4000) == 0 + + def test_a_jump_with_no_repeats_is_not_followed(self): + # followed, it would skip the waypoint straight after it + items = [waypoint(2000, 0), self.jump(4, 0), waypoint(2000, 2000), + waypoint(0, 2000)] + assert self.visits(fly(items), 2000, 2000) == 1 + + def test_a_jump_to_a_tag(self): + tag = (600, 0, 0, None, (7, 0, 0, 0)) + items = [waypoint(2000, 0), self.jump(7, 1, command=601), + waypoint(-3000, -3000), tag, waypoint(0, 2000)] + track = fly(items) + assert self.visits(track, -3000, -3000) == 0 + assert self.visits(track, 0, 2000) == 1 + + def test_a_jump_nowhere_ends_the_mission(self): + # AP_Mission finds no next command, and the mission is over: the + # aircraft returns to launch + track = fly([waypoint(2000, 0), self.jump(9, 1), waypoint(0, 2000)]) + assert self.visits(track, 2000, 0) == 1 + assert self.visits(track, 0, 2000) == 0 + assert math.hypot(*local(track[-1])) < PARAMS['WP_LOITER_RAD'] * 1.8 + + def test_a_completed_mission_returns_to_launch(self): + # Plane::exit_mission_callback changes mode to RTL, which circles home + track = fly([waypoint(3000, 0), waypoint(3000, 3000)]) + assert self.visits(track, 3000, 3000) == 1 + circling = [local(p) for p in track[-20:]] + for p in circling: + assert math.hypot(*p) <= PARAMS['WP_LOITER_RAD'] * 1.8 + + def test_a_speed_change_before_a_mission_runs_out_is_flown_home_at(self): + """the items passed on the way to a mission's end are done before + the return which follows it: a DO_CHANGE_SPEED among them is the + speed the return is flown at, so its turn for home is wider or + tighter than it would have been""" + def turn(speed): + items = [waypoint(1500, 0), waypoint(1500, 900)] + if speed is not None: + items.append(change_speed(speed)) + track = fly(items, dict(PARAMS, RTL_RADIUS=200)) + return max(local(p)[1] for p in track) + unchanged = turn(None) + assert turn(30.0) > unchanged + 25.0 + assert turn(12.0) < unchanged - 20.0 + + def test_waypoints_closer_than_a_turn_are_passed_on_the_way_home(self): + # a box smaller than the distance a turn is started from: each + # waypoint is done with as soon as it is started, and the mission is + # over in moments, so the path is the return to launch's circle + box = [waypoint(50, -5), waypoint(50, 45), waypoint(0, 45), + waypoint(0, -5)] + track = fly(box, dict(PARAMS, WP_RADIUS=90.0)) + assert len(track) > 20 + far = max(math.hypot(*local(p)) for p in track) + assert far > PARAMS['WP_LOITER_RAD'] * 0.9 + + def test_a_mission_which_loops_for_ever_does_not_return(self): + items = [waypoint(2000, 0), waypoint(2000, 2000), waypoint(0, 2000), + self.jump(1, -1)] + track = fly(items) + # it ends going round the loop, not circling home + assert math.hypot(*local(track[-1])) > 1000 + + def takeoff(self, alt=50.0, command=mavlink.MAV_CMD_NAV_TAKEOFF): + # a takeoff item placed at home, as a ground station puts it + return (command, HOME[0], HOME[1], HOME[2] + alt, (0, 0, 0, 0)) + + def climbed_out(self, track, alt=50.0): + '''metres from home where the track first reaches the takeoff + altitude''' + for p in track: + if p[2] >= HOME[2] + alt - 0.5: + return mp_util.gps_distance(HOME[0], HOME[1], p[0], p[1]) + return None + + def takeoff_at(self, pitch, alt=50.0): + return (mavlink.MAV_CMD_NAV_TAKEOFF, HOME[0], HOME[1], HOME[2] + alt, + (pitch, 0, 0, 0)) + + def test_a_fixed_wing_takeoff_climbs_at_its_pitch(self): + # Plane::takeoff_calc_pitch: with no airspeed sensor the aircraft + # holds the item's pitch, 10 degrees here, and 4 where it gives none + for (pitch, flown) in ((10, 10), (0, 4)): + track = fly([self.takeoff_at(pitch), waypoint(3000, 0)]) + expected = 50.0 / math.tan(math.radians(flown)) + assert self.climbed_out(track) == pytest.approx(expected, rel=0.1) + (lat, lon, _) = up_to(track, 3000, 0)[-1] + assert mp_util.gps_bearing(HOME[0], HOME[1], lat, lon) == pytest.approx(0.0, abs=5.0) + + def test_a_sensor_the_throttle_turns_off_is_not_flown_by(self): + # ARSPD_USE 2 is a glider's sensor behind the propeller, which is + # not used while the throttle runs: the climb out is its pitch's + track = fly([self.takeoff_at(10), waypoint(3000, 0)], + dict(AIRSPEED_SENSOR, ARSPD_USE=2)) + assert self.climbed_out(track) == pytest.approx( + 50.0 / math.tan(math.radians(10)), rel=0.1) + + def test_a_takeoff_with_an_airspeed_sensor_climbs_out_at_full_rate(self): + # TECS flies it no lower than its pitch: at TECS_CLMB_MAX, 50m at + # 5m/s is ten seconds at 22m/s, not a slow approach to the altitude + track = fly([self.takeoff(), waypoint(3000, 0)], AIRSPEED_SENSOR) + assert self.climbed_out(track) == pytest.approx(220.0, abs=40.0) + # but a steeper pitch than that is still flown + track = fly([self.takeoff_at(20), waypoint(3000, 0)], AIRSPEED_SENSOR) + assert self.climbed_out(track) == pytest.approx( + 50.0 / math.tan(math.radians(20)), rel=0.1) + + def test_a_fixed_wing_takeoff_heads_for_the_next_waypoint(self): + # it holds whatever course it was launched or rolled down the + # runway on, which the way it pointed beforehand says little about: + # north, for the waypoint, whatever heading it is given + for heading in (None, 90.0): + track = plane_track.mission_track( + HOME, [self.takeoff(), waypoint(3000, 0)], AIRSPEED_SENSOR, + heading=heading) + climbed = [local(p) for p in track if p[2] >= HOME[2] + 49.5][0] + assert climbed[0] == pytest.approx(220.0, abs=40.0) + assert abs(climbed[1]) < 5.0 + # where a VTOL takeoff transitions the way it points + track = plane_track.mission_track( + HOME, [self.takeoff(command=mavlink.MAV_CMD_NAV_VTOL_TAKEOFF), + waypoint(3000, 0)], dict(PARAMS, Q_ENABLE=1), heading=90.0) + assert local(track[2])[1] > 10.0 + + def test_a_fixed_wing_takeoff_flies_the_course_it_was_flown_on(self): + # a caller which has a flight of the takeoff to measure knows the + # course it held, which the mission does not say + for course in (90.0, 270.0): + track = plane_track.mission_track( + HOME, [self.takeoff(), waypoint(3000, 0)], AIRSPEED_SENSOR, + heading=0.0, takeoff_course=course) + climbed = [local(p) for p in track if p[2] >= HOME[2] + 49.5][0] + east = 220.0 if course == 90.0 else -220.0 + assert climbed[1] == pytest.approx(east, abs=40.0) + assert abs(climbed[0]) < 5.0 + + def test_a_quadplane_takes_off_straight_up(self): + params = dict(PARAMS, Q_ENABLE=1, Q_OPTIONS=0) + for command in (mavlink.MAV_CMD_NAV_TAKEOFF, + mavlink.MAV_CMD_NAV_VTOL_TAKEOFF): + # and sets off from overhead home towards the waypoint behind it + track = up_to(fly([self.takeoff(command=command), + waypoint(-3000, 0)], params), -3000, 0) + assert self.climbed_out(track) == pytest.approx(0.0, abs=1.0) + assert max(local(p)[0] for p in track) < 5.0 + # unless Q_OPTIONS lets NAV_TAKEOFF be a fixed-wing one + track = fly([self.takeoff(), waypoint(3000, 0)], + dict(params, Q_OPTIONS=plane_track.Q_OPTION_ALLOW_FW_TAKEOFF)) + assert self.climbed_out(track) > 100.0 + + def test_a_mission_started_away_from_home(self): + # ArduPlane starts where the aircraft is when AUTO starts; home is + # only where it came from + start = offset(-1000, 500, 20) + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), waypoint(3000, 3000)], PARAMS, + start=start) + assert track[0] == pytest.approx(start) + # home still stands for home: a return to launch goes back there + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), rtl], PARAMS, start=start) + home_ward = [p for p in track + if mp_util.gps_distance(p[0], p[1], + HOME[0], HOME[1]) < 200.0] + assert home_ward + + def test_a_return_to_launch_circles_where_it_returns_to(self): + """ModeRTL::update circles at RTL_RADIUS, or WP_LOITER_RAD""" + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + for (rtl_radius, radius, clockwise) in ((0.0, 60.0, True), + (250.0, 250.0, True), + (-250.0, 250.0, False)): + params = dict(PARAMS, RTL_RADIUS=rtl_radius) + points = [local(p) for p in + fly([waypoint(3000, 0), rtl], params=params)] + # once it is home for the last time, it is circling there + home_ward = [i for (i, p) in enumerate(points) + if math.hypot(p[0], p[1]) > 600][-1] + circling = points[home_ward:] + circling = circling[len(circling) // 2:] + # at the radius asked for, allowing the room an aircraft which + # cannot bank that tightly takes + for p in circling: + assert radius <= math.hypot(p[0], p[1]) <= radius * 1.8 + swept = 0.0 + for (a, b) in zip(circling, circling[1:]): + swept += mp_util.wrap_180( + math.degrees(math.atan2(b[1], b[0])) - + math.degrees(math.atan2(a[1], a[0]))) + # a bearing from the centre grows clockwise + assert swept > 120 if clockwise else swept < -120 + + def test_a_return_to_launch_joins_its_circle(self): + # ArduPlane circles from the start of an RTL, so the aircraft turns + # onto the circle rather than flying over its middle first + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + + def closest_on_return(params): + distances = [math.hypot(*local(p)) + for p in fly([waypoint(3000, 0), rtl], params=params)] + return min(distances[distances.index(max(distances)):]) + for (params, radius) in ((dict(PARAMS, RTL_RADIUS=250), 250.0), + (PARAMS, PARAMS['WP_LOITER_RAD'])): + assert closest_on_return(params) > radius * 0.9 + # a plane given the parameter which only a QuadPlane takes is still + # one of those + assert closest_on_return( + dict(PARAMS, RTL_RADIUS=250, Q_RTL_MODE=1)) > 225.0 + + def test_a_quadplane_returns_to_land(self): + '''QRTL, which Q_RTL_MODE 1 switches to on the way home and 3 goes + to at once, lands where it returns to rather than circling''' + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + for (mode, alt) in ((1, 100.0), (3, 15.0)): + params = dict(PARAMS, RTL_RADIUS=250, Q_ENABLE=1, Q_RTL_MODE=mode) + track = fly([waypoint(3000, 0), rtl], params=params) + assert math.hypot(*local(track[-1])) < 5.0 + # QRTL comes home at Q_RTL_ALT, and RTL at RTL_ALTITUDE + assert track[-1][2] == pytest.approx(HOME[2] + alt, abs=10.0) + # and it never went round + assert all(local(p)[0] > -20.0 for p in track) + + def test_qrtl_comes_home_on_its_own_altitude_profile(self): + '''QRTL, which Q_RTL_MODE 3 goes to at once, comes in at + RTL_ALTITUDE, not down a slope to Q_RTL_ALT, and drops to that only + over the stretch its sink rate needs (ModeQRTL:: + update_target_altitude)''' + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + params = dict(PARAMS, RTL_RADIUS=250, Q_ENABLE=1, Q_RTL_MODE=3, + RTL_ALTITUDE=100, Q_RTL_ALT=15) + + def heights(start_alt): + track = fly([waypoint(3000, 0, start_alt), rtl], params=params) + turned = next(i for (i, p) in enumerate(track) + if local(p)[0] > 2900) + return [(math.hypot(*local(p)), p[2] - HOME[2]) + for p in track[turned:]] + + def at(heights, distance): + return next(h for (d, h) in heights if d < distance) + # from RTL_ALTITUDE: 2 * RTL_RADIUS, plus 85m at 3m/s (0.6 of + # TECS_SINK_MAX) and AIRSPEED_CRUISE, out: 500m + 22m/s * 28.3s + level = heights(100) + assert all(h == pytest.approx(100, abs=1) for (d, h) in level + if 1150 < d < 2500) + assert 30 < at(level, 700) < 90 + assert at(level, 20) == pytest.approx(15, abs=5) + # from higher, down a slope to RTL_ALTITUDE by then + high = heights(300) + assert 150 < at(high, 2000) < 270 + assert at(high, 1150) == pytest.approx(100, abs=20) + assert at(high, 20) == pytest.approx(15, abs=5) + + def test_qrtl_does_not_climb_back_up_to_its_slope(self): + '''an aircraft which has got below QRTL's slope down to + RTL_ALTITUDE is followed down, not sent back up to it''' + params = dict(PARAMS, RTL_RADIUS=250, Q_ENABLE=1, Q_RTL_MODE=3, + RTL_ALTITUDE=100, Q_RTL_ALT=15) + flight = plane_track.MissionFlight((HOME[0], HOME[1]), HOME, [], + params) + flight.qrtl = True + flight.next_wp = (flight.home, HOME[2] + 15) + # the approach began 3km out, 300m up, and the slope from there + # is at 200m 2km out, where the aircraft is only 150m up + flight.qrtl_start = (285.0, 3000.0) + flight.position = (flight.home[0] + 2000.0, flight.home[1]) + flight.amsl = HOME[2] + 150 + flight.update_target_altitude() + assert flight.target_amsl == pytest.approx(HOME[2] + 150) + # and never below RTL_ALTITUDE while out there + flight.amsl = HOME[2] + 50 + flight.update_target_altitude() + assert flight.target_amsl == pytest.approx(HOME[2] + 100) + + def test_the_first_item_navigated_to(self): + items = [change_speed(30), self.jump(4, 1), waypoint(2000, 0), + waypoint(3000, 0)] + assert plane_track.first_navigation_item(items) == 4 + assert plane_track.first_navigation_item([change_speed(30)]) is None + # a mission nothing can make sense of has no first item to navigate + # to either, as it has no path: the caller is told so rather than + # having the item raised at it + nonsense = [self.jump(float('nan'), 1), waypoint(2000, 0)] + assert plane_track.first_navigation_item(nonsense) is None + assert plane_track.mission_track(HOME, nonsense, PARAMS) is None + + def test_a_mission_across_the_antimeridian(self): + import time + home = (10.0, 179.998, 0.0) + east = mp_util.gps_newpos(home[0], home[1], 90, 300) + assert east[1] < 0 + start = time.time() + track = fly([(mavlink.MAV_CMD_NAV_WAYPOINT, east[0], east[1], 100.0, + (0, 0, 0, 0))], home=home) + assert time.time() - start < 2.0 + # reaching the waypoint, having flown the short way round + distances = [mp_util.gps_distance(p[0], p[1], east[0], east[1]) + for p in track] + assert min(distances) < 100.0 + assert distances.index(min(distances)) < 100 + + def test_a_long_loiter_is_flown_through(self): + # its turns are not taken for being stuck + for (turns, radius) in ((100, 80), (40, 200)): + track = fly([loiter_turns(3000, 0, turns, radius), + waypoint(3000, 3000)]) + (lat, lon, _) = offset(3000, 3000) + end = up_to(track, 3000, 3000)[-1] + assert mp_util.gps_distance(end[0], end[1], lat, lon) < 100.0 + circled = sum(mp_util.gps_distance(a[0], a[1], b[0], b[1]) + for (a, b) in zip(track, track[1:])) + assert circled > turns * 2 * math.pi * radius + # nor is a long climb, longer than the leg to it and twenty laps + track = fly([loiter_to_alt(500, 0, 6000, radius=80), + waypoint(3000, 3000, 6000)]) + assert max(p[2] for p in track) >= HOME[2] + 5990 + + def test_a_jump_which_cannot_be_counted_is_not_flown(self): + items = [waypoint(2000, 0), + (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, + (float('nan'), 1, 0, 0)), + waypoint(0, 2000)] + assert plane_track.mission_track(HOME, items, PARAMS) is None + + def test_a_mission_too_long_to_fly_is_not(self): + import time + far = offset(2000 * 1000, 0) + start = time.time() + assert plane_track.mission_track( + HOME, [waypoint(3000, 0), + (mavlink.MAV_CMD_NAV_WAYPOINT, far[0], far[1], far[2], + (0, 0, 0, 0))], PARAMS) is None + assert time.time() - start < 0.5 + + def test_a_mission_too_long_to_fly_is_not_flown_at_all(self, monkeypatch): + # the check is there to save flying it: it must not be flown first + flights = [] + monkeypatch.setattr(plane_track.MissionFlight, 'run', + lambda self: flights.append(self)) + far = offset(2000 * 1000, 0) + assert plane_track.mission_track( + HOME, [waypoint(3000, 0), + (mavlink.MAV_CMD_NAV_WAYPOINT, far[0], far[1], far[2], + (0, 0, 0, 0))], PARAMS) is None + assert flights == [] + + def test_a_mission_is_measured_from_where_it_starts(self): + # the aircraft carried a long way from home before the mission + # starts flies from there, so it is no longer than it looks + start = offset(2000 * 1000, 0) + items = [(mavlink.MAV_CMD_NAV_WAYPOINT, start[0], start[1], start[2], + (0, 0, 0, 0))] + assert plane_track.least_flight_time( + HOME, items, PARAMS, start=start) < 60.0 + assert plane_track.least_flight_time(HOME, items, PARAMS) > 4 * 3600 + assert plane_track.mission_track(HOME, items, PARAMS, + start=start) is not None + assert plane_track.mission_track(HOME, items, PARAMS) is None + + def test_a_jump_to_the_last_item_is_followed(self): + # items are numbered from home, which is not among them, so the + # last item is numbered as many as there are + items = [waypoint(2000, 0), waypoint(2000, 2000), + (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (3, 1, 0, 0)), + waypoint(0, 2000)] + flight = plane_track.MissionFlight((HOME[0], HOME[1]), HOME, + list(items), PARAMS) + assert flight.jump_target(2) == 2 + # and one past it goes nowhere + beyond = list(items) + beyond[2] = (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (5, 1, 0, 0)) + flight = plane_track.MissionFlight((HOME[0], HOME[1]), HOME, + list(beyond), PARAMS) + assert flight.jump_target(2) is None + + def test_the_flight_time_limit_holds_inside_an_item(self, monkeypatch): + # a mission short enough in straight lines, but which takes longer + # than the limit to fly, stops at the limit rather than flying on to + # the end of the item it is in + longest = [] + real = plane_track.MissionFlight.fly + + def timed(flight, *args, **kwargs): + real(flight, *args, **kwargs) + longest.append(flight.time) + monkeypatch.setattr(plane_track.MissionFlight, 'fly', timed) + monkeypatch.setattr(plane_track, 'MAX_FLIGHT_TIME', 250.0) + items = [waypoint(8000, 0)] + assert plane_track.mission_track(HOME, items, PARAMS) is None + assert longest and max(longest) < 250.0 + plane_track.NAV_PERIOD * 2 + monkeypatch.setattr(plane_track, 'MAX_FLIGHT_TIME', 3600.0) + assert plane_track.mission_track(HOME, items, PARAMS) is not None + + def test_a_return_to_launch_climbs_to_rtl_altitude(self): + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + items = [waypoint(3000, 0, alt=40), rtl] + + def arrives(params=None): + return fly(items, dict(PARAMS, **(params or {})))[-1][2] - HOME[2] + # ArduPlane's default is 100m above home; the height follows the + # glide slope home a few seconds behind + assert arrives() == pytest.approx(100, abs=10.0) + assert arrives({'RTL_ALTITUDE': 250}) == pytest.approx(250, abs=15.0) + # and a negative one comes home at the altitude it is at + assert arrives({'RTL_ALTITUDE': -1}) == pytest.approx(40, abs=3.0) + + def climbed_by(self, track, north): + '''metres above home the track has climbed to by the time it is so + far north''' + return [p for p in track if local(p)[0] >= north][0][2] - HOME[2] + + def test_a_return_to_launch_climbs_at_once(self): + # Plane::setup_alt_slope: RTL slopes only down to where it goes, and + # climbs to it as fast as it can + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + track = fly([waypoint(-3000, 0, alt=40), rtl], + dict(PARAMS, RTL_ALTITUDE=200)) + home_ward = track[track.index(up_to(track, -3000, 0)[-1]):] + # 160m at 5m/s is 32s, or 700m at 22m/s, not the whole way home + assert self.climbed_by(home_ward, -2000) > 190 + + def test_a_climb_in_auto_is_sloped_unless_asked_otherwise(self): + items = [waypoint(1000, 0, alt=30), waypoint(4000, 0, alt=200)] + # CLIMB_SLOPE_HGT and above: along the leg, halfway up it halfway + sloped = fly(items) + assert self.climbed_by(sloped, 2500) == pytest.approx(115, abs=15) + # FLIGHT_OPTIONS' IMMEDIATE_CLIMB_IN_AUTO climbs as fast as it can + at_once = fly(items, dict( + PARAMS, FLIGHT_OPTIONS=plane_track.FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO)) + assert self.climbed_by(at_once, 2500) > 190 + # and is still sloped on the way down + down = [waypoint(1000, 0, alt=200), waypoint(4000, 0, alt=30)] + for params in (PARAMS, dict( + PARAMS, FLIGHT_OPTIONS=plane_track.FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO)): + assert self.climbed_by(fly(down, params), 2500) == pytest.approx( + 115, abs=15) + + def test_a_loiter_time_holds_to_the_flight_time_limit(self, monkeypatch): + longest = [] + real = plane_track.MissionFlight.fly + + def timed(flight, *args, **kwargs): + real(flight, *args, **kwargs) + longest.append(flight.time) + monkeypatch.setattr(plane_track.MissionFlight, 'fly', timed) + monkeypatch.setattr(plane_track, 'MAX_FLIGHT_TIME', 250.0) + (lat, lon, amsl) = offset(2200, 0) + items = [(mavlink.MAV_CMD_NAV_LOITER_TIME, lat, lon, amsl, + (100, 0, 0, 0)), waypoint(2200, 300)] + params = dict(PARAMS, AIRSPEED_CRUISE=12.0, AIRSPEED_MAX=22.0) + # plainly short enough to try, in straight lines + assert plane_track.least_flight_time(HOME, items, params) < 250.0 + assert plane_track.mission_track(HOME, items, params) is None + assert longest and max(longest) < 250.0 + plane_track.NAV_PERIOD * 2 + + def test_the_least_flight_time_follows_jumps(self): + # a jump straight over a waypoint a long way off: AP_Mission never + # goes there, so neither is it counted + far = offset(1000 * 1000, 0) + jump = (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (3, 1, 0, 0)) + items = [jump, (mavlink.MAV_CMD_NAV_WAYPOINT, far[0], far[1], far[2], + (0, 0, 0, 0)), waypoint(1000, 0)] + assert plane_track.least_flight_time(HOME, items, PARAMS) < 60.0 + assert plane_track.mission_track(HOME, items, PARAMS) is not None + # and without the jump, it is far too far + assert plane_track.least_flight_time(HOME, items[1:], PARAMS) > 20000.0 + + def rtl_end(self, rally, params=None): + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), rtl], dict(PARAMS, **(params or {})), + rally=rally) + return track[-1] + + def test_a_return_to_launch_goes_to_the_nearest_rally_point(self): + near = offset(2500, 500, 150) + further = offset(-2000, 0, 150) + end = self.rtl_end([further, near]) + assert mp_util.gps_distance(end[0], end[1], near[0], near[1]) < 150.0 + assert end[2] == pytest.approx(near[2], abs=15.0) + # unless it is beyond RALLY_LIMIT_KM, when it goes home + end = self.rtl_end([near], {'RALLY_LIMIT_KM': 0.3}) + assert mp_util.gps_distance(end[0], end[1], HOME[0], HOME[1]) < 150.0 + # or RALLY_INCL_HOME has it take home, being nearer + end = self.rtl_end([further], {'RALLY_INCL_HOME': 1}) + assert mp_util.gps_distance(end[0], end[1], HOME[0], HOME[1]) < 150.0 + end = self.rtl_end([further]) + assert mp_util.gps_distance(end[0], end[1], further[0], + further[1]) < 150.0 + + def test_turns_are_flown_as_ap_mission_keeps_them(self): + def turns(n): + return fly([loiter_turns(2000, 0, n, 200), waypoint(2000, 3000)]) + # whole turns, the fraction dropped, as AP_Mission stores them + assert turns(2.5) == turns(2.0) + assert turns(2.5) != turns(3.0) + + def test_a_loiter_time_flies_the_vehicles_radius_whatever_it_asks(self): + def loiter(param3): + (lat, lon, amsl) = offset(2000, 0) + return fly([(mavlink.MAV_CMD_NAV_LOITER_TIME, lat, lon, amsl, + (60, 0, param3, 0)), waypoint(2000, 3000)]) + assert loiter(300) == loiter(0) + assert loiter(-300) != loiter(0) + + def test_no_glide_slope_with_alt_slope_min_zero(self): + # a climb along a leg follows the slope to the waypoint, unless + # ALT_SLOPE_MIN is zero or less, when it climbs as fast as it can + items = [waypoint(0, 50, alt=100), waypoint(6000, 50, alt=400)] + + def halfway(params): + track = fly(items, dict(PARAMS, CLIMB_SLOPE_HGT=0, **params)) + return min((abs(local(p)[0] - 3000), p[2]) for p in track)[1] + sloped = halfway({}) + assert sloped == pytest.approx(HOME[2] + 250, abs=25.0) + flat_out = halfway({'ALT_SLOPE_MIN': 0}) + assert flat_out == pytest.approx(HOME[2] + 400, abs=5.0) + + def test_the_altitude_of_a_rally_point(self): + # above home unless the flags say otherwise + assert plane_track.rally_amsl(100, 0, 584.0) == 684.0 + frame_valid = 1 << 2 + assert plane_track.rally_amsl(700, frame_valid | (0 << 3), 584.0) == 700.0 + assert plane_track.rally_amsl(100, frame_valid | (1 << 3), 584.0) == 684.0 + # above the EKF origin, and above the terrain under the point, + # which the caller looks up + assert plane_track.rally_amsl(100, frame_valid | (2 << 3), 584.0, + origin_amsl=500.0) == 600.0 + assert plane_track.rally_amsl(100, frame_valid | (2 << 3), 584.0) is None + assert plane_track.rally_amsl(100, frame_valid | (3 << 3), 584.0, + terrain_amsl=300.0) == 400.0 + assert plane_track.rally_amsl(100, frame_valid | (3 << 3), 584.0) is None + assert plane_track.rally_amsl(100, 0, None) is None + # only a point above home moves when home does + assert plane_track.rally_alt_is_fixed(0) is False + assert plane_track.rally_alt_is_fixed(frame_valid | (1 << 3)) is False + for frame in (0, 2, 3): + assert plane_track.rally_alt_is_fixed( + frame_valid | (frame << 3)) is True + + def test_a_rally_point_at_an_unknown_altitude_is_not_flown_to(self): + # terrain nobody has, most likely: how high the aircraft flies the + # return is anyone's guess, so the mission is not drawn flown + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + items = [waypoint(2000, 0), rtl] + (lat, lon, amsl) = offset(2200, 200) + assert plane_track.mission_track(HOME, items, PARAMS, + rally=[(lat, lon, amsl)]) is not None + assert plane_track.mission_track(HOME, items, PARAMS, + rally=[(lat, lon, None)]) is None + # one it does not go to does not matter + far = offset(2000 * 100, 0) + assert plane_track.mission_track( + HOME, items, PARAMS, rally=[(far[0], far[1], None)]) is not None + + def test_the_altitude_an_item_with_no_position_is_flown_at(self): + # Location::sanitize(): its own altitude, unless that is a relative 0 + assert plane_track.positionless_amsl(700.0, 0, 584.0) == 700.0 + assert plane_track.positionless_amsl(100.0, 3, 584.0) == 684.0 + assert plane_track.positionless_amsl(0.0, 3, 584.0) is None + assert plane_track.positionless_amsl(100.0, 3, None) is None + # the terrain under wherever the aircraft is, is not known here + assert plane_track.positionless_amsl(100.0, 10, 584.0) is None + assert plane_track.positionless_amsl(None, 0, 584.0) is None + + def test_without_parameters_arduplanes_defaults_fly(self): + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), loiter_turns(3000, 3000, 1, 0)], {}) + assert track is not None + + +# SITL flights of ArduPilot autotest missions -- added by ArduPilot PR 34424, +# https://github.com/ArduPilot/ardupilot/pull/34424 -- recorded by +# missions/record_flight.py. All were flown by ArduPlane V4.8.0-dev, which +# each recording's source says: the drawing follows what a version of +# ArduPlane does, so one which changes how it navigates -- as the QRTL +# approach did in 4.8 -- may want the drawing changed and these flown +# again. Then how near the path drawn for each has to +# keep to it: the median and 90th percentile of the distance across from +# each point flown to the path drawn, the worst of them, and the worst after +# the first 30s, which takes in a QuadPlane's transition and a plane's +# takeoff; and the 90th percentile and worst of the height of each point +# flown above or below the nearest point of the path drawn From 9781abc58dbf1d1a49b8470bcfbe10ea31561c02 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 3 Oct 2026 12:39:39 +1000 Subject: [PATCH 15/22] map3d: draw the path a plane flies its mission along The 3D map draws the path the vehicle is going to fly its mission along, worked out by plane_track, rather than the lines between its items. It is flown on a thread of its own, since flying a long mission takes a moment, and drawn as soon as it is worked out; a mission which cannot be flown through is drawn as its geometry, and the map says why once. The mission is flown again when anything it is flown by changes: the mission itself, home moving along or up and down, the wind a VTOL landing approaches into, the way the vehicle points before a takeoff, or any of the parameters. A download still coming in is drawn but not flown, since the items so far would be flown as a mission which runs out. A rally point a return would go to is drawn where the return goes to it, at the altitude its own frame gives it. The view has controls of its own for what to draw: the mission as the path flown, as its geometry, or as plain lines between items, and whether to label each item with its number and kind, at a size the labelsize setting gives. Those and the module's settings end up agreeing whichever way a change crosses between them, and a view opened later is told what they are. The labels are the 2D map's, which both now take from mp_util. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/modules/lib/mp_util.py | 26 + MAVProxy/modules/mavproxy_map/__init__.py | 22 +- MAVProxy/modules/mavproxy_map3d/__init__.py | 485 ++++++++- MAVProxy/modules/mavproxy_map3d/elements.py | 176 ++- MAVProxy/modules/mavproxy_map3d/map3d.py | 32 +- MAVProxy/modules/mavproxy_map3d/map3d_ui.py | 93 +- MAVProxy/modules/mavproxy_map3d/terrain.py | 33 +- tests/conftest.py | 144 +++ tests/test_map3d_mission_labels.py | 220 ++++ tests/test_map3d_settings_sync.py | 238 ++++ tests/test_mission_geometry.py | 97 +- tests/test_plane_track.py | 1086 +++++++++++++++++++ 12 files changed, 2561 insertions(+), 91 deletions(-) create mode 100644 tests/test_map3d_mission_labels.py create mode 100644 tests/test_map3d_settings_sync.py diff --git a/MAVProxy/modules/lib/mp_util.py b/MAVProxy/modules/lib/mp_util.py index 43c3a3a618..706f5cbbe8 100644 --- a/MAVProxy/modules/lib/mp_util.py +++ b/MAVProxy/modules/lib/mp_util.py @@ -199,6 +199,32 @@ def arc_points(latlon1, latlon2, arc_angle, steps=None): TAKEOFF_COMMANDS = (22, 84) # NAV_TAKEOFF, NAV_VTOL_TAKEOFF +# what each kind of mission item is called where a map labels one, as the 2D +# map has always labelled its waypoints. Numeric for the same reason as +# TAKEOFF_COMMANDS above +MISSION_LABEL_SUFFIXES = { + 22: "TOff", # NAV_TAKEOFF + 189: "DLS", # DO_LAND_START + 82: "SW", # NAV_SPLINE_WAYPOINT + MAV_CMD_NAV_ARC_WAYPOINT: "AW", + 17: "LU", # NAV_LOITER_UNLIM + 18: "LT", # NAV_LOITER_TURNS + 19: "LTime", # NAV_LOITER_TIME + 31: "LAlt", # NAV_LOITER_TO_ALT + MAV_CMD_DO_ORBIT: "Orbit", + 85: "VL", # NAV_VTOL_LAND +} + + +def mission_item_label(seq, command): + """the label a map puts on a mission item: its number, and what it is + where that is worth saying -- "3(DLS)" for a land start""" + suffix = MISSION_LABEL_SUFFIXES.get(command) + if suffix is None: + return str(seq) + return "%s(%s)" % (seq, suffix) + + # vehicles which hold position at a loiter point rather than flying a circle # around it. MAVProxy's own vehicle type names are accepted as well as the # MAV_TYPEs, since that is what a live module has to hand diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 3832f98c00..7017b5aa45 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -10,6 +10,7 @@ import functools import time import datetime +import types from MAVProxy.modules.lib import mp_util from MAVProxy.modules.lib import mp_settings from MAVProxy.modules.lib import mp_module @@ -165,18 +166,10 @@ def __init__(self, mpstate): # other commands mavutil.mavlink.MAV_CMD_DO_LAND_START: (255, 127, 0), } - self._label_suffix_for_wp_command = { - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: "TOff", - mavutil.mavlink.MAV_CMD_DO_LAND_START: "DLS", - mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT: "SW", - mp_util.MAV_CMD_NAV_ARC_WAYPOINT: "AW", - mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM: "LU", - mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS: "LT", - mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME: "LTime", - mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: "LAlt", - mp_util.MAV_CMD_DO_ORBIT: "Orbit", - mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: "VL", - } + # what each kind of item is called, which the 3D map labels with too, + # so it is not to be changed from here + self._label_suffix_for_wp_command = types.MappingProxyType( + mp_util.MISSION_LABEL_SUFFIXES) self.add_menu(MPMenuSubMenu('Terrain', items=[ MPMenuItem('Show Contours', returnkey='showTerrainContours'), @@ -466,10 +459,7 @@ def label_for_waypoint(self, wp_num, wploader=None): if wploader is None: wploader = get_mission_for_map(self.mpstate) wp = wploader.wp(wp_num) - command = wp.command - if command not in self._label_suffix_for_wp_command: - return str(wp_num) - return str(wp_num) + "(" + self._label_suffix_for_wp_command[command] + ")" + return mp_util.mission_item_label(wp_num, wp.command) def display_waypoints(self): '''display the waypoints''' diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index f9db04fd68..ed2bcc0b0b 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -4,6 +4,8 @@ mission, fence, rally, vehicle) for both live telemetry and log review. Andrew Tridgell / CanberraUAV + +AP_FLAKE8_CLEAN ''' import math @@ -18,7 +20,8 @@ from MAVProxy.modules.lib import mp_util from MAVProxy.modules.mavproxy_misseditor import get_mission_for_map from MAVProxy.modules.mavproxy_map3d.map3d import ( - Map3D, MissionItem, missing_packages, missing_packages_message) + Map3D, MissionItem, MISSION_LABEL_SIZE, MISSION_LABEL_SIZES, + MISSION_STYLES, missing_packages, missing_packages_message) # fence colours as the 2D map's PolyFence layer uses them (OpenCV BGR) FENCE_INCLUSION_BGR = (0, 255, 0) @@ -26,6 +29,17 @@ FENCE_EXCLUSION_BGR = (255, 0, 0) FENCE_RETURN_BGR = (255, 127, 127) FENCE_RETURN_RADIUS = 10.0 +# a plane's mission is flown again when the way it points on the ground turns +# this far, but not more often than this +TAKEOFF_HEADING_CHANGE = 5.0 +TAKEOFF_HEADING_REDRAW = 2.0 +# nor for home moving less than this, in metres, which it does by the GPS +# wandering while ArduPlane keeps setting it before arming +HOME_MOVE_CHANGE = 10.0 +# a VTOL landing approach is flown into the wind, whose direction the +# vehicle's estimate wanders about: a mission landing so is flown again for +# a change of this many degrees +APPROACH_CHANGE = 10.0 def bgr_to_rgb(bgr, default=(1.0, 0.0, 1.0)): @@ -36,24 +50,40 @@ def bgr_to_rgb(bgr, default=(1.0, 0.0, 1.0)): return default +def make_settings(): + '''the module's settings. The view's own controls set some of them: + showlabels, missionpath, follow and the render settings''' + return mp_settings.MPSettings([ + ('service', str, 'MicrosoftSat'), + ('zexag', float, 1.0), + ('debug', bool, False), + mp_settings.MPSetting('fpvfov', float, 90.0, range=(20.0, 150.0)), + mp_settings.MPSetting('terrainbrightness', float, 1.25, + range=(0.25, 2.0)), + ('terrainshading', bool, True), + ('terrainwireframe', bool, False), + ('showdirection', bool, True), + ('showlabels', bool, False), + mp_settings.MPSetting('labelsize', int, MISSION_LABEL_SIZE, + range=MISSION_LABEL_SIZES), + # flown, geometry or plain: see Map3D.set_mission_style() + mp_settings.MPSetting('missionpath', str, MISSION_STYLES[0], + choice=MISSION_STYLES), + ]) + + class Map3DModule(mp_module.MPModule): + # the EKF origin's altitude, which a rally point may be measured from, + # until the vehicle says where it is + origin_amsl = None + def __init__(self, mpstate): # Do not register as a public "map*" module. ADS-B, AIS, KML and other # modules use that wildcard for the 2D SlipMap API (add_object, # remove_object, set_position), which Map3D intentionally does not # implement. super().__init__(mpstate, "map3d", "3D map display") - self.map3d_settings = mp_settings.MPSettings([ - ('service', str, 'MicrosoftSat'), - ('zexag', float, 1.0), - ('debug', bool, False), - mp_settings.MPSetting('fpvfov', float, 90.0, range=(20.0, 150.0)), - mp_settings.MPSetting('terrainbrightness', float, 1.25, - range=(0.25, 2.0)), - ('terrainshading', bool, True), - ('terrainwireframe', bool, False), - ('showdirection', bool, True), - ]) + self.map3d_settings = make_settings() self.add_command('map3d', self.cmd_map3d, "3D map control", ['', 'set (MAP3DSETTING)']) @@ -68,6 +98,17 @@ def __init__(self, mpstate): self.last_attitude = (0.0, 0.0, 0.0) self.home_amsl = None self.home_position = None + # whether the vehicle is armed, and the way it last pointed while it + # was not. A fixed-wing takeoff holds the ground course the vehicle + # has once it gets moving, which is not known until it does: this + # is the best there is before then, and wrong wherever a crosswind, + # a turn on the ground or a compass error has it moving some other + # way + self.armed = False + self.ground_heading = None + self.ground_heading_changed = False + self.ground_heading_redrawn = 0 + self.reset_flown_track() self.icon_type = None self.follow = True self.kml_change_state = None @@ -101,13 +142,17 @@ def cmd_map3d(self, args): self.map.center_on_vehicle() elif cmd == "set": self.map3d_settings.command(args[1:]) + if (len(args) > 1 and args[1] == 'missionpath' and + self.map3d_settings.missionpath == 'flown'): + self.flown_asked = True if self.map is not None and self.map.is_alive(): - self.map.set_fpv_fov(self.map3d_settings.fpvfov) self.map.set_mission_arrows(self.map3d_settings.showdirection) - self.map.set_render_settings( - self.map3d_settings.terrainbrightness, - self.map3d_settings.terrainshading, - self.map3d_settings.terrainwireframe) + self.map.set_mission_labels(self.map3d_settings.showlabels) + self.map.set_mission_label_size(self.map3d_settings.labelsize) + self.map.set_mission_style(self.map3d_settings.missionpath) + # the path flown is only worked out while it is drawn + self.send_mission() + self.send_render_settings() else: print("unknown map3d command: %s" % cmd) @@ -130,12 +175,22 @@ def start_map(self): follow=self.follow) # push whatever we already know self.map.set_mission_arrows(self.map3d_settings.showdirection) + self.map.set_mission_labels(self.map3d_settings.showlabels) + self.map.set_mission_label_size(self.map3d_settings.labelsize) + self.map.set_mission_style(self.map3d_settings.missionpath) self.send_mission() self.send_fence() self.send_rally() self.send_cached_state() self.send_kml() + def send_render_settings(self): + self.map.set_fpv_fov(self.map3d_settings.fpvfov) + self.map.set_render_settings( + self.map3d_settings.terrainbrightness, + self.map3d_settings.terrainshading, + self.map3d_settings.terrainwireframe) + def stop_map(self): if self.map is not None: self.map.close() @@ -169,6 +224,10 @@ def send_cached_state(self): (attitude.Roll, attitude.Pitch, attitude.Yaw)) + origin = messages.get('GPS_GLOBAL_ORIGIN') or messages.get('ORGN') + if origin is not None: + self.mavlink_packet(origin, force=True) + home = messages.get('HOME_POSITION') if home is not None: self.home_amsl = home.altitude * 1.0e-3 @@ -301,11 +360,31 @@ def send_mission(self): if wploader is None: return items = [] + # every item after home, for flying the mission through, and which + # of their altitudes are absolute rather than moving with home: True + # for one of its own, False for one above home, and None for an item + # whose altitude neither moves nor stays, having none to fly at + flown = [] + fixed_alt = [] default_radius = self.default_circle_radius() previous = None for w in wploader.wpoints: frame = getattr(w, 'frame', 0) (lat, lon) = (w.x, w.y) + if w.seq != 0 and (lat == 0 and lon == 0 and + w.command not in mp_util.TAKEOFF_COMMANDS): + from MAVProxy.modules.lib import plane_track + amsl = plane_track.positionless_amsl(w.z, frame, + self.home_amsl) + flown.append((w.command, 0.0, 0.0, amsl, + (w.param1, w.param2, w.param3, w.param4))) + if (amsl is None or + not plane_track.is_navigation_command(w.command)): + # flown at the altitude the aircraft is at, or not + # flown at all + fixed_alt.append(None) + else: + fixed_alt.append(frame in (0, 5)) if lat == 0 and lon == 0 and w.command in mp_util.TAKEOFF_COMMANDS: # draw the climb from home, otherwise the takeoff altitude is # dropped and the mission appears to start at the first waypoint @@ -351,8 +430,261 @@ def send_mission(self): items.append(MissionItem(lat, lon, z, frame, w.command, w.seq, w.param1, circle_radius, circle_turns, exit_converge)) + if w.seq != 0: + flown.append((w.command, lat, lon, amsl, params)) + # a terrain item resolved above has frame 0 by now: its + # altitude is where it is, whatever home does + fixed_alt.append(frame in (0, 5)) previous = (lat, lon, amsl) - self.map.set_mission(items) + self.mission_sent = items + self.map.set_mission( + items, self.flown_track(wploader, flown, fixed_alt)) + + def flight_parameters(self): + '''the vehicle's parameters which the path a mission is flown along + is worked out from, as far as they are known''' + from MAVProxy.modules.lib import plane_track + return dict((name, mp_util.param_value(self.mav_param, name)) + for (names, _) in plane_track.PARAMETERS.values() + for (name, _) in names) + + def reset_flown_track(self): + '''forget any plane mission flown. Flying one can take a second or + more, so it is done on a thread of its own rather than the main one: + the mission is drawn at once, and again with the path flown when the + thread has worked it out''' + # the parameters the mission was last drawn flown with, so a change + # to one of them -- or the first of them to arrive -- draws it again + self.flown_params = None + self.track_lock = threading.Lock() + # the last path worked out, as (key, track, home), so an unchanged + # mission is not flown again + self.plane_track = (None, None, None) + # for the thread: the latest mission to fly, as (key, home, items, + # params, heading, rally), and the thread while it is running + self.track_request = None + self.track_thread = None + # from the thread, for the idle task to draw: (key, track, home) + self.track_result = None + # (key, flown_from, home, items, alt_moves) of the mission on screen + # while it waits for its path: the home it is being flown from, the + # home and items it has now, and whether any of its altitudes move + # with home + self.track_wanted = None + self.mission_sent = None + # the mission last found to have no path flown, so it is said once + self.unflown_key = None + # whether the path flown has been asked for by name since it was + # last said it could not be drawn for this vehicle + self.flown_asked = False + # the course of a VTOL landing approach, into the wind the vehicle + # last said it estimates, to APPROACH_CHANGE; and whether the path + # on screen was flown with it + self.approach = None + self.approach_used = False + + @staticmethod + def same_home(home, other): + '''whether two homes are near enough to fly a mission from alike''' + return (other is not None and + mp_util.gps_distance(home[0], home[1], + other[0], other[1]) < HOME_MOVE_CHANGE and + abs(home[2] - other[2]) < HOME_MOVE_CHANGE) + + def flown_track(self, wploader, items, fixed_alt): + '''the path a plane flies the mission along, where it has already + been worked out; otherwise None, and the thread is asked to work it + out. None too for any other vehicle, where the mission cannot be + flown through, or where the map is not drawing the path flown''' + if self.map3d_settings.missionpath != 'flown': + self.track_wanted = None + return None + if self.vehicle_type != 'plane': + if self.flown_asked and self.vehicle_type is not None: + # asked for by name, rather than left as it starts + print("map3d: only a plane's mission can be drawn as the " + "path flown; drawing its geometry") + self.flown_asked = False + self.track_wanted = None + return None + expected = getattr(wploader, 'expected_count', None) + if expected and wploader.count() < expected: + # a download still coming in: the items so far are drawn, but + # flying them would end the mission where it has got to, with a + # return to launch or a landing the rest of it may say nothing of + self.track_wanted = None + return None + home = self.mission_home(wploader) + if home is None or self.home_amsl is None: + self.track_wanted = None + return None + from MAVProxy.modules.lib import plane_track + home = (home[0], home[1], self.home_amsl) + # copied here, since the thread must not read them as they change + params = self.flight_parameters() + + # the mission as it stands relative to home, so the same mission from + # a home which has only wandered a little is still the same mission. + # An altitude of its own stands as it is, since home does not move it + def keyed_alt(amsl, fixed): + if amsl is None or fixed is None: + return None + return round(amsl if fixed else amsl - home[2], 2) + relative = tuple( + (command, + 'home' if (lat, lon) == home[:2] else (lat, lon), + keyed_alt(amsl, fixed), + tuple(params_of_item)) + for ((command, lat, lon, amsl, params_of_item), fixed) + in zip(items, fixed_alt)) + points = self.rally_points(home) + rally = [point[:3] for point in points] + moves = [not fixed for fixed in fixed_alt if fixed is not None] + moves += [not fixed for (_, _, _, fixed) in points] + # where some of it moves with home and the rest does not, the path + # cannot be moved to fit a home which has: it is moved up or down + # with home all the same while home is near enough to be the same + # one, which puts what does not move out by no more than that, and + # flown again beyond + key = (relative, tuple(sorted(params.items())), self.ground_heading, + tuple((lat, lon, keyed_alt(amsl, fixed)) + for (lat, lon, amsl, fixed) in points)) + # only a mission which lands on a VTOL approach is flown again as + # the wind changes + approach = None + self.approach_used = plane_track.uses_vtol_approach(items, params) + if self.approach_used: + approach = self.approach + key += (approach,) + (cached_key, cached_track, cached_home) = self.plane_track + if cached_key == key and self.same_home(home, cached_home): + self.track_wanted = None + return self.moved_home(cached_track, cached_home, home, items, + any(moves)) + wanted = self.track_wanted + if wanted is not None and wanted[0] == key and self.same_home(home, wanted[1]): + # already being flown, from a home near enough: drawn, when it + # has been, from where home is now + self.track_wanted = (key, wanted[1], home, items, any(moves)) + return None + self.track_wanted = (key, home, home, items, any(moves)) + with self.track_lock: + self.track_request = (key, home, list(items), params, + self.ground_heading, rally, approach) + if self.track_thread is None: + self.track_thread = threading.Thread(target=self.fly_tracks, + daemon=True) + self.track_thread.start() + return None + + def rally_points(self, home): + '''the vehicle's rally points, as (lat, lon, amsl, fixed_alt), for a + return to launch to go to. fixed_alt is a point whose altitude is + its own rather than one which moves with home''' + from MAVProxy.modules.lib import plane_track + try: + loader = self.module('rally').rallyloader + points = [loader.rally_point(i) for i in range(loader.rally_count())] + except Exception: + return [] + out = [] + for r in points: + flags = getattr(r, 'flags', 0) + frame = plane_track.rally_alt_frame(flags) + (lat, lon) = (r.lat * 1.0e-7, r.lng * 1.0e-7) + terrain = None + if frame == plane_track.RALLY_ALT_ABOVE_TERRAIN: + terrain = self.terrain_alt(lat, lon) + # the EKF origin is where the vehicle first had a position, which + # is usually home and near enough to it until it says otherwise + origin = self.origin_amsl + fixed = plane_track.rally_alt_is_fixed(flags) + if frame == plane_track.RALLY_ALT_ABOVE_ORIGIN and origin is None: + (origin, fixed) = (home[2], False) + out.append((lat, lon, + plane_track.rally_amsl(r.alt, flags, home[2], + origin, terrain), + fixed)) + return out + + @staticmethod + def moved_home(track, flown_home, home, items, alt_moves=True): + '''a path flown from flown_home, moved to start from home, which is + near enough not to fly it again. Its altitudes move with home where + the mission's do, and its start moves with home too, less and less + along the way to the first item with a position of its own, which + does not move''' + if track is None or flown_home == home: + return track + dlat = home[0] - flown_home[0] + dlon = (home[1] - flown_home[1] + 180.0) % 360.0 - 180.0 + dalt = home[2] - flown_home[2] if alt_moves else 0.0 + reach = 0.0 + for (command, lat, lon, amsl, params) in items: + if (lat, lon) != (0.0, 0.0) and (lat, lon) != home[:2]: + reach = mp_util.gps_distance(flown_home[0], flown_home[1], + lat, lon) + break + moved = [] + travelled = 0.0 + previous = None + for (lat, lon, amsl) in track: + if previous is not None: + travelled += mp_util.gps_distance(previous[0], previous[1], + lat, lon) + previous = (lat, lon) + weight = max(0.0, 1.0 - travelled / reach) if reach > 0 else 0.0 + moved.append((lat + dlat * weight, lon + dlon * weight, + amsl + dalt)) + return moved + + def fly_tracks(self): + '''the thread flying plane missions: the latest asked for, until there + is none left to fly''' + from MAVProxy.modules.lib import plane_track + while True: + with self.track_lock: + request = self.track_request + self.track_request = None + if request is None: + self.track_thread = None + return + (key, home, items, params, heading, rally, approach) = request + try: + track = plane_track.mission_track(home, items, params, + heading, rally=rally, + approach=approach) + except Exception as ex: + # drawn from its items instead; the thread carries on + print("map3d: could not fly the mission: %s" % ex) + track = None + with self.track_lock: + self.track_result = (key, track, home) + + def draw_flown_track(self): + '''draw the path the thread has flown, if it is for the mission on + screen. Called from the idle task, on the main thread''' + with self.track_lock: + result = self.track_result + self.track_result = None + if result is None: + return + self.plane_track = result + (key, track, home) = result + wanted = self.track_wanted + if wanted is None or wanted[0] != key or home != wanted[1]: + # another mission since, which the thread has yet to fly + return + self.track_wanted = None + if track is None and key != self.unflown_key: + self.unflown_key = key + print("map3d: could not work out the path this mission is flown " + "along -- it may be too long, never finish an item, or use " + "a command which cannot be flown here; drawing its geometry") + if self.map is not None and self.mission_sent is not None: + self.map.set_mission(self.mission_sent, + self.moved_home(track, home, wanted[2], + wanted[3], wanted[4])) def set_icon_type(self, name): '''vehicle type changed: the viewer picks its icon from it. Not named @@ -367,10 +699,11 @@ def set_home_position(self, lat, lon): '''home moved: around-home fence circles are centred on it, and a positionless takeoff is drawn there''' if (lat, lon) == self.home_position: - return + return False self.home_position = (lat, lon) self.send_fence() self.send_mission() + return True @staticmethod def _fence_latlon(item): @@ -425,15 +758,18 @@ def send_fence(self): self.map.set_fence(shapes) def send_rally(self): + '''draw the rally points at the altitude a return to launch goes to + each at, where that can be worked out, and above home otherwise''' + if self.map is None: + return try: - rally_mod = self.module('rally') - pts = [] - for i in range(rally_mod.rallyloader.rally_count()): - r = rally_mod.rallyloader.rally_point(i) - pts.append((r.lat * 1.0e-7, r.lng * 1.0e-7, r.alt)) + loader = self.module('rally').rallyloader + raw = [loader.rally_point(i) for i in range(loader.rally_count())] except Exception: return - self.map.set_rally(pts) + resolved = self.rally_points((0.0, 0.0, self.home_amsl)) + self.map.set_rally([(lat, lon, amsl, r.alt) for + ((lat, lon, amsl, _), r) in zip(resolved, raw)]) @staticmethod def _kml_state(kml_mod): @@ -478,14 +814,30 @@ def idle_task(self): for event in self.map.check_events(): if event[0] == 'startup_error': print("map3d: the 3D view failed to start:\n%s" % event[1]) + # what the view's own controls were set to. Each is sent back + # as the setting now stands: a command given here while the + # event was on its way has already been sent to the view, and + # would otherwise be overwritten there by this older event + # while the setting kept it elif event[0] == 'render_settings': (_, brightness, shading, wireframe, fpvfov) = event self.map3d_settings.terrainbrightness = brightness self.map3d_settings.terrainshading = shading self.map3d_settings.terrainwireframe = wireframe self.map3d_settings.fpvfov = fpvfov + self.send_render_settings() elif event[0] == 'follow': self.follow = bool(event[1]) + self.map.set_follow(self.follow) + elif event[0] == 'mission_labels': + self.map3d_settings.showlabels = bool(event[1]) + self.map.set_mission_labels(self.map3d_settings.showlabels) + elif event[0] == 'mission_style': + self.map3d_settings.missionpath = event[1] + self.flown_asked = event[1] == 'flown' + self.map.set_mission_style(self.map3d_settings.missionpath) + # the path flown is only worked out while it is drawn + self.send_mission() if not alive: self.map = None return @@ -512,12 +864,52 @@ def idle_task(self): if rally_change != self.rally_change_time: self.rally_change_time = rally_change self.send_rally() + # a return to launch may go to one + self.send_mission() except Exception: pass if self.terrain_resolved: # a deferred terrain lookup landed: redo the terrain-frame items self.terrain_resolved = False self.send_mission() + self.send_rally() + try: + params = self.flight_parameters() + except Exception: + params = self.flown_params + if params != self.flown_params: + # a parameter the path is flown by has changed, or arrived for + # the first time: how the vehicle flies the mission has changed + # with it + self.flown_params = params + self.send_mission() + self.redraw_for_ground_heading() + self.draw_flown_track() + + def redraw_for_ground_heading(self, now=None): + '''fly the mission again from the way the vehicle points now, if that + has changed, and not too often''' + if not self.ground_heading_changed: + return + if now is None: + now = time.time() + if now - self.ground_heading_redrawn < TAKEOFF_HEADING_REDRAW: + return + self.ground_heading_changed = False + self.ground_heading_redrawn = now + if self.vehicle_type == 'plane': + self.send_mission() + + def note_ground_heading(self, yaw): + '''the vehicle points yaw radians while disarmed. A change of less + than TAKEOFF_HEADING_CHANGE is not worth flying the mission again''' + heading = round(math.degrees(yaw)) % 360 + if self.ground_heading is not None: + change = abs((heading - self.ground_heading + 180) % 360 - 180) + if change < TAKEOFF_HEADING_CHANGE: + return + self.ground_heading = heading + self.ground_heading_changed = True def mavlink_packet(self, m, force=False): if self.map is None or not self.map.is_alive(): @@ -525,12 +917,51 @@ def mavlink_packet(self, m, force=False): mtype = m.get_type() if mtype in ('HEARTBEAT', 'HIGH_LATENCY2'): self.set_icon_type(mp_util.vehicle_type_name(m.type)) + if (mtype == 'HEARTBEAT' and + m.autopilot != mavutil.mavlink.MAV_AUTOPILOT_INVALID): + self.armed = bool(m.base_mode & + mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + elif mtype == 'GPS_GLOBAL_ORIGIN': + # the EKF origin, which a rally point's altitude may be above + origin = m.altitude * 1.0e-3 # AMSL (mm -> m) + if origin != self.origin_amsl: + self.origin_amsl = origin + self.send_rally() + self.send_mission() + elif mtype == 'ORGN': + if m.Type == 0 and m.Alt != self.origin_amsl: + self.origin_amsl = m.Alt + self.send_rally() + self.send_mission() + elif mtype == 'WIND' and math.isfinite(m.direction): + # where the wind comes from, which is the course ArduPlane + # works out for a VTOL landing approach, by the same sum. An + # EKF with no estimate sends NaN, which says nothing + approach = (round(m.direction / APPROACH_CHANGE) * + APPROACH_CHANGE) + approach = (approach + 180.0) % 360.0 - 180.0 + if approach != self.approach: + self.approach = approach + if self.approach_used: + self.send_mission() elif mtype == 'HOME_POSITION': - self.home_amsl = m.altitude * 1.0e-3 # AMSL (mm -> m) + home_amsl = m.altitude * 1.0e-3 # AMSL (mm -> m) + moved = home_amsl != self.home_amsl + if moved: + self.home_amsl = home_amsl + # rally points above home, or the origin in its place + self.send_rally() self.map.set_home(self.home_amsl) - self.set_home_position(m.latitude * 1.0e-7, m.longitude * 1.0e-7) + # a mission is flown from home, and everything above home is as + # high as home is, so it is drawn again for a home which has + # moved up or down as much as for one which has moved along + if (not self.set_home_position(m.latitude * 1.0e-7, + m.longitude * 1.0e-7) and moved): + self.send_mission() elif mtype == 'ATTITUDE': self.last_attitude = (m.roll, m.pitch, m.yaw) + if not self.armed: + self.note_ground_heading(m.yaw) elif mtype == 'ATT': # ArduPilot DataFlash attitude is recorded in degrees. self.last_attitude = tuple(math.radians(v) for v in diff --git a/MAVProxy/modules/mavproxy_map3d/elements.py b/MAVProxy/modules/mavproxy_map3d/elements.py index 0f8d6792a2..e9cd0eb51a 100644 --- a/MAVProxy/modules/mavproxy_map3d/elements.py +++ b/MAVProxy/modules/mavproxy_map3d/elements.py @@ -10,7 +10,8 @@ import vtk from MAVProxy.modules.lib import mp_util -from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem +from MAVProxy.modules.mavproxy_map3d.map3d import ( + MissionItem, MISSION_LABEL_SIZE, MISSION_LABEL_SIZES, MISSION_STYLES) from MAVProxy.modules.mavproxy_map3d.terrain import enu, R, wrap_longitude # MAV_FRAME altitude conventions @@ -376,6 +377,20 @@ def _arrows(points_enu, colour, renderer=None, count=MISSION_ARROW_COUNT): return actor +def _label(point_enu, text, colour, size=MISSION_LABEL_SIZE): + """text at a point in the world, facing the screen and the same size on + it however far away it is""" + actor = vtk.vtkBillboardTextActor3D() + actor.SetInput(str(text)) + actor.SetPosition(*point_enu) + prop = actor.GetTextProperty() + prop.SetColor(*colour) + prop.SetFontSize(size) + prop.SetJustificationToLeft() + prop.SetVerticalJustificationToBottom() + return actor + + def _points(points_enu, colour, size): vpts = vtk.vtkPoints() verts = vtk.vtkCellArray() @@ -412,13 +427,23 @@ def __init__(self, renderer, lat0, lon0, zexag): self.vehicle_type = DEFAULT_VEHICLE_TYPE self.terrain_height = None self.fence = [] + self.rally = [] self.fence_geometry = None self.kml_features = [] self.kml_geometry = None self.kml_height_cache = {} self.mission_line = [] + self.mission_rings = [] self.mission_markers = [] + # the label of each item drawn, as (marker, label) + self.mission_labels = [] self.mission_arrows = False + self.mission_labelled = False + self.mission_label_size = MISSION_LABEL_SIZE + # the mission last given, so it can be drawn again in another style + self.mission_items = [] + self.mission_track = None + self.mission_style = MISSION_STYLES[0] def _enu(self, lat, lon, amsl): e, n, u = enu(lat, lon, amsl, self.lat0, self.lon0) @@ -441,6 +466,8 @@ def set_home(self, amsl): self.home_amsl = amsl if self.fence: self.refresh_fence() + if self.rally: + self.refresh_rally() if self.kml_features: self.refresh_kml() @@ -510,36 +537,92 @@ def add_trail_point(self, lat, lon, amsl): if len(self.trail) >= 2: self._replace('trail', [_polyline(self.trail, (1.0, 1.0, 0.0), 2.0)]) - def set_mission(self, items): - '''items: list of MissionItem (plain tuples are accepted too). - - The line drawn is the path the vehicle is expected to fly, so it is - continuous throughout: an arc waypoint curves, and an item which - circles about its location is entered from the near side of its - circle and left again where it comes off, rather than the line - running to the middle of the circle where the vehicle never goes. - The markers stay on the mission item locations + def set_mission(self, items, track=None): + '''items: list of MissionItem (plain tuples are accepted too). track + is the path the vehicle is expected to fly them along, as (lat, lon, + amsl) points, where the caller has worked it out by flying the + mission through the vehicle's own navigation. How the mission is + drawn from those is the mission style: see set_mission_style() ''' + self.mission_items = [MissionItem(*item) for item in items] + self.mission_track = list(track) if track else None + self.draw_mission() + + def set_mission_style(self, style): + '''draw the mission as one of MISSION_STYLES. flown draws the track + the mission was given with, and geometry where it came without one: + the path worked out from the items, continuous throughout, where an + arc waypoint curves and an item which circles about its location is + joined and left along tangents rather than the line running to the + middle of a circle the vehicle never goes to. plain runs the line + straight from item to item, arcs apart, with a ring at the altitude + of each item which circles''' + if style not in MISSION_STYLES or style == self.mission_style: + return + self.mission_style = style + self.draw_mission() + + def draw_mission(self): + '''draw the mission last given in the mission style. The markers stay + on the mission item locations''' + flown = [i for i in self.mission_items + if not (i.lat == 0 and i.lon == 0)] + self.mission_markers = [ + self._enu(i.lat, i.lon, self._resolve_amsl(i.alt, i.frame)) + for i in flown] + self.mission_labels = [ + (marker, mp_util.mission_item_label(item.seq, item.command)) + for (marker, item) in zip(self.mission_markers, flown)] + self.mission_rings = [] + if self.mission_style == 'flown' and self.mission_track: + self.mission_line = [self._enu(lat, lon, amsl) + for (lat, lon, amsl) in self.mission_track] + elif self.mission_style == 'plain': + self.mission_line = self._plain_line(flown) + else: + self.mission_line = self._geometry_line(flown) + self.refresh_mission() + + def _arc_into(self, line, item, amsl, previous): + '''add the leg into an arc waypoint to the line: a circular arc rather + than a straight line, climbing linearly along it''' + if (item.command != mp_util.MAV_CMD_NAV_ARC_WAYPOINT or + previous is None): + return + ((prev_lat, prev_lon), prev_amsl) = previous + arc = mp_util.arc_points((prev_lat, prev_lon), + (item.lat, item.lon), item.param1) + for i in range(1, len(arc) - 1): + fraction = float(i) / (len(arc) - 1) + line.append(self._enu(arc[i][0], arc[i][1], + prev_amsl + (amsl - prev_amsl) * fraction)) + + def _plain_line(self, flown): + '''the line straight through the items, and a ring for each which + circles, at its own altitude rather than draped over the terrain''' + line = [] + previous = None + for item in flown: + amsl = self._resolve_amsl(item.alt, item.frame) + self._arc_into(line, item, amsl, previous) + line.append(self._enu(item.lat, item.lon, amsl)) + if item.circle_radius: + ring = circle_latlon((item.lat, item.lon), + abs(item.circle_radius), + MISSION_CIRCLE_SEGMENTS) + ring = [self._enu(la, lo, amsl) for (la, lo) in ring] + self.mission_rings.append(ring + ring[:1]) + previous = ((item.lat, item.lon), amsl) + return line + + def _geometry_line(self, flown): + '''the path worked out from the items alone''' line = [] - markers = [] previous = None rejoin = None - flown = [MissionItem(*item) for item in items] - flown = [i for i in flown if not (i.lat == 0 and i.lon == 0)] for (index, item) in enumerate(flown): amsl = self._resolve_amsl(item.alt, item.frame) - if (item.command == mp_util.MAV_CMD_NAV_ARC_WAYPOINT and - previous is not None): - # the leg into an arc waypoint is a circular arc rather - # than a straight line; climb linearly along it - ((prev_lat, prev_lon), prev_amsl) = previous - arc = mp_util.arc_points((prev_lat, prev_lon), - (item.lat, item.lon), item.param1) - for i in range(1, len(arc) - 1): - fraction = float(i) / (len(arc) - 1) - line.append(self._enu(arc[i][0], arc[i][1], - prev_amsl + (amsl - prev_amsl) * fraction)) - markers.append(self._enu(item.lat, item.lon, amsl)) + self._arc_into(line, item, amsl, previous) if item.circle_radius: previous = self._append_circle( line, item, amsl, previous, @@ -557,9 +640,7 @@ def set_mission(self, items): rejoin = None line.append(self._enu(item.lat, item.lon, amsl)) previous = ((item.lat, item.lon), amsl) - self.mission_line = line - self.mission_markers = markers - self.refresh_mission() + return line def set_mission_arrows(self, enable): '''show or hide the direction of travel along the mission''' @@ -569,6 +650,24 @@ def set_mission_arrows(self, enable): self.mission_arrows = enable self.refresh_mission() + def set_mission_labels(self, enable): + '''show or hide the label of each mission item''' + enable = bool(enable) + if enable == self.mission_labelled: + return + self.mission_labelled = enable + self.refresh_mission() + + def set_mission_label_size(self, size): + '''how big those labels are drawn, in points on the screen''' + (smallest, largest) = MISSION_LABEL_SIZES + size = min(largest, max(smallest, int(size))) + if size == self.mission_label_size: + return + self.mission_label_size = size + if self.mission_labelled: + self.refresh_mission() + def refresh_mission(self): '''rebuild the mission actors from the last mission drawn''' line = self.mission_line @@ -579,8 +678,14 @@ def refresh_mission(self): arrows = _arrows(line, (1.0, 1.0, 1.0), self.ren) if arrows is not None: actors.append(arrows) + for ring in self.mission_rings: + actors.append(_polyline(ring, (1.0, 1.0, 1.0), 2.0, dashed=True)) if self.mission_markers: actors.append(_points(self.mission_markers, (1.0, 1.0, 1.0), 9)) + if self.mission_labelled: + actors += [_label(marker, text, (1.0, 1.0, 1.0), + self.mission_label_size) + for (marker, text) in self.mission_labels] self._replace('mission', actors) def _append_rejoin(self, line, rejoin, target, target_amsl): @@ -803,11 +908,20 @@ def set_kml(self, features, terrain_height=None, refresh=True): self.refresh_kml() def set_rally(self, pts): - '''pts: list of (lat,lon,alt_rel)''' - if not pts: + '''pts: list of (lat, lon, amsl, alt): the AMSL altitude a return to + launch goes to the point at, or None where that is not known, and + the altitude the point carries, which is taken to be above home + where there is nothing better''' + self.rally = list(pts) + self.refresh_rally() + + def refresh_rally(self): + if not self.rally: self._replace('rally', []) return - markers = [self._enu(lat, lon, self.home_amsl + alt) for (lat, lon, alt) in pts] + markers = [self._enu(lat, lon, + self.home_amsl + alt if amsl is None else amsl) + for (lat, lon, amsl, alt) in self.rally] self._replace('rally', [_points(markers, (0.4, 0.8, 1.0), 12)]) def refresh_vehicle(self): diff --git a/MAVProxy/modules/mavproxy_map3d/map3d.py b/MAVProxy/modules/mavproxy_map3d/map3d.py index f5b4c587b8..5f5b185527 100644 --- a/MAVProxy/modules/mavproxy_map3d/map3d.py +++ b/MAVProxy/modules/mavproxy_map3d/map3d.py @@ -24,6 +24,18 @@ 'exit_converge', defaults=(0.0, None, None, None)) +# how the mission line is drawn. flown is the path the vehicle is worked +# out to fly, where the caller could work one out, and geometry where it +# could not; geometry joins and leaves circles on tangents and walks spirals +# round them; plain runs straight from item to item, with a ring at each +# item which circles +MISSION_STYLES = ('flown', 'geometry', 'plain') + +# how big a mission item's label is drawn, in points on the screen, and the +# sizes worth asking for: too small to read through to filling the view +MISSION_LABEL_SIZE = 14 +MISSION_LABEL_SIZES = (6, 48) + PACKAGES = ('vtk', 'quantized_mesh_tile') @@ -128,8 +140,12 @@ def set_time_range(self, trange): '''Limit timestamped path points to a Matplotlib date-number range.''' self._put(('time_range', None if trange is None else tuple(trange))) - def set_mission(self, items): - self._put(('mission', list(items))) + def set_mission(self, items, track=None): + '''items: MissionItems. track is the path the vehicle is expected to + fly them along, as (lat, lon, amsl) points, where the caller can + work that out; otherwise the path is drawn from the items alone''' + self._put(('mission', list(items), + None if track is None else list(track))) def set_fence(self, shapes): '''shapes: ('polygon', [(lat,lon), ...], rgb) / ('circle', (lat,lon), radius_m, rgb)''' @@ -147,6 +163,18 @@ def set_follow(self, enable): def set_mission_arrows(self, enable): self._put(('mission_arrows', bool(enable))) + def set_mission_labels(self, enable): + '''label each mission item with its number and what it is''' + self._put(('mission_labels', bool(enable))) + + def set_mission_label_size(self, size): + '''how big those labels are drawn, in points on the screen''' + self._put(('mission_label_size', int(size))) + + def set_mission_style(self, style): + '''one of MISSION_STYLES''' + self._put(('mission_style', str(style))) + def set_fpv_fov(self, fov): self._put(('fpvfov', float(fov))) diff --git a/MAVProxy/modules/mavproxy_map3d/map3d_ui.py b/MAVProxy/modules/mavproxy_map3d/map3d_ui.py index cbb26fa79f..21ac24f261 100644 --- a/MAVProxy/modules/mavproxy_map3d/map3d_ui.py +++ b/MAVProxy/modules/mavproxy_map3d/map3d_ui.py @@ -16,6 +16,8 @@ from MAVProxy.modules.mavproxy_map3d.terrain import ( TerrainManager, R, TILE_DOWNLOAD_THREADS, wrap_longitude) from MAVProxy.modules.mavproxy_map3d.elements import ElementManager +from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_STYLES) import vtk @@ -97,6 +99,13 @@ def __init__(self, state): self.overlay_mesh_revision = -1 self.fpv_enabled = False self.fpv_fov = min(150.0, max(20.0, float(state.fpvfov))) + # how the mission is drawn, as the element manager's own defaults. + # The parent pushes what it wants, which may be before there is a + # scene to draw it in, so the frame holds it for init_scene + self.mission_arrows = False + self.mission_labelled = False + self.mission_label_size = MISSION_LABEL_SIZE + self.mission_style = MISSION_STYLES[0] self.terrain_brightness = min( 2.0, max(0.25, float(state.terrain_brightness))) self.terrain_shading = bool(state.terrain_shading) @@ -128,6 +137,22 @@ def __init__(self, state): self.fpv_button.SetToolTip("Toggle level-horizon first-person view") self.Bind(wx.EVT_TOGGLEBUTTON, self.on_fpv_toggle, self.fpv_button) controls.Add(self.fpv_button, 0, wx.ALL, 4) + self.labels_check = wx.CheckBox(self, -1, "Labels") + self.labels_check.SetValue(self.mission_labelled) + self.labels_check.SetToolTip( + "Number each mission item, as the 2D map does") + self.Bind(wx.EVT_CHECKBOX, self.on_labels_toggle, self.labels_check) + controls.Add(self.labels_check, 0, + wx.ALL | wx.ALIGN_CENTER_VERTICAL, 4) + controls.Add(wx.StaticText(self, -1, "Mission"), 0, + wx.ALL | wx.ALIGN_CENTER_VERTICAL, 4) + self.style_choice = wx.Choice(self, -1, choices=list(MISSION_STYLES)) + self.style_choice.SetStringSelection(self.mission_style) + self.style_choice.SetToolTip( + "Draw the mission as the path the vehicle flies, as the geometry " + "of its items, or as plain lines from item to item") + self.Bind(wx.EVT_CHOICE, self.on_style_choice, self.style_choice) + controls.Add(self.style_choice, 0, wx.ALL, 4) gear = wx.ArtProvider.GetBitmap(wx.ART_HELP_SETTINGS, wx.ART_BUTTON, (16, 16)) self.settings_button = wx.BitmapButton(self, -1, gear) @@ -158,6 +183,10 @@ def init_scene(self, lat0, lon0, amsl_ref): shading=self.terrain_shading, wireframe=self.terrain_wireframe) self.elements = ElementManager(self.ren, lat0, lon0, self.state.zexag) + self.elements.set_mission_arrows(self.mission_arrows) + self.elements.set_mission_label_size(self.mission_label_size) + self.elements.set_mission_labels(self.mission_labelled) + self.elements.set_mission_style(self.mission_style) if self.vehicle_type is not None: self.elements.set_vehicle_type(self.vehicle_type) self.elements.set_terrain_height(self.terrain.height_at) @@ -189,6 +218,47 @@ def on_fpv_toggle(self, event): def on_follow_toggle(self, event): self.set_follow_enabled(self.follow_button.GetValue(), notify=True) + def on_labels_toggle(self, event): + self.set_mission_labels(self.labels_check.GetValue(), notify=True) + + def on_style_choice(self, event): + self.set_mission_style(self.style_choice.GetStringSelection(), + notify=True) + + def set_mission_arrows(self, enable): + self.mission_arrows = bool(enable) + if self.elements is not None: + self.elements.set_mission_arrows(self.mission_arrows) + + def set_mission_labels(self, enable, notify=False): + self.mission_labelled = bool(enable) + self.labels_check.SetValue(self.mission_labelled) + if self.elements is not None: + self.elements.set_mission_labels(self.mission_labelled) + self.render() + if notify: + self.state.event_queue.put( + ('mission_labels', self.mission_labelled)) + + def set_mission_label_size(self, size): + self.mission_label_size = int(size) + if self.elements is not None: + self.elements.set_mission_label_size(self.mission_label_size) + self.render() + + def set_mission_style(self, style, notify=False): + if style not in MISSION_STYLES: + return + self.mission_style = style + self.style_choice.SetStringSelection(self.mission_style) + if self.elements is not None: + self.elements.set_mission_style(self.mission_style) + self.render() + if notify: + # the parent draws the path flown only while it is asked for, so + # it may send the mission again rather than just take this + self.state.event_queue.put(('mission_style', self.mission_style)) + def schedule_kml_refresh(self): if self.elements is None or not self.kml_features: self.kml_refresh_due = None @@ -335,6 +405,11 @@ def handle(self, msg): if kind == 'follow': self.set_follow_enabled(msg[1]) return + if kind == 'render_settings': + (_, brightness, shading, wireframe) = msg + self.apply_render_settings((brightness, shading, wireframe, + self.fpv_fov)) + return if kind == 'vehicletype': # may arrive before the scene exists, so keep it for init_scene self.vehicle_type = msg[1] @@ -342,6 +417,20 @@ def handle(self, msg): self.elements.set_vehicle_type(self.vehicle_type) self.render() return + # how the mission is drawn may arrive before there is a scene to + # draw it in, so the frame keeps it for init_scene + if kind == 'mission_arrows': + self.set_mission_arrows(msg[1]) + return + if kind == 'mission_labels': + self.set_mission_labels(msg[1]) + return + if kind == 'mission_label_size': + self.set_mission_label_size(msg[1]) + return + if kind == 'mission_style': + self.set_mission_style(msg[1]) + return if kind == 'kml': self.kml_features = list(msg[1]) if self.elements is not None: @@ -371,9 +460,7 @@ def handle(self, msg): elif kind == 'time_range': self.elements.set_time_range(msg[1]) elif kind == 'mission': - self.elements.set_mission(msg[1]) - elif kind == 'mission_arrows': - self.elements.set_mission_arrows(msg[1]) + self.elements.set_mission(msg[1], msg[2] if len(msg) > 2 else None) elif kind == 'fence': self.elements.set_fence(msg[1], self.terrain.height_at) elif kind == 'rally': diff --git a/MAVProxy/modules/mavproxy_map3d/terrain.py b/MAVProxy/modules/mavproxy_map3d/terrain.py index 48c8341a14..5faea5813f 100644 --- a/MAVProxy/modules/mavproxy_map3d/terrain.py +++ b/MAVProxy/modules/mavproxy_map3d/terrain.py @@ -11,6 +11,7 @@ import os import queue import threading +import time import urllib.request import warnings @@ -72,13 +73,30 @@ def np_rgb_to_texture(img): return tex -def fetch_terrain_tile(z, x, y): +def fetch_terrain_tile(z, x, y, timeout=30): path = os.path.join(CACHE_DIR, str(z), str(x), "%d.terrain" % y) if not os.path.exists(path): os.makedirs(os.path.dirname(path), exist_ok=True) url = "%s/%d/%d/%d.terrain" % (QUANTIZED_BASE, z, x, y) req = urllib.request.Request(url, headers={"Accept": "application/octet-stream"}) - data = urllib.request.urlopen(req, timeout=30).read() + # a socket timeout is per read, so a tile dripping in a few bytes at + # a time would hold the caller for as long as it liked: give the + # whole fetch the deadline the caller asked for + deadline = time.time() + timeout + response = urllib.request.urlopen(req, timeout=timeout) + # read() waits to fill its buffer, however long the bytes take to + # come; read1() hands back what has arrived + read = getattr(response, 'read1', response.read) + chunks = [] + while True: + chunk = read(64 * 1024) + if not chunk: + break + chunks.append(chunk) + if time.time() > deadline: + raise TimeoutError("terrain tile took longer than %gs" + % timeout) + data = b"".join(chunks) # publish atomically under a unique name: the viewer child process and # the module share this cache, so a reader must never see a partial tile tmppath = "%s.tmp.%u.%u" % (path, os.getpid(), threading.get_ident()) @@ -90,11 +108,11 @@ def fetch_terrain_tile(z, x, y): return path, gz -def decode_terrain(z, x, y): +def decode_terrain(z, x, y, timeout=30): '''worker-safe: return dict(bbox, verts, idx) using numpy only''' g = GlobalGeodetic(True) bbox = g.TileBounds(x, y, z) - path, gz = fetch_terrain_tile(z, x, y) + path, gz = fetch_terrain_tile(z, x, y, timeout) tile = qmt_decode(path, bbox, gzipped=gz) verts = np.array(tile.getVerticesCoordinates()) nv = len(verts) @@ -106,14 +124,15 @@ def decode_terrain(z, x, y): _sample_cache = {} -def sample_terrain(lat, lon, zoom=12, cache_only=False): +def sample_terrain(lat, lon, zoom=12, cache_only=False, timeout=30): '''return terrain elevation (m AMSL) at lat/lon from the quantized mesh (same source we render), or None. Decoded tiles are cached. Assumes the regular grid mesh ArduPilot publishes; falls back to the tile mean for a non-grid tile. cache_only returns None rather than fetching/decoding a missing tile, so - callers on a latency-sensitive thread can defer the work.''' + callers on a latency-sensitive thread can defer the work; timeout is how + long a caller which does fetch waits for the whole tile.''' g = GlobalGeodetic(True) lon = wrap_longitude(lon) x, y = g.LonLatToTile(lon, lat, zoom) @@ -123,7 +142,7 @@ def sample_terrain(lat, lon, zoom=12, cache_only=False): if cache_only: return None try: - dec = decode_terrain(zoom, x, y) + dec = decode_terrain(zoom, x, y, timeout) except Exception: return None _sample_cache[key] = dec diff --git a/tests/conftest.py b/tests/conftest.py index f5a292d43a..48d768232a 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -18,3 +18,147 @@ def restore_mavlink20(): os.environ.pop('MAVLINK20', None) else: os.environ['MAVLINK20'] = before + + +class Widget(object): + '''as much of a wx control as the 3D viewer's frame uses''' + + def __init__(self, value): + self.value = value + + def SetValue(self, value): + self.value = value + + def GetValue(self): + return self.value + + def SetStringSelection(self, value): + self.value = value + + def GetStringSelection(self): + return self.value + + def Enable(self, enable=True): + pass + + +class Terrain(object): + '''as much of the 3D viewer's terrain manager as the frame uses, with + nothing fetched''' + mesh_revision = 0 + tiles = [] + + def __init__(self, *args, **kwargs): + self.render_settings = (kwargs.get('brightness'), + kwargs.get('shading'), + kwargs.get('wireframe')) + + def height_at(self, lat, lon): + return 0.0 + + def set_render_settings(self, brightness, shading, wireframe): + self.render_settings = (brightness, shading, wireframe) + + def update(self, camera): + pass + + +@pytest.fixture +def map3d_frame(monkeypatch): + """build the 3D viewer's frame without wx: its real message handling, + controls and scene building, with the widgets, the renderer's window and + the terrain stood in for. The camera dialog's tests build their wx + object the same way. + + elements is the ElementManager to draw into, or None for a frame which + has no scene yet. state is what the frame was started with -- a Map3D, + to connect it to one -- and otherwise events collects what the frame + tells its parent""" + def build(elements=None, events=None, state=None): + pytest.importorskip("wx") + pytest.importorskip("vtk") + import vtk + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d_ui + from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_STYLES) + # a scene is built with the real element manager and camera, over + # terrain and imagery which are never fetched + monkeypatch.setattr(map3d_ui, 'TerrainManager', Terrain) + monkeypatch.setattr(map3d_ui.mp_tile, 'MPTile', + lambda **kwargs: None) + if state is None: + state = SimpleNamespace(event_queue=SimpleNamespace( + put=(events if events is not None else []).append)) + for (name, value) in (('service', None), ('zexag', 1.0), + ('height', 800)): + if not hasattr(state, name): + setattr(state, name, value) + frame = map3d_ui.Map3DFrame.__new__(map3d_ui.Map3DFrame) + frame.state = state + frame.terrain = None if elements is None else Terrain() + frame.elements = elements + frame.ren = vtk.vtkRenderer() + frame.widget = SimpleNamespace(SetInteractorStyle=lambda style: None) + frame.status_actor = SimpleNamespace(SetInput=lambda text: None) + frame.GetClientSize = lambda: (1100, 800) + frame.render = lambda: None + frame.tc = None + frame.follow = False + frame.fpv_enabled = False + frame.fpv_fov = 90.0 + frame.terrain_brightness = 1.25 + frame.terrain_shading = True + frame.terrain_wireframe = False + frame.saved_map_view = None + frame.vehicle_type = None + frame.kml_features = [] + frame.kml_refresh_due = None + frame.overlay_mesh_revision = -1 + frame.mission_arrows = False + frame.mission_labelled = False + frame.mission_label_size = MISSION_LABEL_SIZE + frame.mission_style = MISSION_STYLES[0] + frame.labels_check = Widget(frame.mission_labelled) + frame.style_choice = Widget(frame.mission_style) + frame.follow_button = Widget(frame.follow) + return frame + return build + + +@pytest.fixture +def map3d_module(): + """build the live map3d module with the settings it makes, around the + map it is given: there is no other module for it to ask anything of, + and no mission to fly""" + def build(map=None): + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_map3d + module = mavproxy_map3d.Map3DModule.__new__( + mavproxy_map3d.Map3DModule) + module.map3d_settings = mavproxy_map3d.make_settings() + module.map = map + module.follow = True + module.send_mission = lambda: None + module.mpstate = SimpleNamespace(module=lambda name: None, + mav_param={}) + module.kml_change_state = None + module.terrain_resolved = False + module.ground_heading_changed = False + module.reset_flown_track() + return module + return build + + +@pytest.fixture +def mavexplorer(): + '''MAVExplorer, which MAVProxy/tools is not a package to import''' + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module diff --git a/tests/test_map3d_mission_labels.py b/tests/test_map3d_mission_labels.py new file mode 100644 index 0000000000..8c2e4339dc --- /dev/null +++ b/tests/test_map3d_mission_labels.py @@ -0,0 +1,220 @@ +'''the 3D map labelling each mission item, as the 2D map does''' + +import pytest + +from pymavlink import mavutil + +mavlink = mavutil.mavlink + +HOME = (-35.363262, 149.165238, 584.0) + + +def items(): + '''home, a waypoint, a loiter and a land start, as the map is given them''' + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + return [ + MissionItem(HOME[0], HOME[1], HOME[2], 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 0), + MissionItem(HOME[0] + 0.01, HOME[1], HOME[2] + 100, 0, + mavlink.MAV_CMD_NAV_LOITER_TURNS, 1), + MissionItem(HOME[0] + 0.02, HOME[1] + 0.01, HOME[2] + 100, 0, + mavlink.MAV_CMD_DO_LAND_START, 2), + ] + + +LABELS = ['0', '1(LT)', '2(DLS)'] + + +def elements(): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + return em + + +def labels(em): + '''the text and position of each number drawn''' + import vtk + return [(a.GetInput(), a.GetPosition()) + for a in em.actors.get('mission', []) + if isinstance(a, vtk.vtkBillboardTextActor3D)] + + +def label_sizes(em): + '''how big each of those is drawn''' + import vtk + return [a.GetTextProperty().GetFontSize() + for a in em.actors.get('mission', []) + if isinstance(a, vtk.vtkBillboardTextActor3D)] + + +def recording_map(pushed): + '''a map which notes the label settings it is sent''' + from types import SimpleNamespace + return SimpleNamespace( + is_alive=lambda: True, set_fpv_fov=lambda fov: None, + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: pushed.append(('labels', enable)), + set_mission_label_size=lambda size: pushed.append(('size', size)), + set_mission_style=lambda style: None, + set_render_settings=lambda *args: None) + + +class TestMissionLabels(object): + + def test_the_labels_are_drawn_only_when_they_are_asked_for(self): + em = elements() + em.set_mission(items()) + assert labels(em) == [] + em.set_mission_labels(True) + drawn = labels(em) + assert [text for (text, _) in drawn] == LABELS + # each beside the item it numbers + for ((_, at), marker) in zip(drawn, em.mission_markers): + assert at == pytest.approx(marker) + # and they go again when they are not wanted + em.set_mission_labels(False) + assert labels(em) == [] + + def test_the_labels_are_drawn_the_size_they_are_asked_for(self): + from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_LABEL_SIZES) + em = elements() + em.set_mission(items()) + em.set_mission_labels(True) + assert label_sizes(em) == [MISSION_LABEL_SIZE] * len(LABELS) + em.set_mission_label_size(24) + assert label_sizes(em) == [24] * len(LABELS) + # and only to a size there is some reading + (smallest, largest) = MISSION_LABEL_SIZES + em.set_mission_label_size(smallest - 1) + assert label_sizes(em) == [smallest] * len(LABELS) + em.set_mission_label_size(largest + 1) + assert label_sizes(em) == [largest] * len(LABELS) + # a size set while they are not drawn is the size they come back at + em.set_mission_labels(False) + em.set_mission_label_size(18) + em.set_mission_labels(True) + assert label_sizes(em) == [18] * len(LABELS) + + def test_a_mission_drawn_again_keeps_its_labels(self): + em = elements() + em.set_mission_labels(True) + em.set_mission(items()) + assert [text for (text, _) in labels(em)] == LABELS + # an item with no position of its own is not drawn, so it is not + # labelled either, and the numbers stay those of the items + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + mission = items() + mission[1] = MissionItem(0, 0, 0, 3, mavlink.MAV_CMD_DO_CHANGE_SPEED, 1) + em.set_mission(mission) + assert [text for (text, _) in labels(em)] == ['0', '2(DLS)'] + + def test_the_labels_are_the_ones_the_2d_map_uses(self): + """both maps call an item the same thing""" + from MAVProxy.modules.lib import mp_util + assert mp_util.mission_item_label( + 3, mavlink.MAV_CMD_NAV_WAYPOINT) == '3' + for (command, suffix) in ( + (mavlink.MAV_CMD_DO_LAND_START, 'DLS'), + (mavlink.MAV_CMD_NAV_TAKEOFF, 'TOff'), + (mavlink.MAV_CMD_NAV_LOITER_TIME, 'LTime'), + (mavlink.MAV_CMD_NAV_VTOL_LAND, 'VL'), + (mp_util.MAV_CMD_NAV_ARC_WAYPOINT, 'AW'), + (mp_util.MAV_CMD_DO_ORBIT, 'Orbit')): + assert mp_util.mission_item_label( + 4, command) == '4(%s)' % suffix + # and the 2D map's own labelling is the same labelling + pytest.importorskip("wx") + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_map + module = mavproxy_map.MapModule.__new__(mavproxy_map.MapModule) + wp = SimpleNamespace(command=mavlink.MAV_CMD_DO_LAND_START) + module.mpstate = SimpleNamespace( + module=lambda name: (SimpleNamespace( + wploader=SimpleNamespace(wp=lambda i: wp)) + if name == 'wp' else None)) + assert module.label_for_waypoint(2) == '2(DLS)' + + def test_the_viewer_is_told_to_label_the_mission(self, map3d_frame): + pytest.importorskip("vtk") + pytest.importorskip("wx") + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.map3d import Map3D + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + sent = [] + viewer = Map3D.__new__(Map3D) + viewer.child = SimpleNamespace(is_alive=lambda: True) + viewer.object_queue = SimpleNamespace(put=sent.append) + em = elements() + frame = map3d_frame(em) + viewer.set_mission(items()) + viewer.set_mission_labels(True) + viewer.set_mission_label_size(22) + for msg in sent: + Map3DFrame.handle(frame, msg) + assert [text for (text, _) in labels(em)] == LABELS + assert label_sizes(em) == [22] * len(LABELS) + # and the checkbox shows what the map is doing + assert frame.labels_check.GetValue() is True + + def test_the_live_map_settings(self, map3d_module): + from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_LABEL_SIZES) + pushed = [] + module = map3d_module(recording_map(pushed)) + # off unless it is asked for, since a mission of any size is busy + assert module.map3d_settings.showlabels is False + assert module.map3d_settings.labelsize == MISSION_LABEL_SIZE + module.cmd_map3d(['set', 'showlabels', 'true']) + assert ('labels', True) in pushed + assert module.map3d_settings.showlabels is True + module.cmd_map3d(['set', 'labelsize', '20']) + assert ('size', 20) in pushed + assert module.map3d_settings.labelsize == 20 + # and a size there is no reading is not taken + module.cmd_map3d(['set', 'labelsize', + str(MISSION_LABEL_SIZES[1] + 1)]) + assert module.map3d_settings.labelsize == 20 + + def test_the_checkbox_on_the_map_labels_the_mission(self, map3d_frame, + map3d_module): + em = elements() + em.set_mission(items()) + events = [] + frame = map3d_frame(em, events) + frame.labels_check.SetValue(True) + frame.on_labels_toggle(None) + assert [text for (text, _) in labels(em)] == LABELS + # and the setting follows the box, so it is what a view opened next + # is drawn with + assert events == [('mission_labels', True)] + pushed = [] + module = map3d_module(recording_map(pushed)) + module.map.check_events = lambda: events + module.idle_task() + assert module.map3d_settings.showlabels is True + # and the view is told what the setting now is + assert pushed == [('labels', True)] + + def test_a_map_with_no_scene_yet_keeps_what_it_is_told(self, map3d_frame): + '''the live map is started before the vehicle has said where it is, + so what it is told to draw waits for a scene to draw it in''' + # the fixture skips without wx, which the frame's module needs + frame = map3d_frame() + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + Map3DFrame.handle(frame, ('mission_labels', True)) + Map3DFrame.handle(frame, ('mission_label_size', 20)) + Map3DFrame.handle(frame, ('mission_style', 'plain')) + Map3DFrame.handle(frame, ('mission_arrows', True)) + assert frame.labels_check.GetValue() is True + assert frame.style_choice.GetStringSelection() == 'plain' + # which is what the scene is built with, once there is one + Map3DFrame.handle(frame, ('origin', HOME[0], HOME[1], HOME[2])) + Map3DFrame.handle(frame, ('mission', items(), None)) + em = frame.elements + assert label_sizes(em) == [20] * len(LABELS) + assert em.mission_style == 'plain' + assert em.mission_arrows is True diff --git a/tests/test_map3d_settings_sync.py b/tests/test_map3d_settings_sync.py new file mode 100644 index 0000000000..6bc45513e7 --- /dev/null +++ b/tests/test_map3d_settings_sync.py @@ -0,0 +1,238 @@ +'''the 3D map's own controls and the settings they stand for. The view runs +in a process of its own, so what it is told and what it tells come and go +on two queues; however those cross, the view and whatever started it -- +the map3d module, or MAVExplorer -- end up agreeing''' + +import os +import queue +import threading +from types import SimpleNamespace + +import pytest + +HOME = (-35.363262, 149.165238, 584.0) + + +def connect(map3d_frame): + '''a Map3D with plain queues, and the frame at the other end of them''' + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.map3d import Map3D + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + viewer = Map3D.__new__(Map3D) + viewer.child = SimpleNamespace(is_alive=lambda: True) + viewer.object_queue = queue.Queue() + viewer.event_queue = queue.Queue() + viewer._origin_set = True + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + return (viewer, map3d_frame(em, state=viewer)) + + +def deliver(viewer, frame): + '''the view takes everything it has been sent''' + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + while True: + try: + msg = viewer.object_queue.get_nowait() + except queue.Empty: + return + Map3DFrame.handle(frame, msg) + + +def labels_on_the_view(frame): + frame.labels_check.SetValue(True) + frame.on_labels_toggle(None) + + +def style_on_the_view(frame): + frame.style_choice.SetStringSelection('plain') + frame.on_style_choice(None) + + +def follow_on_the_view(frame): + frame.follow_button.SetValue(False) + frame.on_follow_toggle(None) + + +def render_on_the_view(frame): + # OK in the settings dialog + frame.apply_render_settings((0.5, False, True, 60.0), notify=True) + + +def rendered(frame): + return (frame.terrain_brightness, frame.terrain_shading, + frame.terrain_wireframe, frame.fpv_fov) + + +# for each control on the view: what the user does there, the command which +# says otherwise, how the module and the view each hold the result, and what +# the command sets +CONTROLS = { + 'labels': (labels_on_the_view, ['set', 'showlabels', 'false'], + lambda module: module.map3d_settings.showlabels, + lambda frame: [frame.mission_labelled, + frame.labels_check.GetValue(), + frame.elements.mission_labelled], + lambda held: held is False), + 'style': (style_on_the_view, ['set', 'missionpath', 'geometry'], + lambda module: module.map3d_settings.missionpath, + lambda frame: [frame.mission_style, + frame.style_choice.GetStringSelection(), + frame.elements.mission_style], + lambda held: held == 'geometry'), + 'follow': (follow_on_the_view, ['follow'], + lambda module: module.follow, + lambda frame: [frame.follow, frame.follow_button.GetValue()], + lambda held: held is True), + 'render': (render_on_the_view, ['set', 'terrainbrightness', '1.75'], + lambda module: (module.map3d_settings.terrainbrightness, + module.map3d_settings.terrainshading, + module.map3d_settings.terrainwireframe, + module.map3d_settings.fpvfov), + lambda frame: [rendered(frame), + frame.terrain.render_settings + (frame.fpv_fov,)], + # the rest are the dialog's, which the command leaves be + lambda held: held == (1.75, False, True, 60.0)), +} + + +class TestSettingsSync(object): + + @pytest.mark.parametrize('control', sorted(CONTROLS)) + def test_a_command_given_while_the_view_is_changed(self, control, + map3d_frame, + map3d_module): + '''the user changes a control on the view, and before the module + has heard of it, a command says otherwise''' + (on_the_view, command, in_the_module, in_the_view, _) = CONTROLS[control] + (viewer, frame) = connect(map3d_frame) + module = map3d_module(viewer) + on_the_view(frame) + module.cmd_map3d(command) + module.idle_task() + deliver(viewer, frame) + # nothing more is on its way either side + module.idle_task() + assert viewer.event_queue.empty() + deliver(viewer, frame) + for held in in_the_view(frame): + assert held == in_the_module(module) + + @pytest.mark.parametrize('control', sorted(CONTROLS)) + def test_a_command_given_after_the_view_is_changed(self, control, + map3d_frame, + map3d_module): + '''the same, the other way round: the command comes second, and + is what both end up with''' + (on_the_view, command, in_the_module, in_the_view, + commanded) = CONTROLS[control] + (viewer, frame) = connect(map3d_frame) + module = map3d_module(viewer) + on_the_view(frame) + module.idle_task() + deliver(viewer, frame) + module.cmd_map3d(command) + deliver(viewer, frame) + # it is the command which stands, not the control + assert commanded(in_the_module(module)) + for held in in_the_view(frame): + assert held == in_the_module(module) + + def test_a_render_setting_reaches_an_open_view(self, map3d_frame, + map3d_module): + (viewer, frame) = connect(map3d_frame) + module = map3d_module(viewer) + module.cmd_map3d(['set', 'terrainbrightness', '0.75']) + module.cmd_map3d(['set', 'terrainwireframe', 'true']) + module.cmd_map3d(['set', 'fpvfov', '45']) + deliver(viewer, frame) + assert rendered(frame) == (0.75, True, True, 45.0) + assert frame.terrain.render_settings == (0.75, True, True) + + def test_render_settings_wait_for_a_scene(self, map3d_frame, + map3d_module): + (viewer, frame) = connect(map3d_frame) + frame.elements = None + frame.terrain = None + module = map3d_module(viewer) + module.cmd_map3d(['set', 'terrainshading', 'false']) + deliver(viewer, frame) + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + Map3DFrame.handle(frame, ('origin', HOME[0], HOME[1], HOME[2])) + assert frame.terrain.render_settings == (1.25, False, False) + + +def display(): + return bool(os.environ.get('DISPLAY') or os.environ.get('WAYLAND_DISPLAY')) + + +@pytest.mark.skipif(not display(), reason='needs a display to build a window') +class TestToolbar(object): + '''the frame as it is really built, which the other tests stand in for''' + + def frame(self): + wx = pytest.importorskip("wx") + pytest.importorskip("vtk") + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + self.app = wx.GetApp() or wx.App(False) + closed = threading.Semaphore() + closed.acquire() + state = SimpleNamespace( + title='test', width=600, height=400, follow=True, fpvfov=90.0, + terrain_brightness=1.25, terrain_shading=True, + terrain_wireframe=False, service=None, zexag=1.0, + object_queue=queue.Queue(), event_queue=queue.Queue(), + close_window=closed) + frame = Map3DFrame(state) + frame.timer.Stop() + return frame + + def click(self, control, kind): + import wx + event = wx.CommandEvent(kind, control.GetId()) + event.SetEventObject(control) + control.GetEventHandler().ProcessEvent(event) + + def events(self, frame): + out = [] + while not frame.state.event_queue.empty(): + out.append(frame.state.event_queue.get_nowait()) + return out + + def test_the_controls_start_as_the_view_draws(self): + from MAVProxy.modules.mavproxy_map3d.map3d import MISSION_STYLES + frame = self.frame() + try: + assert frame.labels_check.GetValue() is False + assert frame.style_choice.GetStringSelection() == MISSION_STYLES[0] + assert (list(frame.style_choice.GetStrings()) == + list(MISSION_STYLES)) + assert frame.follow_button.GetValue() is True + finally: + frame.Destroy() + + def test_the_controls_tell_the_parent(self): + import wx + frame = self.frame() + try: + frame.labels_check.SetValue(True) + self.click(frame.labels_check, wx.wxEVT_CHECKBOX) + frame.style_choice.SetStringSelection('plain') + self.click(frame.style_choice, wx.wxEVT_CHOICE) + assert self.events(frame) == [('mission_labels', True), + ('mission_style', 'plain')] + finally: + frame.Destroy() + + def test_the_controls_show_what_the_view_is_told(self): + frame = self.frame() + try: + frame.handle(('mission_labels', True)) + frame.handle(('mission_style', 'geometry')) + assert frame.labels_check.GetValue() is True + assert frame.style_choice.GetStringSelection() == 'geometry' + # and being told is not the user changing them + assert self.events(frame) == [] + finally: + frame.Destroy() diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 1b54cef7e1..39d5468977 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -48,7 +48,12 @@ def live_mission_items(wpoints, home_amsl=584.0, params=None, module=lambda name: (SimpleNamespace(wploader=loader) if name == 'wp' else None)) sent = [] - module.map = SimpleNamespace(set_mission=sent.extend) + module.map = SimpleNamespace( + set_mission=lambda items, track=None: sent.extend(items)) + module.reset_flown_track() + module.ground_heading = None + # only the items are looked at, so there is no path to fly for them + module.map3d_settings = SimpleNamespace(missionpath='geometry') module.home_amsl = home_amsl module.home_position = None module.default_circle_radius = lambda: default_radius @@ -141,16 +146,98 @@ def test_terrain_is_sampled_either_side_of_the_antimeridian(self, monkeypatch): from MAVProxy.modules.mavproxy_map3d import terrain fetched = [] - def decode(z, x, y): - fetched.append((z, x, y)) + def decode(z, x, y, timeout=30): + fetched.append((z, x, y, timeout)) return {"bbox": (-180.0, -20.0, -170.0, -10.0), "verts": np.array([(-180.0, -10.0, 1.0), (-170.0, -10.0, 1.0), (-180.0, -20.0, 1.0), (-170.0, -20.0, 1.0)])} monkeypatch.setattr(terrain, 'decode_terrain', decode) monkeypatch.setattr(terrain, '_sample_cache', {}) assert terrain.sample_terrain(-16.5, 180.001) == pytest.approx(1.0) - assert fetched == [(12,) + terrain.GlobalGeodetic(True).LonLatToTile( - -179.999, -16.5, 12)] + tile = terrain.GlobalGeodetic(True).LonLatToTile(-179.999, -16.5, 12) + assert fetched == [(12,) + tile + (30,)] + # and a caller which will not wait that long says so + monkeypatch.setattr(terrain, '_sample_cache', {}) + assert terrain.sample_terrain(-16.5, 180.001, + timeout=5.0) == pytest.approx(1.0) + assert fetched[-1] == (12,) + tile + (5.0,) + + def test_a_terrain_tile_is_fetched_with_the_timeout_asked_for( + self, monkeypatch, tmp_path): + pytest.importorskip("vtk") + import time + import urllib.request + from MAVProxy.modules.mavproxy_map3d import terrain + waited = [] + + class Response(object): + def __init__(self): + self.left = [b'terrain ', b'tile'] + + def read(self, size=None): + return self.left.pop(0) if self.left else b'' + + def urlopen(request, timeout=None): + waited.append(timeout) + return Response() + monkeypatch.setattr(urllib.request, 'urlopen', urlopen) + monkeypatch.setattr(terrain, 'CACHE_DIR', str(tmp_path)) + terrain.fetch_terrain_tile(12, 1, 2, timeout=5.0) + assert waited == [5.0] + # and the whole fetch, where nobody says how long to wait for it + monkeypatch.setattr(terrain, 'CACHE_DIR', str(tmp_path / 'again')) + terrain.fetch_terrain_tile(12, 1, 2) + assert waited[-1] == 30 + # decoding a tile waits as long as it is told to as well + + class Stop(Exception): + pass + + def fetch(z, x, y, timeout=30): + waited.append(timeout) + raise Stop() + # a tile which drips in for ever is given up on at the deadline + monkeypatch.setattr(terrain, 'CACHE_DIR', str(tmp_path / 'slow')) + + class Dripping(object): + def __init__(self): + self.left = 100 + + def read(self, size=None): + if self.left <= 0: + return b'' + self.left -= 1 + time.sleep(0.01) + return b'.' + monkeypatch.setattr(urllib.request, 'urlopen', + lambda request, timeout=None: Dripping()) + with pytest.raises(Exception): + terrain.fetch_terrain_tile(12, 1, 2, timeout=0.05) + + # as a real response does, whose read() waits to fill its buffer + # however long the bytes take, and whose read1() does not + class Buffering(Dripping): + def read(self, size=None): + time.sleep(0.01 * self.left) + (data, self.left) = (b'.' * self.left, 0) + return data + + def read1(self, size=None): + return Dripping.read(self, size) + monkeypatch.setattr(urllib.request, 'urlopen', + lambda request, timeout=None: Buffering()) + started = time.time() + with pytest.raises(TimeoutError, match='longer than 0.05s'): + terrain.fetch_terrain_tile(12, 1, 2, timeout=0.05) + assert time.time() - started < 0.5 + monkeypatch.setattr(terrain, 'fetch_terrain_tile', fetch) + for asked in (5.0, None): + with pytest.raises(Stop): + if asked is None: + terrain.decode_terrain(12, 1, 2) + else: + terrain.decode_terrain(12, 1, 2, asked) + assert waited[-1] == (30 if asked is None else asked) def test_the_view_is_turned_across_the_antimeridian(self): pytest.importorskip("vtk") diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index db4355e39d..3d9ad1315a 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -1202,3 +1202,1089 @@ def test_without_parameters_arduplanes_defaults_fly(self): # the first 30s, which takes in a QuadPlane's transition and a plane's # takeoff; and the 90th percentile and worst of the height of each point # flown above or below the nearest point of the path drawn + + +class TestDrawnTrack(object): + """where the maps get the path from, and what they do with it""" + + def items(self): + '''a mission in the form the wp module's loader holds it: home, a + speed change with no position, and two waypoints''' + from types import SimpleNamespace + wpoints = [] + rows = [(mavlink.MAV_CMD_NAV_WAYPOINT, offset(0, 0, 0)), + (mavlink.MAV_CMD_DO_CHANGE_SPEED, (0, 0, 0)), + (mavlink.MAV_CMD_NAV_WAYPOINT, offset(3000, 0)), + (mavlink.MAV_CMD_NAV_WAYPOINT, offset(3000, 3000))] + for (seq, (command, (lat, lon, amsl))) in enumerate(rows): + params = (0, 30, -1, 0) if command == mavlink.MAV_CMD_DO_CHANGE_SPEED else (0, 0, 0, 0) + wpoints.append(SimpleNamespace( + seq=seq, command=command, x=lat, y=lon, + z=0 if seq == 0 else amsl - HOME[2], frame=0 if seq == 0 else 3, + param1=params[0], param2=params[1], param3=params[2], + param4=params[3])) + wpoints[0].z = HOME[2] + return wpoints + + def live_module(self, vehicle, params=PARAMS, settle=True): + '''the live map3d module, with only what send_mission() reads''' + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import Map3DModule + module = Map3DModule.__new__(Map3DModule) + wpoints = self.items() + loader = SimpleNamespace(wpoints=wpoints, wp=lambda i: wpoints[i]) + module.mpstate = SimpleNamespace( + mav_param=dict(params), vehicle_type=vehicle, + module=lambda name: (SimpleNamespace(wploader=loader) + if name == 'wp' else None)) + module.sent = [] + module.map = SimpleNamespace( + set_mission=lambda items, track=None: module.sent.append(track)) + module.home_amsl = HOME[2] + module.home_position = None + module.reset_flown_track() + module.map3d_settings = SimpleNamespace(missionpath='flown') + module.armed = False + module.ground_heading = None + module.ground_heading_changed = False + module.ground_heading_redrawn = 0 + module.default_circle_radius = lambda: 80.0 + module.terrain_alt = lambda lat, lon: None + if settle: + # the path is flown on a thread of its own, and drawn from the + # idle task: wait for it and draw it, wherever the module sends + # the mission, so these tests can look at what is drawn + send_mission = module.send_mission + + def send_and_draw(): + send_mission() + self.settle(module) + module.send_mission = send_and_draw + return module + + @staticmethod + def settle(module, timeout=10.0): + '''wait for the module's thread to fly the mission, and draw it''' + import time + deadline = time.time() + timeout + while module.track_thread is not None: + assert time.time() < deadline, 'the mission was never flown' + time.sleep(0.001) + module.draw_flown_track() + + def test_the_live_map_waits_for_the_whole_of_a_mission(self): + """the items of a download still coming in are drawn, but flying + them would end the mission where it has got to, with a return to + launch or a landing the rest of it may say nothing of""" + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + loader.count = lambda: len(loader.wpoints) + loader.expected_count = len(loader.wpoints) + 1 + module.send_mission() + assert module.sent[-1] is None + # and once the last of it arrives, it is flown + loader.expected_count = len(loader.wpoints) + module.send_mission() + assert module.sent[-1] is not None + + def test_the_live_map_flies_a_planes_mission(self): + module = self.live_module('plane') + module.send_mission() + track = module.sent[-1] + assert track is not None + assert track[0][:2] == pytest.approx(HOME[:2]) + # the speed change with no position of its own was flown too: at 30 + # m/s rather than 22 the turn at the first waypoint swings wider + slow = self.live_module('plane', dict(PARAMS, AIRSPEED_CRUISE=22.0)) + slow.mpstate.module('wp').wploader.wpoints[1].param2 = -1 + slow.send_mission() + assert (max(local(p)[0] for p in track) > + max(local(p)[0] for p in slow.sent[-1]) + 5.0) + + def test_the_live_map_does_not_fly_the_mission_again_as_home_wanders( + self, monkeypatch): + # ArduPlane keeps setting home from the GPS every few seconds before + # it arms, so home wanders by a metre or two each time + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + module = self.live_module('plane') + # a takeoff with no position of its own is flown from home, so it + # wanders with it + loader = module.mpstate.module('wp').wploader + loader.wpoints[1].command = mavlink.MAV_CMD_NAV_TAKEOFF + loader.wpoints[1].z = 30.0 + module.home_position = HOME[:2] + module.send_mission() + assert len(calls) == 1 + flown = module.sent[-1] + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, 3) + module.home_position = (lat, lon) + module.home_amsl = HOME[2] + 2 + module.send_mission() + assert len(calls) == 1 + # the path it was flown with is moved to start at the new home, and + # up with it, but still meets the waypoints, which have not moved + moved = module.sent[-1] + assert moved[0][0] == pytest.approx(lat, abs=1e-7) + assert moved[0][1] == pytest.approx(lon, abs=1e-7) + assert moved[0][2] == pytest.approx(flown[0][2] + 2) + far = [i for (i, p) in enumerate(flown) if local(p)[1] > 1000][0] + assert moved[far][:2] == pytest.approx(flown[far][:2], abs=1e-7) + assert moved[far][2] == pytest.approx(flown[far][2] + 2) + # but it moving further is a different flight + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, 50) + module.home_position = (lat, lon) + module.send_mission() + assert len(calls) == 2 + module.home_amsl = HOME[2] + 30 + module.send_mission() + assert len(calls) == 3 + + def test_an_amsl_mission_is_not_flown_again_as_homes_altitude_wanders( + self, monkeypatch): + # an item at an altitude of its own does not move when home does, + # so the mission is the same mission as home's altitude wanders + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + for w in loader.wpoints[1:]: + if w.frame == 3: + w.z += HOME[2] + w.frame = 0 + module.home_position = HOME[:2] + module.send_mission() + assert len(calls) == 1 + for altitude in (0.3, -0.4, 0.2): + module.home_amsl += altitude + module.send_mission() + assert len(calls) == 1 + # and the path it was drawn with stays where it was flown + flown = [track for track in module.sent if track is not None][0] + assert module.sent[-1][0][2] == pytest.approx(flown[0][2]) + # while a mission above home moves up with it + relative = self.live_module('plane') + relative.home_position = HOME[:2] + relative.send_mission() + moved_from = relative.sent[-1][0][2] + relative.home_amsl += 0.3 + relative.send_mission() + assert relative.sent[-1][0][2] == pytest.approx(moved_from + 0.3) + assert len(calls) == 2 + + def counted_flights(self, monkeypatch): + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + return calls + + def test_a_mission_at_both_kinds_of_altitude(self, monkeypatch): + # a path with some altitudes moving with home and some not cannot be + # moved to fit a home which has: it is moved all the same while home + # stays near, and flown again once home has gone further + calls = self.counted_flights(monkeypatch) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + # the last waypoint at an altitude of its own, the rest above home + loader.wpoints[-1].z += HOME[2] + loader.wpoints[-1].frame = 0 + module.home_position = HOME[:2] + module.send_mission() + assert len(calls) == 1 + last = loader.wpoints[-1] + flown = arrival(module.sent[-1], last.x, last.y) + # ArduPlane sends home every few seconds before it arms, a few + # centimetres different each time + for altitude in (0.03, -0.02, 0.04): + module.home_amsl += altitude + module.send_mission() + assert len(calls) == 1 + assert arrival(module.sent[-1], last.x, last.y)[2] == pytest.approx( + flown[2], abs=0.1) + # further, and it is flown again, with the item with an altitude of + # its own still drawn at it, rather than moved up with home + module.home_amsl += 30.0 + module.send_mission() + assert len(calls) == 2 + assert arrival(module.sent[-1], last.x, last.y)[2] == pytest.approx( + flown[2], abs=5.0) + + def test_an_item_with_no_altitude_to_fly_at_does_not_move_with_home( + self, monkeypatch): + # a speed change is at an altitude only because every item carries + # one, and one with no position is flown wherever the aircraft is; + # whatever the frame, neither makes the mission move with home + speed = mavlink.MAV_CMD_DO_CHANGE_SPEED + for (command, frame, z) in ((speed, 3, 0), (speed, 3, 50), + (speed, 10, 0), (speed, 0, 0), + (mavlink.MAV_CMD_NAV_LOITER_UNLIM, 3, 0), + (mavlink.MAV_CMD_NAV_LOITER_UNLIM, 10, 50)): + calls = self.counted_flights(monkeypatch) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + for w in loader.wpoints[1:]: + if w.command == speed: + (w.command, w.frame, w.z) = (command, frame, z) + w.param1 = w.param2 = w.param3 = w.param4 = 0 + else: + (w.frame, w.z) = (0, w.z + HOME[2]) + module.home_position = HOME[:2] + for i in range(4): + module.home_amsl = HOME[2] + 5.0 * (i % 2) + module.send_mission() + assert len(calls) == 1, (command, frame, z) + # nor is the path drawn moved up and down with home + flown = [track for track in module.sent if track is not None][0] + assert module.home_amsl == HOME[2] + 5.0 + assert ([p[2] for p in module.sent[-1]] == + pytest.approx([p[2] for p in flown])), (command, frame, z) + monkeypatch.undo() + + def test_an_item_flown_above_home_moves_with_it(self, monkeypatch): + # a loiter with no position of its own, above home, is flown at an + # altitude which moves with home, among waypoints which do not + calls = self.counted_flights(monkeypatch) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + for w in loader.wpoints[1:]: + if w.command == mavlink.MAV_CMD_DO_CHANGE_SPEED: + (w.command, w.frame, w.z) = ( + mavlink.MAV_CMD_NAV_LOITER_TURNS, 3, 50) + (w.param1, w.param2, w.param3, w.param4) = (1, 0, 0, 0) + else: + (w.frame, w.z) = (0, w.z + HOME[2]) + module.home_position = HOME[:2] + module.send_mission() + module.home_amsl += 30.0 + module.send_mission() + assert len(calls) == 2 + + def test_the_live_map_takes_home_for_an_origin_it_has_not_been_told(self): + from types import SimpleNamespace + module = self.live_module('plane') + frame_valid = 1 << 2 + rally = [SimpleNamespace(lat=int(HOME[0] * 1e7), + lng=int(HOME[1] * 1e7), alt=100, + flags=frame_valid | (2 << 3))] + module.mpstate.module = lambda name: SimpleNamespace( + rallyloader=SimpleNamespace(rally_count=lambda: 1, + rally_point=lambda i: rally[0])) + # ArduPilot logs the EKF origin and home together, and MAVProxy + # keeps only the last of each type, so the origin is often unknown: + # home stands in for it, and moves the point as home moves + assert module.origin_amsl is None + (point,) = module.rally_points((HOME[0], HOME[1], HOME[2])) + assert point[2] == HOME[2] + 100 + assert point[3] is False + + def test_the_live_map_takes_a_rally_points_own_altitude_frame(self): + from types import SimpleNamespace + module = self.live_module('plane') + frame_valid = 1 << 2 + rally = [SimpleNamespace(lat=int(HOME[0] * 1e7), + lng=int(HOME[1] * 1e7), alt=100, + flags=frame_valid | (frame << 3)) + for frame in (1, 2, 3)] + module.mpstate.module = lambda name: SimpleNamespace( + wploader=module.mpstate.module('wp').wploader + if name == 'wp' else None, + rallyloader=SimpleNamespace( + rally_count=lambda: len(rally), + rally_point=lambda i: rally[i])) + module.origin_amsl = 500.0 + module.terrain_alt = lambda lat, lon: 300.0 + points = module.rally_points((HOME[0], HOME[1], HOME[2])) + assert [p[2] for p in points] == [HOME[2] + 100, 600.0, 400.0] + # only the one above home moves when home does + assert [p[3] for p in points] == [False, True, True] + + def test_the_live_map_draws_rally_points_where_they_are_returned_to( + self): + pytest.importorskip("vtk") + import vtk + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + module = self.live_module('plane') + frame_valid = 1 << 2 + # absolute, above home, above the EKF origin, above terrain + rally = [SimpleNamespace(lat=int(HOME[0] * 1e7), + lng=int(HOME[1] * 1e7), alt=100, + flags=frame_valid | (frame << 3)) + for frame in (0, 1, 2, 3)] + module.mpstate.module = lambda name: SimpleNamespace( + rallyloader=SimpleNamespace(rally_count=lambda: len(rally), + rally_point=lambda i: rally[i])) + drawn = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + drawn.set_home(HOME[2]) + module.map = SimpleNamespace( + is_alive=lambda: True, set_rally=drawn.set_rally, + set_home=drawn.set_home) + module.send_mission = lambda: None + module.send_fence = lambda: None + module.origin_amsl = 500.0 + module.terrain_alt = lambda lat, lon: 300.0 + + def heights(): + source = drawn.actors['rally'][0].GetMapper().GetInput() + return [source.GetPoint(i)[2] + for i in range(source.GetNumberOfPoints())] + module.send_rally() + assert heights() == pytest.approx([100.0, HOME[2] + 100, 600.0, 400.0]) + + def message(kind, **fields): + m = SimpleNamespace(**fields) + m.get_type = lambda: kind + return m + # each drawn again as what it is above moves + module.mavlink_packet(message('HOME_POSITION', altitude=600 * 1000, + latitude=int(HOME[0] * 1e7), + longitude=int(HOME[1] * 1e7))) + assert heights() == pytest.approx([100.0, 700.0, 600.0, 400.0]) + module.mavlink_packet(message('GPS_GLOBAL_ORIGIN', altitude=450 * 1000)) + assert heights() == pytest.approx([100.0, 700.0, 550.0, 400.0]) + # and where the terrain is not known yet, above home for now + module.terrain_alt = lambda lat, lon: None + module.send_rally() + assert heights() == pytest.approx([100.0, 700.0, 550.0, 700.0]) + + def test_an_item_with_no_position_keeps_its_altitude(self, monkeypatch): + flown = [] + real = plane_track.mission_track + + def recording(home, items, *args, **kwargs): + flown.append(items) + return real(home, items, *args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + # a loiter to altitude where the aircraft is, 200m above home + loader.wpoints[1].command = mavlink.MAV_CMD_NAV_LOITER_TO_ALT + loader.wpoints[1].z = 200.0 + loader.wpoints[1].param2 = 0 + module.send_mission() + assert flown[-1][0][3] == pytest.approx(HOME[2] + 200.0) + + def test_the_mission_is_flown_off_the_main_thread(self, monkeypatch): + import threading + release = threading.Event() + flown_on = [] + real = plane_track.mission_track + + def slow(*args, **kwargs): + flown_on.append(threading.current_thread()) + assert release.wait(10) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', slow) + module = self.live_module('plane', settle=False) + module.send_mission() + # the mission is drawn at once, while its path is still being flown + assert module.sent == [None] + module.draw_flown_track() + assert module.sent == [None] + # sent again as it is flown, it is not flown twice over + import time + deadline = time.time() + 10 + while not flown_on: + assert time.time() < deadline + time.sleep(0.001) + module.send_mission() + release.set() + self.settle(module) + assert module.sent[-1] is not None + assert flown_on[0] is not threading.main_thread() + assert len(flown_on) == 1 + # and drawn with it straight away the next time, without flying it + module.send_mission() + assert module.sent[-1] is not None + assert len(flown_on) == 1 + + def flying_slowly(self, monkeypatch): + '''plane_track.mission_track made to wait for a release, and a list + of the calls made to it''' + import threading + release = threading.Event() + calls = [] + real = plane_track.mission_track + + def slow(*args, **kwargs): + calls.append(args) + assert release.wait(10) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', slow) + return (release, calls) + + def wait_for(self, condition): + import time + deadline = time.time() + 10 + while not condition(): + assert time.time() < deadline + time.sleep(0.001) + + def test_home_wandering_as_the_mission_is_flown(self, monkeypatch): + (release, calls) = self.flying_slowly(monkeypatch) + module = self.live_module('plane', settle=False) + module.home_position = HOME[:2] + module.send_mission() + self.wait_for(lambda: calls) + # home wanders a few metres while the path is being flown: the path + # is not flown again, and is drawn from where home is now + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, 3) + module.home_position = (lat, lon) + module.send_mission() + release.set() + self.settle(module) + assert len(calls) == 1 + assert module.sent[-1][0][:2] == pytest.approx((lat, lon), abs=1e-7) + + def test_home_wandering_far_as_the_mission_is_flown(self, monkeypatch): + (release, calls) = self.flying_slowly(monkeypatch) + module = self.live_module('plane', settle=False) + module.home_position = HOME[:2] + module.send_mission() + self.wait_for(lambda: calls) + # a few metres at a time, but in all more than ten from the home it + # is being flown from: that is flown again + for metres in (4, 8, 13): + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, metres) + module.home_position = (lat, lon) + module.send_mission() + release.set() + self.settle(module) + assert len(calls) == 2 + assert module.sent[-1][0][:2] == pytest.approx((lat, lon), abs=1e-7) + + def test_the_idle_task_draws_the_path_flown(self): + import time + module = self.live_module('plane', settle=False) + module.map.is_alive = lambda: True + module.map.check_events = lambda: [] + wp_module = module.mpstate.module('wp') + wp_module.wploader.last_change = 1.0 + module.mpstate.module = lambda name: ( + wp_module if name == 'wp' else None) + module.wp_change_time = 1.0 + module.kml_change_state = module._kml_state(None) + module.fence_change_time = module.rally_change_time = 0 + module.terrain_resolved = False + module.send_mission() + assert module.sent == [None] + deadline = time.time() + 10 + while module.track_thread is not None: + assert time.time() < deadline + time.sleep(0.001) + module.idle_task() + assert module.sent[-1] is not None + + def test_the_live_map_redraws_for_a_home_which_moves_up(self): + """home moving up or down takes the mission with it, as much as home + moving along does: everything above home is as high as home is, and + the flight starts there""" + from types import SimpleNamespace + module = self.live_module('plane') + module.map.is_alive = lambda: True + module.map.set_home = lambda amsl: None + module.send_rally = lambda: None + module.send_fence = lambda: None + + def home_position(amsl): + m = SimpleNamespace(altitude=int(amsl * 1000), + latitude=int(HOME[0] * 1e7), + longitude=int(HOME[1] * 1e7)) + m.get_type = lambda: 'HOME_POSITION' + module.mavlink_packet(m) + home_position(HOME[2]) + drawn = len(module.sent) + assert module.sent[-1][0][2] == pytest.approx(HOME[2], abs=1.0) + # a hundred metres higher, and the path is drawn from there + home_position(HOME[2] + 100) + assert len(module.sent) > drawn + assert module.sent[-1][0][2] == pytest.approx(HOME[2] + 100, abs=1.0) + # and no further drawing for a home which has not moved at all + drawn = len(module.sent) + home_position(HOME[2] + 100) + assert len(module.sent) == drawn + + def test_the_live_map_redraws_for_a_changed_parameter(self): + """how the vehicle flies a mission is its parameters' as much as its + items': a parameter the path is flown by changing, or arriving for + the first time, draws the mission again""" + module = self.live_module('plane') + module.map.is_alive = lambda: True + module.map.check_events = lambda: [] + wp_module = module.mpstate.module('wp') + wp_module.wploader.last_change = 1.0 + module.mpstate.module = lambda name: ( + wp_module if name == 'wp' else None) + module.wp_change_time = 1.0 + module.kml_change_state = module._kml_state(None) + module.fence_change_time = module.rally_change_time = 0 + module.terrain_resolved = False + module.send_mission() + # the parameters arriving for the first time is a change of its own: + # the mission is drawn again with them + module.idle_task() + drawn = len(module.sent) + was = max(p[2] for p in module.sent[-1]) + # with nothing changed since, it is not + module.idle_task() + assert len(module.sent) == drawn + # RTL_ALTITUDE, which a return climbs to, changing is: the return + # comes home higher than it did + module.mpstate.mav_param['RTL_ALTITUDE'] = 300.0 + module.idle_task() + assert len(module.sent) > drawn + assert max(p[2] for p in module.sent[-1]) > was + 100.0 + + def test_a_path_for_a_mission_since_changed_is_not_drawn(self, monkeypatch): + import threading + import time + (first, second) = (threading.Event(), threading.Event()) + releases = [first, second] + calls = [] + real = plane_track.mission_track + + def slow(*args, **kwargs): + calls.append(args) + assert releases.pop(0).wait(10) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', slow) + module = self.live_module('plane', settle=False) + loader = module.mpstate.module('wp').wploader + module.send_mission() + # the mission changes while the first is being flown, rather than + # before the thread has taken it, which it would simply replace + self.wait_for(lambda: calls) + loader.wpoints[3].y += 0.01 + module.send_mission() + assert module.sent == [None, None] + first.set() + deadline = time.time() + 10 + while module.track_result is None: + assert time.time() < deadline + time.sleep(0.001) + # the first mission's path is not drawn over the second mission + module.draw_flown_track() + assert module.sent == [None, None] + second.set() + self.settle(module) + (lat, lon, _) = nearest(module.sent[-1], loader.wpoints[3].x, + loader.wpoints[3].y) + assert mp_util.gps_distance(lat, lon, loader.wpoints[3].x, + loader.wpoints[3].y) < 150.0 + + def test_the_live_map_returns_to_its_rally_points(self, monkeypatch): + from types import SimpleNamespace + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + loader.wpoints[3].command = mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH + loader.wpoints[3].x = loader.wpoints[3].y = 0.0 + (lat, lon, _) = offset(2500, 800, 0) + rally = SimpleNamespace(rally_points=[]) + rally.rally_count = lambda: len(rally.rally_points) + rally.rally_point = lambda i: rally.rally_points[i] + modules = SimpleNamespace(wploader=loader, rallyloader=rally) + module.mpstate.module = lambda name: ( + modules if name in ('wp', 'rally') else None) + module.send_mission() + (end_lat, end_lon, end_amsl) = module.sent[-1][-1] + assert mp_util.gps_distance(end_lat, end_lon, HOME[0], HOME[1]) < 150 + # a rally point 150m above home, nearer than home is + rally.rally_points.append(SimpleNamespace( + lat=int(lat * 1e7), lng=int(lon * 1e7), alt=150, flags=0)) + module.send_mission() + (end_lat, end_lon, end_amsl) = module.sent[-1][-1] + assert mp_util.gps_distance(end_lat, end_lon, lat, lon) < 150 + assert end_amsl == pytest.approx(HOME[2] + 150, abs=15) + + def test_a_mission_which_will_not_fly_does_not_stop_the_next(self, monkeypatch): + real = plane_track.mission_track + failures = [RuntimeError('bad mission')] + + def failing(*args, **kwargs): + if failures: + raise failures.pop() + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', failing) + module = self.live_module('plane') + module.send_mission() + assert module.sent[-1] is None + loader = module.mpstate.module('wp').wploader + loader.wpoints[3].y += 0.01 + module.send_mission() + assert module.sent[-1] is not None + + def test_the_path_across_the_antimeridian_is_drawn_with_its_mission(self): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + home = (10.0, 179.998, 0.0) + east = mp_util.gps_newpos(home[0], home[1], 90, 300) + track = plane_track.mission_track( + home, [(mavlink.MAV_CMD_NAV_WAYPOINT, east[0], east[1], 100.0, + (0, 0, 0, 0))], PARAMS) + items = [MissionItem(home[0], home[1], 0.0, 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 0), + MissionItem(east[0], east[1], 100.0, 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 1)] + for origin in (home, (east[0], east[1])): + em = ElementManager(vtk.vtkRenderer(), origin[0], origin[1], 1.0) + em.set_mission(items, track) + marker = em.mission_markers[-1] + assert min(math.hypot(p[0] - marker[0], p[1] - marker[1]) + for p in em.mission_line) < 100.0 + + def test_the_live_map_leaves_other_vehicles_to_the_items(self): + module = self.live_module('copter') + module.send_mission() + assert module.sent[-1] is None + + def test_an_unchanged_mission_is_not_flown_again(self, monkeypatch): + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + module = self.live_module('plane') + module.send_mission() + module.send_mission() + assert len(calls) == 1 + module.mpstate.mav_param['WP_RADIUS'] = 30.0 + module.send_mission() + assert len(calls) == 2 + + def test_the_live_map_takes_off_the_way_the_vehicle_points(self, monkeypatch): + headings = [] + real = plane_track.mission_track + + def recording(home, items, params, heading=None, **kwargs): + headings.append(heading) + return real(home, items, params, heading, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module('plane') + module.map.is_alive = lambda: True + module.map.set_vehicle_type = lambda name: None + module.icon_type = None + mav = mavutil.mavlink + + def heartbeat(armed): + return mav.MAVLink_heartbeat_message( + mav.MAV_TYPE_FIXED_WING, mav.MAV_AUTOPILOT_ARDUPILOTMEGA, + mav.MAV_MODE_FLAG_SAFETY_ARMED if armed else 0, 0, 0, 3) + + def attitude(yaw): + return mav.MAVLink_attitude_message(0, 0, 0, yaw, 0, 0, 0) + + def points(yaw, now): + module.mavlink_packet(attitude(math.radians(yaw))) + module.redraw_for_ground_heading(now) + # on the ground, pointing east: the best guess there is at the course + # it will take off on, and the mission is flown again from it without + # waiting for anything else to change + module.mavlink_packet(heartbeat(False)) + points(90, 100.0) + assert headings == [90] + # a degree or two of wander is not worth flying it again for + points(92, 200.0) + assert headings == [90] + # turning round is, though not more often than every couple of seconds + points(180, 200.5) + assert headings == [90, 180] + points(270, 201.0) + assert headings == [90, 180] + module.redraw_for_ground_heading(203.0) + assert headings == [90, 180, 270] + # once it is armed and flying, where it points is not the takeoff's, + # and a ground station's heartbeat says nothing about the vehicle + module.mavlink_packet(heartbeat(True)) + module.mavlink_packet(mav.MAVLink_heartbeat_message( + mav.MAV_TYPE_GCS, mav.MAV_AUTOPILOT_INVALID, 0, 0, 0, 3)) + points(0, 300.0) + module.send_mission() + assert headings == [90, 180, 270] + + def test_the_live_map_redraws_for_a_new_heading_on_its_own(self, monkeypatch): + # nothing else has to happen for the module's idle task to do it + headings = [] + real = plane_track.mission_track + + def recording(home, items, params, heading=None, **kwargs): + headings.append(heading) + return real(home, items, params, heading, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module('plane') + module.send_mission() + assert headings == [None] + module.map.is_alive = lambda: True + module.map.check_events = lambda: [] + wp_module = module.mpstate.module('wp') + wp_module.wploader.last_change = 1.0 + module.mpstate.module = lambda name: ( + wp_module if name == 'wp' else None) + module.wp_change_time = 1.0 + module.kml_change_state = module._kml_state(None) + module.fence_change_time = module.rally_change_time = 0 + module.terrain_resolved = False + module.note_ground_heading(math.radians(45)) + module.idle_task() + assert headings == [None, 45] + + +class TestMissionStyles(object): + """the 3D map draws a mission as the path flown, as the geometry of its + items, or plain: straight from item to item with a ring at each loiter""" + + RADIUS = 150.0 + + def items(self): + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + (a, b, c) = (offset(0, 0), offset(2000, 0), offset(2000, 2000)) + return [ + MissionItem(a[0], a[1], a[2], 0, mavlink.MAV_CMD_NAV_WAYPOINT, 1), + MissionItem(b[0], b[1], b[2], 0, mavlink.MAV_CMD_NAV_LOITER_TURNS, + 2, 1.0, self.RADIUS, 1.0), + MissionItem(c[0], c[1], c[2], 0, mavlink.MAV_CMD_NAV_WAYPOINT, 3), + ] + + def track(self): + return [offset(0, 0), offset(1000, 30), offset(2000, 2000)] + + def elements(self): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + return em + + def distance_from_loiter(self, em, point): + centre = em._enu(*offset(2000, 0)) + return math.hypot(point[0] - centre[0], point[1] - centre[1]) + + def test_the_path_flown_is_drawn_by_default(self): + em = self.elements() + em.set_mission(self.items(), self.track()) + assert em.mission_line == [em._enu(*p) for p in self.track()] + assert em.mission_rings == [] + + def test_geometry(self): + em = self.elements() + em.set_mission(self.items(), self.track()) + em.set_mission_style('geometry') + # round the circle rather than through its centre, with no ring + assert em.mission_rings == [] + assert min(self.distance_from_loiter(em, p) + for p in em.mission_line) > self.RADIUS - 5.0 + # and a mission which comes with no path flown is drawn the same way + # in the flown style + geometry = em.mission_line + em.set_mission_style('flown') + em.set_mission(self.items()) + assert em.mission_line == geometry + + def test_plain(self): + em = self.elements() + items = self.items() + em.set_mission(items, self.track()) + em.set_mission_style('plain') + assert em.mission_line == [em._enu(i.lat, i.lon, i.alt) for i in items] + (ring,) = em.mission_rings + for point in ring: + assert self.distance_from_loiter(em, point) == pytest.approx( + self.RADIUS, abs=1.0) + assert point[2] == pytest.approx(items[1].alt) + # the ring is drawn as well as the line and the markers + assert len(em.actors['mission']) == 3 + # a new mission is drawn in the style last asked for + em.set_mission(items, self.track()) + assert len(em.mission_rings) == 1 + # and a style it does not know leaves it as it is + em.set_mission_style('fancy') + assert em.mission_style == 'plain' + + def test_the_viewer_is_told_the_style_and_the_mission(self, map3d_frame): + # from the parent's Map3D, over the queue, to the child's frame and + # its ElementManager: nothing but the queue stands in + pytest.importorskip("wx") + pytest.importorskip("vtk") + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.map3d import Map3D + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + sent = [] + viewer = Map3D.__new__(Map3D) + viewer.child = SimpleNamespace(is_alive=lambda: True) + viewer.object_queue = SimpleNamespace(put=sent.append) + em = self.elements() + frame = map3d_frame(em) + items = self.items() + viewer.set_mission_style('plain') + viewer.set_mission(items, self.track()) + for msg in sent: + Map3DFrame.handle(frame, msg) + assert em.mission_style == 'plain' + assert len(em.mission_rings) == 1 + sent[:] = [] + viewer.set_mission_style('flown') + for msg in sent: + Map3DFrame.handle(frame, msg) + assert em.mission_line == [em._enu(*p) for p in self.track()] + # and the map's own control shows the style it is drawing + assert frame.style_choice.GetStringSelection() == 'flown' + + def live_module(self, style): + module = TestDrawnTrack().live_module('plane') + module.map3d_settings.missionpath = style + return module + + def test_the_live_map_only_flies_the_mission_for_the_path_flown(self, monkeypatch): + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + for style in ('geometry', 'plain'): + module = self.live_module(style) + module.send_mission() + assert module.sent[-1] is None + assert calls == [] + module = self.live_module('flown') + module.send_mission() + assert module.sent[-1] is not None + + def test_the_live_map_setting(self): + from types import SimpleNamespace + from MAVProxy.modules.lib import mp_settings + from MAVProxy.modules.mavproxy_map3d.map3d import MISSION_STYLES + module = TestDrawnTrack().live_module('plane') + styles = [] + sends = [] + # settings of the kind the module makes, to see the choice enforced + module.map3d_settings = mp_settings.MPSettings([ + ('fpvfov', float, 90.0), ('terrainbrightness', float, 1.25), + ('terrainshading', bool, True), ('terrainwireframe', bool, False), + ('showdirection', bool, True), ('showlabels', bool, False), + ('labelsize', int, 14), + mp_settings.MPSetting('missionpath', str, MISSION_STYLES[0], + choice=MISSION_STYLES)]) + module.map = SimpleNamespace( + is_alive=lambda: True, set_fpv_fov=lambda fov: None, + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_render_settings=lambda *args: None, + set_mission_style=styles.append, + set_mission=lambda items, track=None: sends.append(track)) + module.cmd_map3d(['set', 'missionpath', 'plain']) + assert styles == ['plain'] + assert sends == [None] + # back to the path flown, which is worked out again to draw + module.cmd_map3d(['set', 'missionpath', 'flown']) + assert styles == ['plain', 'flown'] + assert sends[-1] is not None + # and a style there is not, is not taken + module.cmd_map3d(['set', 'missionpath', 'fancy']) + assert module.map3d_settings.missionpath == 'flown' + + def test_the_control_on_the_map_draws_the_style_it_picks(self, + map3d_frame): + em = self.elements() + em.set_mission(self.items(), self.track()) + events = [] + frame = map3d_frame(em, events) + frame.style_choice.SetStringSelection('plain') + frame.on_style_choice(None) + assert em.mission_style == 'plain' + assert len(em.mission_rings) == 1 + # and the setting follows the control, so a view opened next draws + # the mission the same way + assert events == [('mission_style', 'plain')] + + def test_the_live_map_takes_the_style_picked_on_it(self): + module = self.live_module('plain') + module.map.is_alive = lambda: True + module.map.check_events = lambda: [('mission_style', 'flown')] + echoed = [] + module.map.set_mission_style = echoed.append + # the rest of the idle task has nothing to do here + module.send_kml = lambda kml_mod=None: None + module.kml_change_state = None + module.terrain_resolved = False + module.idle_task() + assert module.map3d_settings.missionpath == 'flown' + # the view is told the setting it now has, whatever it was told since + assert echoed == ['flown'] + # and the mission goes again, with the path flown worked out for it + assert module.sent[-1] is not None + + def unflyable(self, module): + '''give the live module's mission a NAV_DELAY, which cannot be flown + through''' + from types import SimpleNamespace + loader = module.mpstate.module('wp').wploader + loader.wpoints.append(SimpleNamespace( + seq=len(loader.wpoints), command=mavlink.MAV_CMD_NAV_DELAY, + x=0, y=0, z=0, frame=3, param1=10, param2=0, param3=0, param4=0)) + + def test_the_live_map_says_when_a_mission_has_no_path_flown(self, capsys): + module = self.live_module('flown') + self.unflyable(module) + module.send_mission() + assert module.sent[-1] is None + assert 'could not work out the path' in capsys.readouterr().out + # once for the mission, however often it is sent, and when home + # has moved far enough for it to be flown again + module.send_mission() + module.home_amsl += 50.0 + module.send_mission() + assert module.sent[-1] is None + assert capsys.readouterr().out == '' + # and again for another which cannot be flown either + module.mpstate.module('wp').wploader.wpoints[-1].param1 = 20 + module.send_mission() + assert 'could not work out the path' in capsys.readouterr().out + # a mission which can be flown says nothing + module = self.live_module('flown') + module.send_mission() + assert module.sent[-1] is not None + assert capsys.readouterr().out == '' + + def test_the_live_map_says_when_asked_for_a_path_it_cannot_draw( + self, capsys): + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_map3d + module = TestDrawnTrack().live_module('copter') + module.map3d_settings = mavproxy_map3d.make_settings() + styles = [] + module.map = SimpleNamespace( + is_alive=lambda: True, set_fpv_fov=lambda fov: None, + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_mission_style=styles.append, + set_render_settings=lambda *args: None, + set_mission=lambda items, track=None: module.sent.append(track)) + # flown is where it starts, which is not asking for it + module.send_mission() + assert capsys.readouterr().out == '' + module.cmd_map3d(['set', 'missionpath', 'flown']) + assert 'only a plane' in capsys.readouterr().out + module.send_mission() + assert capsys.readouterr().out == '' + # nor is another style + module.cmd_map3d(['set', 'missionpath', 'plain']) + assert capsys.readouterr().out == '' + # and from the view's own choice + module.map.check_events = lambda: [('mission_style', 'flown')] + module.send_kml = lambda kml_mod=None: None + module.kml_change_state = None + module.terrain_resolved = False + module.idle_task() + assert 'only a plane' in capsys.readouterr().out + + +class TestApproachCourse(object): + """where the course of a VTOL landing approach comes from: the wind the + vehicle says it estimates, live, and the course it says it took, in a + log""" + + def live_module(self, approach_landing=True): + module = TestDrawnTrack().live_module( + 'plane', params=dict(PARAMS, Q_ENABLE=1)) + loader = module.mpstate.module('wp').wploader + if approach_landing: + last = loader.wpoints[-1] + (last.command, last.param1) = (mavlink.MAV_CMD_NAV_VTOL_LAND, 1) + module.map.is_alive = lambda: True + return module + + def wind(self, module, direction): + from types import SimpleNamespace + m = SimpleNamespace(direction=direction, speed=5.0, speed_z=0.0) + m.get_type = lambda: 'WIND' + module.mavlink_packet(m) + + def final_course(self, track): + # as the VTOL landing takes over, which is within 30 degrees of + # heading for the landing, or half a radius along the line + return TestVtolApproach().landed(track)[2] + + def test_the_live_map_lands_into_the_wind(self, monkeypatch): + flights = [] + real = plane_track.mission_track + + def recording(*args, **kwargs): + flights.append(kwargs.get('approach')) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module() + module.send_mission() + # still air, until the vehicle says otherwise + assert flights == [None] + self.wind(module, 88.0) + assert flights == [None, 90.0] + assert mp_util.wrap_180(self.final_course(module.sent[-1]) - 90) == \ + pytest.approx(0, abs=45) + # the estimate wandering is not a new flight, and a real change is + self.wind(module, 93.0) + module.send_mission() + assert flights == [None, 90.0] + self.wind(module, 130.0) + assert flights == [None, 90.0, 130.0] + # either side of due south is the one course + self.wind(module, 179.0) + self.wind(module, -176.0) + assert flights == [None, 90.0, 130.0, -180.0] + + def test_a_wind_with_no_estimate_changes_nothing(self, monkeypatch): + # an EKF with no estimate of the wind sends NaN + flights = [] + real = plane_track.mission_track + + def recording(*args, **kwargs): + flights.append(kwargs.get('approach')) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module() + module.send_mission() + self.wind(module, 88.0) + for direction in (float('nan'), float('inf'), -float('inf')): + self.wind(module, direction) + module.send_mission() + # the course stands, so the path is not drawn again + assert module.approach == 90.0 + assert flights == [None, 90.0] + + def test_the_wind_does_not_matter_to_a_mission_landing_otherwise( + self, monkeypatch): + flights = [] + real = plane_track.mission_track + + def recording(*args, **kwargs): + flights.append(kwargs.get('approach')) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module(approach_landing=False) + module.send_mission() + self.wind(module, 90.0) + module.send_mission() + self.wind(module, 200.0) + module.send_mission() + assert flights == [None] From 3b7dc2444480ea9797b26da828044a3b59d8a075 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 3 Oct 2026 12:41:04 +1000 Subject: [PATCH 16/22] MAVExplorer: draw the path a log's mission was flown along MAVExplorer reads the mission a log holds with LogMission, so a dataflash log and a telemetry log are read the same way, and draws the path the vehicle flies it along in the 3D map, with the rally points a return may go to and home as the log gives them. What the log says of the flight is used where the mission does not say: where the mission's first item began, which is where the flight starts, and the course measured over the start of the takeoff, which is the way a QuadPlane pointed as it transitioned and the course a fixed-wing takeoff held -- nothing before a launch says either. The course a VTOL landing approach was flown on is the one the vehicle announced. A mission downloaded again mid-flight is recognised as the one being flown, compared as AP_Mission gives it back and leaving out home, which is the vehicle's own: so where the flight had reached in it survives a ground station asking the vehicle what it holds. What the log holds only part of -- a mission or a rally table a transfer ends part way through -- is drawn, since those items are where the vehicle was told to go, but not flown, since what would have followed them is not known. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/tools/MAVExplorer.py | 609 +++++++++++++- tests/test_map3d_mission_labels.py | 89 ++ tests/test_map3d_settings_sync.py | 30 + tests/test_mission_geometry.py | 11 +- tests/test_plane_track.py | 1230 ++++++++++++++++++++++++++++ 5 files changed, 1939 insertions(+), 30 deletions(-) diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index b6a03a9cc0..02b8f1ad72 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -13,6 +13,7 @@ import threading import shlex import traceback +from types import SimpleNamespace from math import * os.environ['MAVLINK20'] = '1' @@ -23,6 +24,7 @@ from MAVProxy.modules.lib import wxconsole from MAVProxy.modules.lib import param_help from MAVProxy.modules.lib import param_ftp +from MAVProxy.modules.lib import log_mission from MAVProxy.modules.lib.graph_ui import Graph_UI from pymavlink.mavextra import * from MAVProxy.modules.lib.mp_menu import * @@ -31,6 +33,8 @@ from pymavlink import mavwp from pymavlink import DFReader from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting +from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_LABEL_SIZES, MISSION_STYLES) from MAVProxy.modules.lib import wxsettings from MAVProxy.modules.lib.graphdefinition import GraphDefinition from lxml import objectify @@ -122,6 +126,14 @@ def __init__(self): MPSetting('vehicle_type', str, 'Auto', 'force vehicle type for mode handling'), MPSetting('showdirection', bool, True, 'show direction of travel on the 3D map mission'), + MPSetting('showlabels', bool, False, + 'label the mission items on the 3D map'), + MPSetting('labelsize', int, MISSION_LABEL_SIZE, + 'size of the 3D map mission labels, in points', + range=MISSION_LABEL_SIZES), + MPSetting('missionpath', str, MISSION_STYLES[0], + 'draw the 3D map mission as ' + ', '.join(MISSION_STYLES), + choice=list(MISSION_STYLES)), ] ) @@ -661,6 +673,212 @@ def cmd_map(args): map3d_views = [] +# the mission and rally protocol's messages, which a telemetry log carries +# as the ground station and the vehicle sent them +MISSION_PROTOCOL = ('MISSION_COUNT', 'MISSION_ITEM', 'MISSION_ITEM_INT', + 'MISSION_CLEAR_ALL') + + +def rally_points(rally_log): + '''a LogMission's rally table as mission_from_log() gives it, by + sequence: where each point is, and the flags AP_Rally keeps the frame + of its altitude in''' + out = {} + for (seq, item) in rally_log.whole().items(): + if item.command == mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT: + out[seq] = (item.x, item.y, item.z, + rally_flags_for_frame(item.frame)) + return out + + +# how far apart two of a mission's items can be and still be the one item. +# AP_Mission keeps a position in 1e-7 degrees, but turns a float +# MISSION_ITEM's into that in float32, which is up to 6.4e-6 degrees out at +# longitudes past 128; and it gives a float MISSION_ITEM back by turning the +# integer into float32 and then multiplying it in float32, which is out by +# up to 1.6e-5 degrees +SAME_POSITION = 2.0e-5 # degrees, about 2m +SAME_ALTITUDE = 0.1 # metres + +# the commands AP_Mission keeps a position for (stored_in_location()); the +# rest go back with no position, and frame 0, whatever they came in with +LOCATION_COMMANDS = frozenset(( + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, + mavutil.mavlink.MAV_CMD_NAV_LAND, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, + mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT, + mavutil.mavlink.MAV_CMD_NAV_GUIDED_ENABLE, + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + mavutil.mavlink.MAV_CMD_DO_RETURN_PATH_START, + mavutil.mavlink.MAV_CMD_DO_LAND_START, + mavutil.mavlink.MAV_CMD_DO_GO_AROUND, + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + mavutil.mavlink.MAV_CMD_DO_SET_ROI, + mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, + mavutil.mavlink.MAV_CMD_NAV_PAYLOAD_PLACE, + mp_util.MAV_CMD_NAV_ARC_WAYPOINT, +)) + +# the frame AP_Mission gives an item with a position back in: it keeps only +# whether the altitude is above home or terrain, and never says _INT. An +# item uploaded in MISSION, the frame for one with no position, is kept as +# absolute (AP_Mission::mavlink_int_to_mission_cmd) +RETURNED_FRAME = { + mavutil.mavlink.MAV_FRAME_MISSION: mavutil.mavlink.MAV_FRAME_GLOBAL, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT: mavutil.mavlink.MAV_FRAME_GLOBAL, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, +} + + +def kept_whole(value): + '''a parameter as AP_Mission keeps it in an integer''' + return int(value) if isfinite(value) else 0 + + +def kept_signed(value): + '''a radius or an angle AP_Mission keeps as a whole number and a + direction''' + return copysign(kept_whole(abs(value)), value) + + +def kept_float(value): + '''a parameter AP_Mission keeps as it came, where NaN is NaN again''' + return value if isfinite(value) else None + + +def kept_turns(value): + '''the turns of NAV_LOITER_TURNS: fewer than one in 256ths, and at most + 255 of them''' + if 0 < value < 1: + return kept_whole(value * 256) / 256.0 + return min(255, max(0, kept_whole(value))) + + +def kept_loiter_radius(value): + '''the radius of NAV_LOITER_TURNS: past 255m it is kept in tens of + metres, and at most 2550m''' + radius = abs(value) + if radius > 255: + radius = min(255, kept_whole(radius * 0.1)) * 10 + return copysign(kept_whole(radius), value) + + +def kept_ccw(value): + return value < 0 + + +def kept_xtrack(value): + return value > 0 + + +# the parameters of an item AP_Mission sends back as they were sent to it, +# every vehicle alike, as it keeps each of them. Another parameter goes +# back as 0, or packed up, or only on some vehicles -- NAV_WAYPOINT's are +# its delay on a copter and its acceptance radii on a plane -- so is no sign +# of another mission +RETURNED_PARAMS = { + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM: + (None, None, kept_signed, None), + mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS: + (kept_turns, None, kept_loiter_radius, kept_xtrack), + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME: + (kept_whole, None, kept_ccw, kept_xtrack), + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + (None, kept_signed, None, kept_xtrack), + mp_util.MAV_CMD_NAV_ARC_WAYPOINT: + (kept_signed, None, None, None), + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: + (kept_whole, None, None, None), + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: + (kept_whole, None, None, None), + mavutil.mavlink.MAV_CMD_CONDITION_DELAY: + (kept_float, None, None, None), + mavutil.mavlink.MAV_CMD_DO_JUMP: + (kept_whole, kept_whole, None, None), + mavutil.mavlink.MAV_CMD_DO_JUMP_TAG: + (kept_whole, kept_whole, None, None), + mavutil.mavlink.MAV_CMD_JUMP_TAG: + (kept_whole, None, None, None), + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED: + (kept_whole, kept_float, kept_float, None), +} + + +def returned_item(item): + '''a mission item as AP_Mission sends it back: what of it would come + back from the vehicle whichever way it was sent there''' + (lat, lon, alt, frame, command, _, params) = item + kept = RETURNED_PARAMS.get(command, (None,) * 4) + params = tuple(k(p) for (k, p) in zip(kept, params) if k is not None) + if command not in LOCATION_COMMANDS: + return (command, None, params, None) + return (command, RETURNED_FRAME.get(frame, frame), params, (lat, lon, alt)) + + +def same_mission(before, after): + '''whether a mission a log has just seen is the one it held, rather than + another. Item 0 is left out of it: that is home, the vehicle's own and + not the mission's -- an uploaded one is never stored + (AP_Mission::replace_cmd) and a downloaded one is wherever home is now, + which the vehicle keeps setting while it is disarmed -- so a mission + downloaded to see what the vehicle holds differs there from the upload + of it every time. The rest are compared as AP_Mission keeps them, since + a download gives back only that''' + if sorted(before) != sorted(after): + return False + for seq in before: + if seq == 0: + continue + (command, frame, params, where) = returned_item(before[seq]) + (was_command, was_frame, was_params, was_where) = \ + returned_item(after[seq]) + if (command, frame, params) != (was_command, was_frame, was_params): + return False + if where is None: + continue + (lat, lon, alt) = where + (was_lat, was_lon, was_alt) = was_where + if (abs(lat - was_lat) > SAME_POSITION or + abs(lon - was_lon) > SAME_POSITION or + abs(alt - was_alt) > SAME_ALTITUDE): + return False + return True + + +class RallyPoints(list): + '''a log's rally points in order, and whether the table they are of + arrived whole: the points of one a log ends part way through are points + to draw, and not a table to work a return out from, since the one it + would go to may be among those which never arrived''' + whole = True + + +class MissionCmds(dict): + '''a log's CMD entries keyed by sequence, which is how this reads a + mission, and whether the mission they are arrived whole: the items of one + a log ends part way through are a mission to draw and not one to fly''' + whole = True + + +def mission_cmds(mission_log): + '''a LogMission's mission as a log's CMD entries keyed by sequence''' + out = MissionCmds() + out.whole = not mission_log.partial() + for (seq, item) in mission_log.whole().items(): + out[seq] = (item.x, item.y, item.z, item.frame, item.command, seq, + (item.param1, item.param2, item.param3, item.param4)) + return out + + def mission_items_from_cmds(items, started_at=None, flown_from=None): '''the mission items to draw, from a log's CMD entries keyed by sequence. @@ -692,51 +910,189 @@ def mission_items_from_cmds(items, started_at=None, flown_from=None): return mission +# what ArduPlane says as it picks the course of a VTOL landing approach +VTOL_APPROACH_MESSAGE = 'Selected an approach path of ' + +# a rally point's altitude frame, as its flags carry it (Location::AltFrame), +# for each MAVLink frame a rally item in the mission protocol can be in +RALLY_FRAME_FLAGS = { + mavutil.mavlink.MAV_FRAME_GLOBAL: 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT: 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT: 1, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: 1, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT: 3, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: 3, +} + + +def rally_flags_for_frame(frame): + '''the flags of a rally point in frame, as AP_Rally keeps them: the + frame, and the bit which says the flags carry one''' + return (1 << 2) | (RALLY_FRAME_FLAGS.get(frame, 1) << 3) + + def mission_from_log(mlog, condition=None): - '''the flight path in a log, and the mission the log ends with. + '''the flight path in a log, the mission the log ends with, that + mission's CMD entries keyed by sequence -- every one of them, where the + mission drawn has only the ones with a position -- by sequence, the + index into the path at which the log says each of its items began, the + rally points the log ends with, as (lat, lon, alt, flags), the EKF + origin, which a rally point's altitude may be measured from, the + course of the last VTOL landing approach the vehicle announced, or None. A log carries the whole mission again every time one is uploaded, and once more when it starts, so the items are kept by sequence number and the logger's "New mission" -- written before every one of them, even a mission with no items at all -- starts them again. Drawing every mission - the flight ever held would join them into one line + the flight ever held would join them into one line. + + A telemetry log carries the same things as the MAVLink messages the + vehicle sent: its position, home and origin, what it said, and the + mission and rally points as they were downloaded or uploaded while the + log was kept. Only a transfer which arrives whole replaces what was + held, so one the log ends part way through leaves the table before it, + and a log which starts part way through one holds no mission at all ''' path = [] + # the mission and the rally points, as whatever kind of log this is + # carries them: a table which never arrives whole says nothing about the + # one the vehicle holds, so the one before it stands + mission_log = log_mission.LogMission() + rally_log = log_mission.LogMission( + mavutil.mavlink.MAV_MISSION_TYPE_RALLY) items = {} # where the vehicle was when each item began, from the "Mission: " - # the autopilot announces as it starts one + # the autopilot announces as it starts one, and how far along the path flown_from = {} - # home, as the vehicle last logged it + started = {} + # home, and the EKF origin, as the vehicle last logged them home = None + origin = None + # the rally points, by sequence. The logger writes the whole table out + # when one is uploaded, and a single point when just that one is set -- + # appending one is logged as that one alone, with the new total -- so + # they are kept by sequence, and only the ones a smaller total leaves + # out are dropped + rally = {} + # whether the points held are the whole of the table they are of + rally_whole = True + approach = None + # the vehicle, in a telemetry log: its first heartbeat says which it is + vehicle = None while True: - m = mlog.recv_match(type=['POS', 'CMD', 'MSG', 'ORGN'], + m = mlog.recv_match(type=['POS', 'CMD', 'MSG', 'ORGN', 'RALY', + 'HEARTBEAT', 'GLOBAL_POSITION_INT', + 'HOME_POSITION', 'GPS_GLOBAL_ORIGIN', + 'STATUSTEXT', 'MISSION_COUNT', + 'MISSION_ITEM', 'MISSION_ITEM_INT', + 'MISSION_CLEAR_ALL', 'RALLY_POINT'], condition=condition) if m is None: break mtype = m.get_type() + if mtype == 'HEARTBEAT': + if (vehicle is None and + m.autopilot != mavutil.mavlink.MAV_AUTOPILOT_INVALID and + m.type != mavutil.mavlink.MAV_TYPE_GCS): + vehicle = m.get_srcSystem() + continue + if mtype in MISSION_PROTOCOL: + # downloaded from the vehicle, or uploaded to it + if vehicle not in (m.get_srcSystem(), m.target_system): + continue + elif (mtype in ('GLOBAL_POSITION_INT', 'HOME_POSITION', + 'GPS_GLOBAL_ORIGIN', 'STATUSTEXT', 'RALLY_POINT') and + m.get_srcSystem() != vehicle): + continue + if mtype == 'GLOBAL_POSITION_INT': + if m.lat == 0 and m.lon == 0: + continue + m = SimpleNamespace(Lat=m.lat * 1.0e-7, Lng=m.lon * 1.0e-7, + Alt=m.alt * 0.001, _timestamp=m._timestamp) + mtype = 'POS' + elif mtype == 'HOME_POSITION': + m = SimpleNamespace(Type=1, Lat=m.latitude * 1.0e-7, + Lng=m.longitude * 1.0e-7, + Alt=m.altitude * 0.001) + mtype = 'ORGN' + elif mtype == 'GPS_GLOBAL_ORIGIN': + m = SimpleNamespace(Type=0, Lat=m.latitude * 1.0e-7, + Lng=m.longitude * 1.0e-7, + Alt=m.altitude * 0.001) + mtype = 'ORGN' + elif mtype == 'STATUSTEXT': + m = SimpleNamespace(Message=m.text) + mtype = 'MSG' + elif mtype == 'RALLY_POINT': + # the old rally protocol's, which carry AP_Rally's own flags + m = SimpleNamespace(Seq=m.idx, Tot=m.count, Lat=m.lat * 1.0e-7, + Lng=m.lng * 1.0e-7, Alt=m.alt, Flags=m.flags) + mtype = 'RALY' + elif mtype in MISSION_PROTOCOL: + # a rally table downloaded or uploaded, whose points are carried + # as a mission's items are. One which arrives whole is what the + # vehicle holds, even where the table was that already: the + # points the old protocol set may not be there any more + before = rally_points(rally_log) + arrivals = rally_log.arrivals + rally_log.read(m, mtype) + after = rally_points(rally_log) + if after != before or rally_log.arrivals != arrivals: + rally = after + # a table which never arrived whole is what is known of the + # one the vehicle holds, and all that is known: the points + # the old protocol set before it are gone + rally_whole = not rally_log.partial() + if mtype in log_mission.MISSION_TYPES: + # the mission itself, which LogMission keeps: only the last one + # to arrive whole, so a download which stops part way through + # leaves the one before it as it was + before = mission_cmds(mission_log) + mission_log.read(m, mtype) + if not same_mission(before, mission_cmds(mission_log)): + # a mission other than the one flown so far: where the + # flight had got to in that one says nothing about this + flown_from = {} + started = {} + if mtype != 'MSG': + continue if mtype == 'POS': path.append((m.Lat, m.Lng, m.Alt, grapher.timestamp_to_days(m._timestamp))) + elif mtype == 'RALY': + if any(seq >= m.Tot for seq in rally): + rally = dict((seq, point) for (seq, point) in rally.items() + if seq < m.Tot) + if m.Seq < m.Tot: + # RALY only carries the altitude frame from 4.5; before that + # a rally point's altitude is above home, which is flags 0 + rally[m.Seq] = (m.Lat, m.Lng, m.Alt, getattr(m, 'Flags', 0)) + # the logger writes the whole table out, so the points held are + # the table once there are as many of them as it says + rally_whole = len(rally) == m.Tot elif mtype == 'ORGN': + # AP_AHRS: type 0 is the EKF origin, type 1 home if m.Type == 1 and (m.Lat != 0 or m.Lng != 0): home = (m.Lat, m.Lng, m.Alt) + elif m.Type == 0 and (m.Lat != 0 or m.Lng != 0): + origin = (m.Lat, m.Lng, m.Alt) elif mtype == 'MSG': fields = m.Message.split() - if m.Message == 'New mission': - items = {} - flown_from = {} + if m.Message.startswith(VTOL_APPROACH_MESSAGE): + # the course the vehicle took into the wind it had then + try: + approach = float(m.Message[len(VTOL_APPROACH_MESSAGE):]) + except ValueError: + pass + elif m.Message == 'New rally': + # written before the whole table, even an empty one: a + # cleared table is this and no points at all + rally = {} elif (len(fields) > 1 and fields[0] == 'Mission:' and fields[1].isdigit() and path): flown_from.setdefault(int(fields[1]), (path[-1][0], path[-1][1])) - elif mtype == 'CMD': - if m.CNum == 0 and len(items) > 1: - # a log from before the logger said so: the first item of a - # mission still means whatever is held is stale - items = {} - flown_from = {} - params = tuple(getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)) - items[m.CNum] = (m.Lat, m.Lng, m.Alt, getattr(m, 'Frame', 3), - m.CId, m.CNum, params) + started.setdefault(int(fields[1]), len(path) - 1) + items = mission_cmds(mission_log) if (home is not None and 0 in items and items[0][0] == 0 and items[0][1] == 0): # a mission uploaded before the vehicle had a home carries an empty @@ -745,7 +1101,146 @@ def mission_from_log(mlog, condition=None): mission = mission_items_from_cmds( items, started_at=(path[0][0], path[0][1]) if path else None, flown_from=flown_from) - return (path, mission) + points = RallyPoints(rally[seq] for seq in sorted(rally)) + points.whole = rally_whole + return (path, mission, items, started, points, origin, approach) + + +def takeoff_flown(path, items, started): + '''the takeoff of the mission the log shows flown, as its CMD entry, and + the course in degrees it was flown on; (None, None) where the log shows + none. ArduPlane holds the ground course it has once it gets moving, and + a QuadPlane transitions the way it was pointing, neither of which the + mission says, so the course is taken from the path: where the aircraft + was when the takeoff began, towards where it was once it had gone a + little way''' + for (seq, index) in sorted(started.items()): + item = items.get(seq) + if item is None or item[4] not in ( + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF): + continue + (lat, lon) = path[index][:2] + for point in path[index + 1:]: + if mp_util.gps_distance(lat, lon, point[0], point[1]) >= 30.0: + return (item, mp_util.gps_bearing(lat, lon, point[0], + point[1])) + return (None, None) + + +def takeoff_course(path, items, started): + '''the course in degrees the mission's takeoff was flown on, or None''' + return takeoff_flown(path, items, started)[1] + + +# a rally point is one point of one view: wait only so long for the terrain +# under it, rather than the whole fetch the tiles drawn are worth waiting for +RALLY_TERRAIN_TIMEOUT = 5.0 + + +def rally_point_amsl(lat, lon, alt, flags, home_amsl, origin=None): + '''the AMSL altitude of a rally point from a log. A point above the + terrain is resolved through the same quantized mesh the 3D view draws; + one above the EKF origin falls back to home, which the origin is + usually near, where the log does not say where the origin was''' + from MAVProxy.modules.lib import plane_track + frame = plane_track.rally_alt_frame(flags) + terrain = None + if frame == plane_track.RALLY_ALT_ABOVE_TERRAIN: + try: + from MAVProxy.modules.mavproxy_map3d.terrain import sample_terrain + terrain = sample_terrain(lat, lon, timeout=RALLY_TERRAIN_TIMEOUT) + except Exception as ex: + print("map3d: terrain elevation unavailable (%s)" % ex) + origin_amsl = origin[2] if origin else None + if frame == plane_track.RALLY_ALT_ABOVE_ORIGIN and origin_amsl is None: + origin_amsl = home_amsl + return plane_track.rally_amsl(alt, flags, home_amsl, origin_amsl, terrain) + + +def plane_mission_track(cmds, mission, started_at, params=None, mav_type=None, + path=None, started=None, rally=None, origin=None, + approach=None): + '''the path a plane flies a log's mission along, as (lat, lon, amsl) + points, or None for a vehicle which is not a plane or a mission which + cannot be flown through. + + cmds are the mission's CMD entries keyed by sequence, and mission the + MissionItems resolve_mission_amsl() made of them, which carry the + positions and AMSL altitudes worked out for the ones drawn. The flight + starts from home, or from started_at where the log's mission has no home + of its own. path and started are the flight path and where along it + each item began, as mission_from_log() gives them: where the log shows + the mission's first item beginning, the flight starts there instead, + and a takeoff is flown as the course measured over the start of it says: + a fixed-wing one on that course, and a VTOL one transitioning onto it. + rally is the log's rally points, which a return to launch may go to, and + origin the EKF origin one of them may be measured from. approach is the + course of a VTOL landing approach, which the vehicle flies into the wind + it has then: the one the log says it took, where it says so, and + otherwise that of still air, since the wind at some other time says + little about it + ''' + if mp_util.vehicle_type_name(mav_type) != 'plane': + return None + if sorted(cmds) != list(range(len(cmds))): + # an item is missing from the log. The flight is numbered by + # sequence -- jumps go to one, and where each began is kept by one -- + # so there is no flying it with a gap, and no guessing what was there + return None + if not getattr(cmds, 'whole', True): + # the log ends part way through the only transfer of this mission: + # flying what arrived would end it with a return to launch, or a + # landing, which whatever was still to come may say nothing of + return None + if not getattr(rally, 'whole', True): + # nor is a mission flown whose return may go to a rally point among + # those the log ends part way through the table of + return None + from MAVProxy.modules.lib import plane_track + placed = {item.seq: item for item in mission} + home = placed.get(0) + if home is not None: + home = (home.lat, home.lon, home.alt) + elif started_at is not None: + home = tuple(started_at) + items = [] + for seq in sorted(cmds): + if seq == 0: + continue + (lat, lon, alt, frame, command, _, prm) = cmds[seq] + item = placed.get(seq) + if item is not None: + items.append((command, item.lat, item.lon, item.alt, prm)) + else: + items.append((command, 0.0, 0.0, + plane_track.positionless_amsl( + alt, frame, home[2] if home else None), + prm)) + heading = None + course = None + start = None + if path and started: + # the course measured over the start of the takeoff, which is the way + # a QuadPlane pointed as it transitioned, and for a fixed-wing + # takeoff the course it held: nothing but a flight of one says either + (takeoff, measured) = takeoff_flown(path, cmds, started) + if takeoff is not None: + if plane_track.takeoff_is_vtol(takeoff[4], params or {}): + heading = measured + else: + course = measured + first = plane_track.first_navigation_item(items) + if first in started: + (lat, lon, alt) = path[started[first]][:3] + start = (lat, lon, alt) + rally_points = [(lat, lon, rally_point_amsl(lat, lon, alt, flags, + home[2] if home else None, + origin)) + for (lat, lon, alt, flags) in (rally or [])] + return plane_track.mission_track(home, items, params, heading, start, + rally_points, approach, + takeoff_course=course) def path_view(path): @@ -797,11 +1292,12 @@ def cmd_map3d(args): return mlog = mestate.mlog - (path, mission) = mission_from_log(mlog, mestate.settings.condition) + (path, mission, cmds, started, rally, origin, + approach) = mission_from_log(mlog, mestate.settings.condition) mlog.rewind() if len(path) == 0: - print("No POS messages found for 3D map") + print("No POS or GLOBAL_POSITION_INT messages found for 3D map") return (lat0, lon0, ground0, span) = path_view(path) @@ -809,10 +1305,25 @@ def cmd_map3d(args): # resolve mission item altitudes to AMSL before sending. Terrain-frame # waypoints (MAV_FRAME_GLOBAL_TERRAIN_ALT = 10/11) are "z above terrain", so # they need the terrain elevation at the waypoint, not home + z. + track = None if mission: - mission = resolve_mission_amsl(mission, ground0, - mp_util.log_params(mlog), - getattr(mlog, 'mav_type', None)) + params = mp_util.log_params(mlog) + mav_type = getattr(mlog, 'mav_type', None) + mission = resolve_mission_amsl(mission, ground0, params, mav_type) + + # a plane does not fly the lines between its items; where the log + # says enough to fly the mission through its navigation, that path + # is drawn, and worked out only when it is + def fly(): + return plane_mission_track(cmds, mission, + (path[0][0], path[0][1], ground0), + params, mav_type, path, started, + rally, origin, approach) + if mestate.settings.missionpath == 'flown': + track = fly() + if (track is None and + mp_util.vehicle_type_name(mav_type) == 'plane'): + say_unflown() # drop views the user has already closed, so their child processes are reaped for old in [v for v in map3d_views if not v.is_alive()]: @@ -824,11 +1335,18 @@ def cmd_map3d(args): m3d.set_origin(lat0, lon0, ground0) m3d.set_home(ground0) m3d.set_mission_arrows(mestate.settings.showdirection) + m3d.set_mission_labels(mestate.settings.showlabels) + m3d.set_mission_label_size(mestate.settings.labelsize) + m3d.set_mission_style(mestate.settings.missionpath) m3d.set_path(path) if xlimits.last_xlim is not None and mestate.settings.sync_xmap: m3d.set_time_range(xlimits.last_xlim) if mission: - m3d.set_mission(mission) + m3d.set_mission(mission, track) + # kept, to draw the path flown if the style changes to it later + m3d.mission_to_fly = None + if track is None and mestate.settings.missionpath != 'flown': + m3d.mission_to_fly = (mission, fly) m3d.look_at(lat0, lon0, ground0, dist=1.6 * span) def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): @@ -890,10 +1408,50 @@ def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): def cmd_set(args): '''control MAVExporer options''' mestate.settings.command(args) - # settings the open 3D views care about + update_map3d_views() + +def update_map3d_views(): + '''push the settings the open 3D views care about''' for view in map3d_views: if view.is_alive(): view.set_mission_arrows(mestate.settings.showdirection) + view.set_mission_labels(mestate.settings.showlabels) + view.set_mission_label_size(mestate.settings.labelsize) + view.set_mission_style(mestate.settings.missionpath) + pending = getattr(view, 'mission_to_fly', None) + if mestate.settings.missionpath == 'flown' and pending is not None: + (mission, fly) = pending + view.mission_to_fly = None + track = fly() + if track is None: + say_unflown() + view.set_mission(mission, track) + +def say_unflown(): + print("map3d: could not work out the path this mission is flown along " + "-- only a plane's can be, and not one which is too long, never " + "finishes an item, uses a command which cannot be flown here, or " + "which the log does not hold all of; drawing its geometry") + +def poll_map3d_views(): + '''take what the open 3D views' own controls have been set to. A view + which has died is drained too, so we do not lose the reason it failed + to start''' + changed = False + for view in map3d_views: + for event in view.check_events(): + if event[0] == 'startup_error': + print("map3d: the 3D view failed to start:\n%s" % event[1]) + elif event[0] == 'mission_labels': + mestate.settings.showlabels = bool(event[1]) + changed = True + elif event[0] == 'mission_style': + mestate.settings.missionpath = event[1] + changed = True + if changed: + # every view follows the setting, and the path flown is only worked + # out while it is drawn + update_map3d_views() def cmd_condition(args): '''control MAVExporer conditions''' @@ -2118,6 +2676,7 @@ def main_loop(): cmds = line.split(';') for c in cmds: process_stdin(c) + poll_map3d_views() remlist = [] for i in range(0, len(grui)): diff --git a/tests/test_map3d_mission_labels.py b/tests/test_map3d_mission_labels.py index 8c2e4339dc..2851aa6b42 100644 --- a/tests/test_map3d_mission_labels.py +++ b/tests/test_map3d_mission_labels.py @@ -218,3 +218,92 @@ def test_a_map_with_no_scene_yet_keeps_what_it_is_told(self, map3d_frame): assert label_sizes(em) == [20] * len(LABELS) assert em.mission_style == 'plain' assert em.mission_arrows is True + + def test_mavexplorer_labels_a_view_it_opens(self, monkeypatch, mavexplorer): + mx = mavexplorer + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + labelled = [] + + class Viewer(object): + def __init__(self, title=None): + pass + + def is_alive(self): + return True + + def set_mission_labels(self, enable): + labelled.append(enable) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + + def message(kind, **fields): + m = SimpleNamespace(_timestamp=0, **fields) + m.get_type = lambda: kind + return m + rows = [message('POS', Lat=HOME[0], Lng=HOME[1], Alt=HOME[2]), + message('POS', Lat=HOME[0] + 0.01, Lng=HOME[1], + Alt=HOME[2] + 50)] + log = SimpleNamespace(rewind=lambda: None, mav_type=None) + log.recv_match = lambda **kwargs: rows.pop(0) if rows else None + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=SimpleNamespace( + condition=None, showdirection=True, showlabels=True, + labelsize=20, sync_xmap=False, missionpath='geometry')), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + assert labelled == [True] + + def test_mavexplorer_labels_a_mission_it_draws(self, monkeypatch, mavexplorer): + from types import SimpleNamespace + mx = mavexplorer + labelled = [] + sizes = [] + view = SimpleNamespace( + is_alive=lambda: True, + set_mission_arrows=lambda enable: None, + set_mission_labels=labelled.append, + set_mission_label_size=sizes.append, + set_mission_style=lambda style: None, + mission_to_fly=None) + settings = SimpleNamespace(showdirection=True, showlabels=True, + labelsize=20, missionpath='geometry', + command=lambda args: None) + monkeypatch.setattr(mx, 'mestate', SimpleNamespace(settings=settings), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', [view]) + mx.cmd_set(['showlabels', 'true']) + assert labelled == [True] + assert sizes == [20] + + def test_mavexplorer_takes_the_checkbox_on_a_view(self, monkeypatch, mavexplorer): + '''a view labelled from its own checkbox sets the setting, so every + view is labelled and one opened later is too''' + from types import SimpleNamespace + mx = mavexplorer + labelled = [] + other = SimpleNamespace( + is_alive=lambda: True, check_events=lambda: [], + set_mission_arrows=lambda enable: None, + set_mission_labels=labelled.append, + set_mission_label_size=lambda size: None, + set_mission_style=lambda style: None, mission_to_fly=None) + view = SimpleNamespace( + is_alive=lambda: True, + check_events=lambda: [('mission_labels', True)], + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_mission_style=lambda style: None, mission_to_fly=None) + settings = SimpleNamespace(showdirection=True, showlabels=False, + labelsize=14, missionpath='geometry') + monkeypatch.setattr(mx, 'mestate', SimpleNamespace(settings=settings), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', [view, other]) + mx.poll_map3d_views() + assert settings.showlabels is True + assert labelled == [True] diff --git a/tests/test_map3d_settings_sync.py b/tests/test_map3d_settings_sync.py index 6bc45513e7..3e77e8e21a 100644 --- a/tests/test_map3d_settings_sync.py +++ b/tests/test_map3d_settings_sync.py @@ -162,6 +162,36 @@ def test_render_settings_wait_for_a_scene(self, map3d_frame, Map3DFrame.handle(frame, ('origin', HOME[0], HOME[1], HOME[2])) assert frame.terrain.render_settings == (1.25, False, False) + def test_mavexplorer_and_its_views_agree(self, map3d_frame, mavexplorer, + monkeypatch): + '''MAVExplorer has the same crossing, between a view's checkbox and + its own set command''' + from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting + settings = MPSettings([ + MPSetting('showdirection', bool, True), + MPSetting('showlabels', bool, False), + MPSetting('labelsize', int, 14), + MPSetting('missionpath', str, 'flown', + choice=['flown', 'geometry', 'plain'])]) + monkeypatch.setattr(mavexplorer, 'mestate', + SimpleNamespace(settings=settings), raising=False) + views = [connect(map3d_frame), connect(map3d_frame)] + monkeypatch.setattr(mavexplorer, 'map3d_views', + [viewer for (viewer, _) in views]) + labels_on_the_view(views[0][1]) + style_on_the_view(views[0][1]) + mavexplorer.cmd_set(['showlabels', 'false']) + mavexplorer.poll_map3d_views() + for (viewer, frame) in views: + deliver(viewer, frame) + for (viewer, frame) in views: + assert viewer.event_queue.empty() + assert CONTROLS['labels'][3](frame) == [settings.showlabels] * 3 + assert CONTROLS['style'][3](frame) == [settings.missionpath] * 3 + # where the two crossed, what MAVExplorer heard last stands: the + # view's, which it read after the command + assert settings.showlabels is True + def display(): return bool(os.environ.get('DISPLAY') or os.environ.get('WAYLAND_DISPLAY')) diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 39d5468977..f3bd0917db 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -668,7 +668,7 @@ def dump(self, where, count, command=16): def test_a_log_draws_the_last_mission_it_holds(self): first = HERE second = mp_util.gps_newpos(HERE[0], HERE[1], 0, 5000) - (path, mission) = self.module().mission_from_log( + (path, mission, _, _, _, _, _) = self.module().mission_from_log( self.log(*(self.dump(first, 4) + self.dump(second, 2)))) # just the second mission, even though it is the shorter of the two assert len(mission) == 2 @@ -676,7 +676,7 @@ def test_a_log_draws_the_last_mission_it_holds(self): def test_a_cleared_mission_draws_nothing(self): # clearing the mission writes the message and then no items at all - (path, mission) = self.module().mission_from_log( + (path, mission, _, _, _, _, _) = self.module().mission_from_log( self.log(*(self.dump(HERE, 4) + [self.message('MSG', Message='New mission')]))) assert mission == [] @@ -690,7 +690,7 @@ def cmd(seq, command, lat, lng, alt): return self.message('CMD', CNum=seq, CId=command, Lat=lat, Lng=lng, Alt=alt, Frame=3, Prm1=0, Prm2=0, Prm3=0, Prm4=0) - (path, mission) = self.module().mission_from_log(self.log( + (path, mission, _, _, _, _, _) = self.module().mission_from_log(self.log( self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), self.message('MSG', Message='New mission'), cmd(0, m.MAV_CMD_NAV_WAYPOINT, HERE[0], HERE[1], 584.0), @@ -714,7 +714,7 @@ def cmd(seq, command, lat, lng, alt, frame): return self.message('CMD', CNum=seq, CId=command, Lat=lat, Lng=lng, Alt=alt, Frame=frame, Prm1=0, Prm2=0, Prm3=0, Prm4=0) - (path, mission) = self.module().mission_from_log(self.log( + (path, mission, cmds, _, _, _, _) = self.module().mission_from_log(self.log( self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=0.1), self.message('MSG', Message='New mission'), cmd(0, m.MAV_CMD_NAV_WAYPOINT, 0.0, 0.0, 0.0, 0), @@ -724,6 +724,7 @@ def cmd(seq, command, lat, lng, alt, frame): self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), )) assert (mission[0][0], mission[0][1], mission[0][2]) == HERE + (584.0,) + assert cmds[0][:3] == HERE + (584.0,) ground0 = min(p[2] for p in path) out = self.module().resolve_mission_amsl(mission, ground0, {}) # measured from the home the vehicle had, not the lowest position @@ -735,7 +736,7 @@ def test_a_log_from_before_the_logger_said_new_mission(self): second = [m for m in self.dump( mp_util.gps_newpos(HERE[0], HERE[1], 0, 5000), 2) if m.get_type() == 'CMD'] - (path, mission) = self.module().mission_from_log( + (path, mission, _, _, _, _, _) = self.module().mission_from_log( self.log(*(first + second))) assert len(mission) == 2 diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index 3d9ad1315a..6e9e6ed95e 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -2,6 +2,8 @@ navigation and a flight flown through it''' import math +import os +import struct import pytest @@ -1945,6 +1947,1011 @@ def recording(home, items, params, heading=None, **kwargs): module.idle_task() assert headings == [None, 45] + def explorer(self): + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + def log_mission(self, with_home=True): + '''a log's CMD entries, as mission_from_log() keeps them''' + cmds = {} + for w in self.items(): + if w.seq == 0 and not with_home: + cmds[0] = (0.0, 0.0, 0.0, 0, w.command, 0, (0, 0, 0, 0)) + continue + cmds[w.seq] = (w.x, w.y, w.z, w.frame, w.command, w.seq, + (w.param1, w.param2, w.param3, w.param4)) + return cmds + + def test_mavexplorer_flies_a_planes_mission(self): + mx = self.explorer() + cmds = self.log_mission() + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert track is not None + assert track[0][:2] == pytest.approx(HOME[:2]) + # the speed change is in the CMDs but not among the items drawn + slow = dict(cmds) + slow[1] = slow[1][:6] + ((0, -1, -1, 0),) + slow = mx.plane_mission_track(slow, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert (max(local(p)[0] for p in track) > + max(local(p)[0] for p in slow) + 5.0) + assert mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_QUADROTOR) is None + + def test_mavexplorer_draws_no_path_for_a_mission_with_an_item_missing(self): + # jumps, and where each item began, go by sequence number, which a + # gap would put out by one for every item after it + mx = self.explorer() + cmds = self.log_mission() + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) is not None + del cmds[1] + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) is None + + def test_mavexplorer_keeps_the_altitude_of_an_item_with_no_position( + self, monkeypatch): + flown = [] + real = plane_track.mission_track + + def recording(home, items, *args, **kwargs): + flown.append(items) + return real(home, items, *args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + mx = self.explorer() + cmds = self.log_mission() + cmds[1] = (0.0, 0.0, 200.0, 3, mavlink.MAV_CMD_NAV_LOITER_TO_ALT, 1, + (0, 0, 0, 0)) + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert flown[-1][0][3] == pytest.approx(HOME[2] + 200.0) + + def test_mavexplorer_starts_where_the_flight_did_with_no_home(self): + mx = self.explorer() + cmds = self.log_mission(with_home=False) + started = offset(-500, -500, 0) + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds, started_at=started[:2]), + HOME[2], PARAMS, mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, started, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert track is not None + assert track[0][:2] == pytest.approx(started[:2]) + + def log(self, *messages): + '''a dataflash log handing back these messages in order''' + queue = list(messages) + + class Log(object): + def recv_match(self, type=None, condition=None): + while queue: + m = queue.pop(0) + if type is None or m.get_type() in type: + return m + return None + return Log() + + def message(self, kind, **fields): + from types import SimpleNamespace + m = SimpleNamespace(_timestamp=0, **fields) + m.get_type = lambda: kind + return m + + def cmd(self, seq, command, lat, lon, alt): + return self.message('CMD', CNum=seq, CId=command, Lat=lat, Lng=lon, + Alt=alt, Frame=3, Prm1=0, Prm2=0, Prm3=0, Prm4=0) + + def pos(self, north, east, alt=0.0): + (lat, lon, amsl) = offset(north, east, alt) + return self.message('POS', Lat=lat, Lng=lon, Alt=amsl) + + def mission_dump(self, new_mission=True, + takeoff=mavlink.MAV_CMD_NAV_TAKEOFF): + (lat, lon, _) = offset(3000, 0) + return (([self.message('MSG', Message='New mission')] if new_mission + else []) + + [self.cmd(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2]), + self.cmd(1, takeoff, 0, 0, 50), + self.cmd(2, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100)]) + + def test_mavexplorer_takes_the_takeoff_course_from_the_log(self): + mx = self.explorer() + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + # taxied north a little before the mission started + self.pos(10, 0), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(10, 10), self.pos(10, 25), self.pos(10, 45), + self.pos(10, 80)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + # east, from where the takeoff began, not from where the log did + assert mx.takeoff_course(path, cmds, started) == pytest.approx(90.0, abs=1.0) + assert mx.takeoff_course(path, cmds, {}) is None + + def mavlink(self, message, sysid=1, mission_type=None): + '''a MAVLink message from sysid, as a telemetry log holds it. The + mission protocol's mission_type is a MAVLink 2 extension, which is + set after packing where the dialect loaded is MAVLink 1''' + message.pack(mavlink.MAVLink(None, srcSystem=sysid, + srcComponent=1 if sysid < 200 else 190)) + message._timestamp = 0 + if mission_type is not None: + message.mission_type = mission_type + return message + + def mission_count(self, target, count, mission_type, sysid): + return self.mavlink(mavlink.MAVLink_mission_count_message( + target, 1, count), sysid, mission_type) + + def telemetry(self, vehicle=1): + '''a telemetry log of a vehicle, and a ground station which + uploads it a mission and watches it take off''' + gcs = 255 + out = [self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_GCS, mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0, 3), + gcs)] + out.append(self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, + 0, 0, 0, 3), vehicle)) + + def position(north, east, alt=0.0, sysid=vehicle): + (lat, lon, amsl) = offset(north, east, alt) + return self.mavlink(mavlink.MAVLink_global_position_int_message( + 0, int(lat * 1e7), int(lon * 1e7), int(amsl * 1000), + int(alt * 1000), 0, 0, 0, 0), sysid) + + def item(seq, command, lat, lon, alt, frame, params=(0, 0, 0, 0), + mission_type=0, sysid=gcs, target=vehicle): + return self.mavlink(mavlink.MAVLink_mission_item_int_message( + target, 1, seq, frame, command, 0, 1, *params, + int(lat * 1e7), int(lon * 1e7), alt), sysid, mission_type) + (lat, lon, _) = offset(3000, 0) + (rlat, rlon, _) = offset(-2000, 500) + out += [ + position(0, 0), + # another vehicle, whose position is nothing to do with this one + position(5000, 5000, sysid=vehicle + 1), + self.mavlink(mavlink.MAVLink_home_position_message( + int(HOME[0] * 1e7), int(HOME[1] * 1e7), int(HOME[2] * 1000), + 0, 0, 0, [1, 0, 0, 0], 0, 0, 0), vehicle), + self.mavlink(mavlink.MAVLink_gps_global_origin_message( + int(HOME[0] * 1e7), int(HOME[1] * 1e7), int(500 * 1000)), + vehicle), + # an old mission, then the one uploaded over it + self.mission_count(vehicle, 2, 0, gcs), + item(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2], 0), + item(1, mavlink.MAV_CMD_NAV_WAYPOINT, rlat, rlon, 80, 3), + self.mission_count(vehicle, 3, 0, gcs), + item(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2], 0), + item(1, mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 50, 3, (10, 0, 0, 0)), + item(2, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100, 3), + # and a rally point downloaded from the vehicle + self.mission_count(gcs, 1, 2, vehicle), + item(0, mavlink.MAV_CMD_NAV_RALLY_POINT, rlat, rlon, 120, 3, + mission_type=2, sysid=vehicle, target=gcs), + self.mavlink(mavlink.MAVLink_statustext_message( + 6, b'Mission: 1 Takeoff'), vehicle), + position(10, 10, 5), position(20, 20, 10), + ] + return out + + def test_mavexplorer_reads_a_telemetry_log(self): + mx = self.explorer() + (path, mission, cmds, started, rally, origin, + approach) = mx.mission_from_log(self.log(*self.telemetry())) + # the vehicle's own positions, not another's + assert len(path) == 3 + assert path[0][:3] == pytest.approx(offset(0, 0, 0)) + # the mission last uploaded, which replaced the one before + assert sorted(cmds) == [0, 1, 2] + assert cmds[1][4] == mavlink.MAV_CMD_NAV_TAKEOFF + assert cmds[1][6][0] == 10 + assert cmds[2][:2] == pytest.approx(offset(3000, 0)[:2]) + # the takeoff placed where it began + assert started == {1: 0} + assert (mission[1][0], mission[1][1]) == pytest.approx(offset(0, 0)[:2]) + assert origin == pytest.approx((HOME[0], HOME[1], 500.0)) + (lat, lon, _) = offset(-2000, 500) + assert len(rally) == 1 + assert rally[0][:3] == pytest.approx((lat, lon, 120.0)) + assert plane_track.rally_alt_frame(rally[0][3]) == \ + plane_track.RALLY_ALT_ABOVE_HOME + assert approach is None + # which is enough to fly: up at the takeoff's pitch, towards the + # waypoint after it + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track( + cmds, mission, path[0][:3], PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin, approach) + climbed = [local(p) for p in track if p[2] >= HOME[2] + 49.5][0] + assert climbed[0] == pytest.approx(50 / math.tan(math.radians(10)), + rel=0.1) + assert abs(climbed[1]) < 5.0 + + def test_mavexplorer_keeps_the_frame_a_rally_point_came_in(self): + """a rally point downloaded or uploaded carries the frame its + altitude is in, which AP_Rally keeps as flags of its own""" + mx = self.explorer() + frames = {mavlink.MAV_FRAME_GLOBAL: plane_track.RALLY_ALT_ABSOLUTE, + mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT: + plane_track.RALLY_ALT_ABOVE_HOME, + mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT: + plane_track.RALLY_ALT_ABOVE_TERRAIN} + (lat, lon, _) = offset(-2000, 500) + for (frame, expected) in frames.items(): + log = self.telemetry_mission( + self.mission_count(255, 1, 2, 1), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + 255, 1, 0, frame, mavlink.MAV_CMD_NAV_RALLY_POINT, 0, 1, + 0, 0, 0, 0, int(lat * 1e7), int(lon * 1e7), 120), 1, 2)) + (_, _, _, _, rally, _, _) = mx.mission_from_log(log) + assert len(rally) == 1 + assert plane_track.rally_alt_frame(rally[0][3]) == expected + + def rally_messages(self, count=2, sysid=1, target=255): + """a rally table as it travels: the points 2km south of home""" + out = [self.mission_count(target, count, 2, sysid)] + for seq in range(count): + (lat, lon, _) = offset(-2000, 500 * seq) + out.append(self.mavlink( + mavlink.MAVLink_mission_item_int_message( + target, 1, seq, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavlink.MAV_CMD_NAV_RALLY_POINT, 0, 1, 0, 0, 0, 0, + int(lat * 1e7), int(lon * 1e7), 120), sysid, 2)) + return out + + def test_mavexplorer_keeps_a_rally_table_a_download_stops_through(self): + """as with a mission, a rally download the log ends part way through + says nothing about the table the vehicle holds""" + mx = self.explorer() + table = self.rally_messages() + log = self.telemetry_mission(*(table + self.rally_messages()[:2])) + (_, _, _, _, rally, _, _) = mx.mission_from_log(log) + assert len(rally) == 2 + + def test_mavexplorer_drops_old_rally_points_a_download_says_are_gone( + self): + """a point the old RALLY_POINT protocol set, and then a download of + the table in the mission protocol with nothing in it: the vehicle + holds no rally points, though the mission protocol's table was + empty already""" + mx = self.explorer() + (lat, lon, _) = offset(-2000, 500) + log = self.telemetry_mission( + self.mavlink(mavlink.MAVLink_rally_point_message( + 255, 1, 0, 1, int(lat * 1e7), int(lon * 1e7), 120, 0, 0, 0), + 1), + self.mission_count(255, 0, mavlink.MAV_MISSION_TYPE_RALLY, 1)) + (_, _, _, _, rally, _, _) = mx.mission_from_log(log) + assert rally == [] + + def test_mavexplorer_does_not_fly_a_rally_table_cut_short(self): + """a download of the rally table which the log ends part way + through: what arrived is what is known of the table the vehicle + holds, and the points the old protocol set before it are gone, but + a return may go to one of those which never arrived, so there is no + flying the mission either""" + mx = self.explorer() + + def legacy(seq, count, north, east): + (lat, lon, _) = offset(north, east) + return self.mavlink(mavlink.MAVLink_rally_point_message( + 255, 1, seq, count, int(lat * 1e7), int(lon * 1e7), 120, + 0, 0, 0), 1) + table = self.rally_messages(count=2) + log = self.telemetry_mission(legacy(0, 2, -2000, 0), + legacy(1, 2, -2000, 500), *table[:2]) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + # the one point of the new table which arrived, not the old two + assert len(rally) == 1 + assert rally[0][:2] == pytest.approx(offset(-2000, 0)[:2]) + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is None + # as there is none for a table of the old protocol's which the log + # holds fewer points of than it says the table has + log = self.telemetry_mission(legacy(0, 2, -2000, 0)) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + assert len(rally) == 1 + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is None + # where the whole of the table is flown + log = self.telemetry_mission(legacy(0, 2, -2000, 0), *table) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + assert len(rally) == 2 + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is not None + + def test_mavexplorer_drops_rally_points_cleared_away(self): + """MISSION_CLEAR_ALL for the rally table, and for everything, which + takes the mission with it""" + mx = self.explorer() + for mission_type in (mavlink.MAV_MISSION_TYPE_RALLY, + mavlink.MAV_MISSION_TYPE_ALL): + log = self.telemetry_mission(*(self.rally_messages() + [ + self.mavlink(mavlink.MAVLink_mission_clear_all_message(1, 1), + 255, mission_type)])) + (_, mission, cmds, _, rally, _, _) = mx.mission_from_log(log) + assert rally == [] + everything = mission_type == mavlink.MAV_MISSION_TYPE_ALL + assert (cmds == {}) is everything + + def test_mavexplorer_takes_home_from_a_telemetry_log_as_home(self): + """HOME_POSITION is where the vehicle says home is, not the EKF + origin, which GPS_GLOBAL_ORIGIN says""" + mx = self.explorer() + vehicle = 1 + # a mission uploaded before the vehicle had a home, whose first item + # is the empty one it carries, which home is put into + log = self.telemetry_mission(self.mavlink( + mavlink.MAVLink_home_position_message( + int(HOME[0] * 1e7), int(HOME[1] * 1e7), int(400 * 1000), + 0, 0, 0, [1, 0, 0, 0], 0, 0, 0), vehicle), + self.mission_count(vehicle, 2, 0, 255), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 0, 0, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, + 0, 0, 0, 0, 0, 0, 0), 255, 0), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 1, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, + 0, 0, int(HOME[0] * 1e7), int(HOME[1] * 1e7), 100), 255, 0)) + (_, _, cmds, _, _, origin, _) = mx.mission_from_log(log) + assert cmds[0][:3] == pytest.approx((HOME[0], HOME[1], 400.0)) + # and nothing said where the origin was + assert origin is None + + def mission_messages(self, sysid=255, target=1): + """a three item mission as it travels: a MISSION_COUNT and its + items, uploaded to the vehicle, or downloaded from it the other way + about""" + (lat, lon, _) = offset(3000, 0) + items = [(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2], 0), + (1, mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 50, 3), + (2, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100, 3)] + out = [self.mission_count(target, len(items), 0, sysid)] + for (seq, command, ilat, ilon, alt, frame) in items: + out.append(self.mavlink( + mavlink.MAVLink_mission_item_int_message( + target, 1, seq, frame, command, 0, 1, 0, 0, 0, 0, + int(ilat * 1e7), int(ilon * 1e7), alt), sysid, 0)) + return out + + def telemetry_mission(self, *messages, upload=None): + """a telemetry log of a mission uploaded -- mission_messages()'s, + or upload -- taken off on 600m east of home, and then whatever + messages follow""" + if upload is None: + upload = self.mission_messages() + vehicle = 1 + + def position(north, east, alt=0.0): + (plat, plon, amsl) = offset(north, east, alt) + return self.mavlink(mavlink.MAVLink_global_position_int_message( + 0, int(plat * 1e7), int(plon * 1e7), int(amsl * 1000), + int(alt * 1000), 0, 0, 0, 0), vehicle) + return self.log(*( + [self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, + mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 3), vehicle), + position(0, 0)] + list(upload) + + [position(0, 600), + self.mavlink(mavlink.MAVLink_statustext_message( + 6, b'Mission: 1 Takeoff'), vehicle)] + list(messages))) + + def test_mavexplorer_keeps_a_mission_a_download_stops_part_way_through( + self): + """a telemetry log carries the missions downloaded from the vehicle + as well as those uploaded to it, and one the log ends part way + through says nothing about the mission the vehicle holds""" + mx = self.explorer() + download = self.mission_messages(sysid=1, target=255) + log = self.telemetry_mission(*download[:3]) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + # the whole mission, not the two items of the download over it + assert sorted(cmds) == [0, 1, 2] + assert cmds[2][:2] == pytest.approx(offset(3000, 0)[:2]) + # and where the flight had got to in it + assert started == {1: 1} + + def test_mavexplorer_draws_a_mission_cut_short_but_does_not_fly_it(self): + """the only transfer a log carries of its mission ends part way + through it: those items are where the vehicle was told to go, and + are drawn, but flying them would end the flight with a return to + launch which whatever was still to come may say nothing of""" + mx = self.explorer() + vehicle = 1 + (lat, lon, _) = offset(3000, 0) + messages = ( + [self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, + mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 3), vehicle), + self.mavlink(mavlink.MAVLink_global_position_int_message( + 0, int(HOME[0] * 1e7), int(HOME[1] * 1e7), + int(HOME[2] * 1000), 0, 0, 0, 0, 0), vehicle), + self.mission_count(vehicle, 3, 0, 255), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 0, 0, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, + 0, 0, 0, 0, int(HOME[0] * 1e7), int(HOME[1] * 1e7), + HOME[2]), 255, 0), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 1, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, + 0, 0, 0, 0, int(lat * 1e7), int(lon * 1e7), 100), 255, 0)]) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log( + self.log(*messages)) + # the two items which arrived, drawn where they were sent + assert sorted(cmds) == [0, 1] + assert len(mission) == 2 + assert (mission[1][0], mission[1][1]) == pytest.approx((lat, lon)) + # and no path flown for them + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started) is None + # where a download cut short after a whole mission arrived leaves + # that mission, which is flown + after = self.telemetry_mission(*self.downloaded_mission()[:2]) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(after) + assert sorted(cmds) == [0, 1, 2] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started) is not None + # where the whole of it is flown once the rest arrives + whole = self.log(*(messages + [self.mavlink( + mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 2, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, + 0, 0, int(lat * 1e7), int(lon * 1e7), 100), 255, 0)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(whole) + assert sorted(cmds) == [0, 1, 2] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started) is not None + + def test_mavexplorer_keeps_where_a_mission_downloaded_again_was_flown( + self): + """a ground station downloads a mission mid-flight to see what the + vehicle holds: the same mission, so the flight goes on""" + mx = self.explorer() + log = self.telemetry_mission(*self.mission_messages(sysid=1, + target=255)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert sorted(cmds) == [0, 1, 2] + assert started == {1: 1} + # so the takeoff is still drawn from where it was flown + assert (mission[1][0], mission[1][1]) == \ + pytest.approx(offset(0, 600)[:2]) + + def downloaded_mission(self, alt=None, lat=None, north=3000): + """the mission of telemetry_mission() as the vehicle sends it back, + with home or the waypoint moved""" + (wlat, wlon, _) = offset(north, 0) + items = [(0, mavlink.MAV_CMD_NAV_WAYPOINT, + HOME[0] if lat is None else lat, HOME[1], + HOME[2] if alt is None else alt, 0), + (1, mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 50, 3), + (2, mavlink.MAV_CMD_NAV_WAYPOINT, wlat, wlon, 100, 3)] + out = [self.mission_count(255, len(items), 0, 1)] + for (seq, command, ilat, ilon, ialt, frame) in items: + out.append(self.mavlink( + mavlink.MAVLink_mission_item_int_message( + 255, 1, seq, frame, command, 0, 1, 0, 0, 0, 0, + int(ilat * 1e7), int(ilon * 1e7), ialt), 1, 0)) + return out + + def test_mavexplorer_knows_a_mission_downloaded_again_by_its_items(self): + """home is the vehicle's own, not the mission's: an uploaded one is + never stored, and a downloaded one is wherever home is now, which + the vehicle keeps setting while it is disarmed. A download which + differs there alone is still the mission being flown""" + mx = self.explorer() + for kwargs in ({}, dict(alt=HOME[2] + 0.4), dict(lat=HOME[0] + 1.0e-7)): + log = self.telemetry_mission(*self.downloaded_mission(**kwargs)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert sorted(cmds) == [0, 1, 2] + assert started == {1: 1} + assert (mission[1][0], mission[1][1]) == \ + pytest.approx(offset(0, 600)[:2]) + # where a mission whose items are somewhere else is another mission + log = self.telemetry_mission(*self.downloaded_mission(north=3050)) + (_, _, _, started, _, _, _) = mx.mission_from_log(log) + assert started == {} + + def transferred(self, items, sysid, target, as_float=False): + """a mission as it travels, from its items as (command, frame, + (param1, param2, param3, param4), lat, lon, alt): as MISSION_ITEM_INT + with lat and lon given in 1e-7 degrees, or as MISSION_ITEM""" + out = [self.mission_count(target, len(items), 0, sysid)] + for (seq, (command, frame, params, lat, lon, alt)) in \ + enumerate(items): + if as_float: + message = mavlink.MAVLink_mission_item_message( + target, 1, seq, frame, command, 0, 1, *params, + lat, lon, alt) + else: + message = mavlink.MAVLink_mission_item_int_message( + target, 1, seq, frame, command, 0, 1, *params, + lat, lon, alt) + out.append(self.mavlink(message, sysid, 0)) + return out + + def uploaded_and_downloaded(self, upload, download, **kwargs): + """where the log says the flight had got to, with the mission + uploaded as upload and downloaded again as download: each a list of + items as transferred() takes them, after home and the takeoff""" + home = [(mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0), + int(HOME[0] * 1e7), int(HOME[1] * 1e7), HOME[2]), + (mavlink.MAV_CMD_NAV_TAKEOFF, 3, (0, 0, 0, 0), 0, 0, 50)] + up_kwargs = dict((k[3:], v) for (k, v) in kwargs.items() + if k.startswith('up_')) + down_kwargs = dict((k[5:], v) for (k, v) in kwargs.items() + if k.startswith('down_')) + up_home = home + if up_kwargs.get('as_float'): + up_home = [item[:3] + (item[3] * 1e-7, item[4] * 1e-7) + item[5:] + for item in home] + log = self.telemetry_mission( + *self.transferred(home + download, 1, 255, **down_kwargs), + upload=self.transferred(up_home + upload, 255, 1, **up_kwargs)) + (_, _, cmds, started, _, _, _) = self.explorer().mission_from_log(log) + assert len(cmds) == 2 + len(upload) + return started + + def test_mavexplorer_knows_a_mission_as_ardupilot_sends_it_back(self): + """AP_Mission keeps an item's position only for a command which has + one, and gives it back in frame 0, 3 or 10 alone; it keeps only + some of an item's parameters, and a plane's waypoint keeps neither + param1 nor param4. A download of the mission as the vehicle sends + it is still the mission being flown""" + (lat, lon, _) = offset(3000, 0) + (lat, lon) = (int(lat * 1e7), int(lon * 1e7)) + speed = mavlink.MAV_CMD_DO_CHANGE_SPEED + waypoint = mavlink.MAV_CMD_NAV_WAYPOINT + loiter = mavlink.MAV_CMD_NAV_LOITER_TURNS + for (upload, download) in ( + # a speed change, which has no position and so no frame + ([(speed, 3, (0, 18, -1, 0), 0, 0, 0)], + [(speed, 0, (0, 18, -1, 0), 0, 0, 0)]), + ([(speed, mavlink.MAV_FRAME_MISSION, (0, 18, -1, 0), + 0, 0, 0)], + [(speed, 0, (0, 18, -1, 0), 0, 0, 0)]), + # a location command uploaded in MISSION, the frame for an + # item with no position, which is kept as an absolute one + ([(mavlink.MAV_CMD_DO_LAND_START, + mavlink.MAV_FRAME_MISSION, (0, 0, 0, 0), lat, lon, 100)], + [(mavlink.MAV_CMD_DO_LAND_START, 0, (0, 0, 0, 0), + lat, lon, 100)]), + # a waypoint's _INT frame + ([(waypoint, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + (0, 0, 0, 0), lat, lon, 100)], + [(waypoint, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + (0, 0, 0, 0), lat, lon, 100)]), + # a plane's waypoint, uploaded with a delay and a yaw + ([(waypoint, 3, (5, 0, 0, 45), lat, lon, 100)], + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 100)]), + # an altitude, which is kept in centimetres + ([(waypoint, 3, (0, 0, 0, 0), lat, lon, 100.009)], + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 100)]), + # turns kept in 256ths, and a radius in tens of metres + ([(loiter, 3, (0.3, 0, 305, 0), lat, lon, 100)], + [(loiter, 3, (76 / 256.0, 0, 300, 0), lat, lon, 100)])): + assert self.uploaded_and_downloaded(upload, download) == {1: 1} + + def test_mavexplorer_knows_a_mission_changed_by_its_parameters(self): + """where the vehicle does keep a parameter, a mission uploaded again + with it changed is another mission""" + speed = mavlink.MAV_CMD_DO_CHANGE_SPEED + jump = mavlink.MAV_CMD_DO_JUMP + for (upload, download) in ( + ([(speed, 0, (0, 18, -1, 0), 0, 0, 0)], + [(speed, 0, (0, 20, -1, 0), 0, 0, 0)]), + ([(jump, 0, (1, 2, 0, 0), 0, 0, 0)], + [(jump, 0, (1, 3, 0, 0), 0, 0, 0)])): + assert self.uploaded_and_downloaded(upload, download) == {} + + def test_mavexplorer_knows_a_mission_changed_in_altitude_alone(self): + """a waypoint moved up or down, with its position left where it + was, is another mission""" + waypoint = mavlink.MAV_CMD_NAV_WAYPOINT + (lat, lon, _) = offset(3000, 0) + (lat, lon) = (int(lat * 1e7), int(lon * 1e7)) + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 100)], + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 105)]) == {} + + def test_mavexplorer_knows_a_mission_uploaded_as_floats_sent_back(self): + """AP_Mission turns a MISSION_ITEM's position into 1e-7 degrees in + float32, and gives a MISSION_ITEM back by turning those into float32 + and multiplying them by 1e-7 in float32: either way the position + comes back more than a tenth of a metre from where it went, and the + second as much as a metre and more""" + def float32(value): + return struct.unpack(' 5.0e-6 + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), flat, flon, 100)], + [(waypoint, 3, (0, 0, 0, 0), klat, klon, 100)], + up_as_float=True) == {1: 1} + # and uploaded as MISSION_ITEM_INT, downloaded as MISSION_ITEM + (ilat, ilon) = (int(lat * 1e7), int(lon * 1e7)) + (dlat, dlon) = (float32(float32(ilat) * float32(1.0e-7)), + float32(float32(ilon) * float32(1.0e-7))) + assert abs(dlon - ilon * 1e-7) > 1.0e-5 + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), ilat, ilon, 100)], + [(waypoint, 3, (0, 0, 0, 0), dlat, dlon, 100)], + down_as_float=True) == {1: 1} + # where a waypoint 5m away is somewhere else + (lat, lon, _) = offset(3005, 10) + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), ilat, ilon, 100)], + [(waypoint, 3, (0, 0, 0, 0), int(lat * 1e7), int(lon * 1e7), + 100)]) == {} + + def test_mavexplorer_reads_a_log_from_before_items_carried_params(self): + # CMD carried neither the frame nor the parameters of an item until + # 2014 + mx = self.explorer() + (lat, lon, _) = offset(2000, 0) + entries = [(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2]), + (1, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100)] + messages = [self.pos(0, 0)] + for (seq, command, clat, clon, alt) in entries: + messages.append(self.message('CMD', CNum=seq, CId=command, + Lat=clat, Lng=clon, Alt=alt)) + (_, _, cmds, _, _, _, _) = mx.mission_from_log(self.log(*messages)) + assert sorted(cmds) == [0, 1] + assert cmds[1][:3] == pytest.approx((lat, lon, 100)) + assert cmds[1][6] == (0.0, 0.0, 0.0, 0.0) + # and takes the altitude as being above home, as it was then + assert cmds[1][3] == mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + + def test_mavexplorer_drops_a_mission_cleared_away(self): + mx = self.explorer() + log = self.telemetry_mission(self.mavlink( + mavlink.MAVLink_mission_clear_all_message(1, 1), 255, 0)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert cmds == {} + assert mission == [] + assert started == {} + + def test_mavexplorer_reads_a_mission_uploaded_as_float_items(self): + # MISSION_ITEM, which a ground station may upload instead of + # MISSION_ITEM_INT + mx = self.explorer() + (lat, lon, _) = offset(2000, 0) + log = self.telemetry_mission( + self.mission_count(1, 2, 0, 255), + self.mavlink(mavlink.MAVLink_mission_item_message( + 1, 1, 0, 0, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + HOME[0], HOME[1], HOME[2]), 255, 0), + self.mavlink(mavlink.MAVLink_mission_item_message( + 1, 1, 1, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + lat, lon, 120), 255, 0)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert sorted(cmds) == [0, 1] + assert cmds[1][:3] == pytest.approx((lat, lon, 120)) + # a mission of its own, so where the one before it was flown is not + # where this one is + assert started == {} + + def test_mavexplorer_takes_the_course_a_vtol_takeoff_left_on(self): + # a QuadPlane climbs where it is, then transitions the way it + # points, which is nowhere in the mission either + mx = self.explorer() + log = self.log(*([self.pos(0, 0)] + self.mission_dump( + takeoff=mavlink.MAV_CMD_NAV_VTOL_TAKEOFF) + [ + self.message('MSG', Message='Mission: 1 VTOLTakeoff'), + self.pos(0, 0, 25), self.pos(0, 0, 50), + self.pos(-10, -10, 50), self.pos(-30, -30, 50)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + # south-west, away from the next waypoint, which is north + assert mx.takeoff_course(path, cmds, started) == pytest.approx(225.0, abs=1.0) + + def test_mavexplorer_gives_the_course_as_what_the_takeoff_flies_on( + self, monkeypatch): + """the course measured over a takeoff is the way a VTOL one points + as it transitions, and the course a fixed-wing one holds: each goes + to the drawing as what it is""" + mx = self.explorer() + given = {} + real = plane_track.mission_track + + def recording(*args, **kwargs): + # heading is the fourth of them, passed either way about + names = ('home', 'items', 'params', 'heading', 'start', 'rally', + 'approach', 'takeoff_course') + given.update(dict(zip(names, args))) + given.update(kwargs) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + for (command, params, field) in ( + (mavlink.MAV_CMD_NAV_TAKEOFF, PARAMS, 'takeoff_course'), + (mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, PARAMS, 'heading'), + (mavlink.MAV_CMD_NAV_TAKEOFF, dict(PARAMS, Q_ENABLE=1), + 'heading')): + log = self.log(*([self.pos(0, 0)] + + self.mission_dump(takeoff=command) + [ + self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(0, 20, 10), self.pos(0, 50, 20), self.pos(0, 90, 30)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + mission = mx.resolve_mission_amsl(mission, HOME[2], params, + mavlink.MAV_TYPE_FIXED_WING) + given.clear() + mx.plane_mission_track(cmds, mission, None, params, + mavlink.MAV_TYPE_FIXED_WING, path, started) + # east, as the log shows it flown + assert given[field] == pytest.approx(90.0, abs=2.0) + other = 'heading' if field == 'takeoff_course' else 'takeoff_course' + assert given[other] is None + + def test_mavexplorer_takes_the_takeoff_course_of_the_last_mission(self): + # a log from before the logger wrote "New mission": the first item + # of a mission written out again still starts it afresh, and where + # the items of the mission before began has to go with it + mx = self.explorer() + log = self.log(*( + [self.pos(0, 0)] + self.mission_dump(new_mission=False) + + [self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(0, 20), self.pos(0, 50)] + + self.mission_dump(new_mission=False) + + [self.pos(0, 60), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(40, 60), self.pos(80, 60)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + course = mx.takeoff_course(path, cmds, started) + # north, the second takeoff, not east, the first + assert mp_util.wrap_180(course) == pytest.approx(0.0, abs=1.0) + + def test_mavexplorer_returns_to_the_rally_points_the_log_ends_with(self): + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + (old_lat, old_lon, _) = offset(-2000, 0, 0) + + def raly(seq, total, lat, lon, alt, **fields): + return self.message('RALY', Tot=total, Seq=seq, Lat=lat, Lng=lon, + Alt=alt, **fields) + dump = self.mission_dump() + # home as the logger writes it, in absolute altitude + dump[1].Frame = 0 + dump[-1] = self.cmd(2, mavlink.MAV_CMD_NAV_WAYPOINT, + *offset(3000, 0)[:2], 100) + dump.append(self.cmd(3, mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0)) + log = self.log(*([self.pos(0, 0)] + dump + [ + raly(0, 2, old_lat, old_lon, 80, Flags=0), + raly(1, 2, old_lat, old_lon, 80, Flags=0), + # written out again, as one point + raly(0, 1, lat, lon, 150, Flags=0)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(lat, lon, 150, 0)] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING, + rally=rally) + (end_lat, end_lon, end_amsl) = track[-1] + assert mp_util.gps_distance(end_lat, end_lon, lat, lon) < 150 + assert end_amsl == pytest.approx(HOME[2] + 150, abs=15) + + def test_mavexplorer_keeps_rally_points_a_new_one_is_appended_to(self): + """AP_Rally::append() logs the new point alone, with the new total""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + (other_lat, other_lon, _) = offset(-2000, 0, 0) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('RALY', Tot=1, Seq=0, Lat=other_lat, Lng=other_lon, + Alt=80, Flags=0), + self.message('RALY', Tot=2, Seq=1, Lat=lat, Lng=lon, Alt=150, + Flags=0)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(other_lat, other_lon, 80, 0), (lat, lon, 150, 0)] + # while a smaller table drops the points it no longer has + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('RALY', Tot=2, Seq=0, Lat=other_lat, Lng=other_lon, + Alt=80, Flags=0), + self.message('RALY', Tot=2, Seq=1, Lat=lat, Lng=lon, Alt=150, + Flags=0), + self.message('RALY', Tot=1, Seq=0, Lat=lat, Lng=lon, Alt=90, + Flags=0)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(lat, lon, 90, 0)] + + def test_mavexplorer_waits_only_so_long_for_rally_terrain( + self, monkeypatch): + pytest.importorskip("vtk") + mx = self.explorer() + from MAVProxy.modules.mavproxy_map3d import terrain + asked = [] + + def sample(lat, lon, **kwargs): + asked.append(kwargs) + return 300.0 + monkeypatch.setattr(terrain, 'sample_terrain', sample) + frame_valid = 1 << 2 + amsl = mx.rally_point_amsl(HOME[0], HOME[1], 100, + frame_valid | (3 << 3), HOME[2]) + assert amsl == 400.0 + assert asked == [{'timeout': mx.RALLY_TERRAIN_TIMEOUT}] + assert 0 < mx.RALLY_TERRAIN_TIMEOUT <= 10.0 + # and an origin the log does not give falls back to home + assert mx.rally_point_amsl(HOME[0], HOME[1], 100, + frame_valid | (2 << 3), + HOME[2]) == HOME[2] + 100 + + def test_mavexplorer_forgets_rally_points_which_were_cleared(self): + """the logger writes "New rally" before the whole table, even an + empty one, which is all a cleared table is logged as""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('RALY', Tot=1, Seq=0, Lat=lat, Lng=lon, Alt=150, + Flags=0), + self.message('MSG', Message='New rally')])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [] + + def test_mavexplorer_measures_a_rally_point_from_the_ekf_origin(self): + """ORGN type 0 is the EKF origin, type 1 home""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + frame_valid = 1 << 2 + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('ORGN', Type=0, Lat=HOME[0], Lng=HOME[1], + Alt=HOME[2] - 50), + self.message('ORGN', Type=1, Lat=HOME[0], Lng=HOME[1], + Alt=HOME[2]), + self.message('RALY', Tot=1, Seq=0, Lat=lat, Lng=lon, Alt=120, + Flags=frame_valid | (2 << 3))])) + (path, mission, cmds, started, rally, origin, _) = mx.mission_from_log(log) + assert origin == (HOME[0], HOME[1], HOME[2] - 50) + assert mx.rally_point_amsl(lat, lon, 120, frame_valid | (2 << 3), + HOME[2], origin) == HOME[2] + 70 + + def test_mavexplorer_takes_rally_points_from_an_older_log(self): + """RALY had no Flags before 4.5, and one point set is logged alone""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + (other_lat, other_lon, _) = offset(-2000, 0, 0) + + def raly(seq, total, lat, lon, alt, **fields): + return self.message('RALY', Tot=total, Seq=seq, Lat=lat, Lng=lon, + Alt=alt, **fields) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + raly(0, 2, other_lat, other_lon, 80), + raly(1, 2, lat, lon, 150), + # each point set again on its own, which is logged alone: the + # other points still stand, rather than being forgotten + raly(1, 2, lat, lon, 160), + raly(0, 2, other_lat, other_lon, 90)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(other_lat, other_lon, 90, 0), (lat, lon, 160, 0)] + + @staticmethod + def bearing_to(a, b): + return mp_util.gps_bearing(a[0], a[1], b[0], b[1]) + + def takeoff_bearing(self, track): + '''the course from where the track starts to where it has climbed + to the takeoff altitude''' + climbed = [p for p in track if p[2] >= HOME[2] + 49.5][0] + return self.bearing_to(track[0], climbed) + + def test_mavexplorer_starts_where_the_log_says_the_mission_did(self): + mx = self.explorer() + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + # carried off to the east before the mission started + self.pos(0, 600, 5), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(40, 600, 30), self.pos(80, 600, 60)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING, + path, started) + assert track[0] == pytest.approx(offset(0, 600, 5)) + # and takes off from there on the course the log shows it flown, + # north, rather than towards the waypoint away to the north-west + assert mp_util.wrap_180(self.takeoff_bearing(track)) == \ + pytest.approx(0.0, abs=2.0) + assert self.bearing_to(track[0], offset(3000, 0)) < 350.0 + # without the log to go on, it starts from home + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert track[0][:2] == pytest.approx(HOME[:2]) + + def test_mavexplorer_draws_the_path_from_where_the_log_started(self, monkeypatch): + # the 3D map command itself, with a stand-in for the viewer + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + mx = self.explorer() + drawn = [] + + class Viewer(object): + def __init__(self, title=None): + pass + + def set_mission(self, items, track=None): + drawn.append(track) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.pos(0, 600, 5), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(40, 600, 30), self.pos(80, 600, 60)])) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mavlink.MAV_TYPE_FIXED_WING + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=SimpleNamespace( + condition=None, showdirection=True, showlabels=False, + labelsize=14, sync_xmap=False, + missionpath='flown')), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + (track,) = drawn + assert track[0] == pytest.approx(offset(0, 600, 5)) + # on the course the log shows the takeoff flown on + assert mp_util.wrap_180(self.takeoff_bearing(track)) == \ + pytest.approx(0.0, abs=2.0) + + def test_the_3d_map_draws_the_track_it_is_given(self): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + items = [MissionItem(p[0], p[1], p[2], 0, mavlink.MAV_CMD_NAV_WAYPOINT, i) + for (i, p) in enumerate((offset(0, 0), offset(3000, 0)))] + track = [offset(0, 0), offset(1000, 50), offset(2000, 20), offset(3000, 0)] + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + em.set_mission(items, track) + assert em.mission_line == [em._enu(*p) for p in track] + assert em.mission_markers == [em._enu(i.lat, i.lon, i.alt) for i in items] + # and without one, the items are drawn as they always were + em.set_mission(items) + assert em.mission_line == [em._enu(i.lat, i.lon, i.alt) for i in items] + class TestMissionStyles(object): """the 3D map draws a mission as the path flown, as the geometry of its @@ -2103,6 +3110,71 @@ def test_the_live_map_setting(self): module.cmd_map3d(['set', 'missionpath', 'fancy']) assert module.map3d_settings.missionpath == 'flown' + def test_mavexplorer_draws_the_style_asked_for(self, monkeypatch): + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + drawn = TestDrawnTrack() + mx = drawn.explorer() + views = [] + + class Viewer(object): + def __init__(self, title=None): + self.styles = [] + self.missions = [] + views.append(self) + + def is_alive(self): + return True + + def set_mission_style(self, style): + self.styles.append(style) + + def set_mission(self, items, track=None): + self.missions.append(track) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + flights = [] + real = mx.plane_mission_track + + def counting(*args, **kwargs): + flights.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(mx, 'plane_mission_track', counting) + log = drawn.log(*([drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.message('MSG', Message='Mission: 1 Takeoff'), + drawn.pos(40, 0, 30), drawn.pos(80, 0, 60)])) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mavlink.MAV_TYPE_FIXED_WING + + class Settings(SimpleNamespace): + def command(self, args): + setattr(self, args[0], args[1]) + settings = Settings(condition=None, showdirection=True, + showlabels=False, labelsize=14, + sync_xmap=False, missionpath='geometry') + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=settings), raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + (view,) = views + assert view.styles == ['geometry'] + assert view.missions == [None] + # the path flown was not worked out for a map not drawing it + assert flights == [] + # asked for later, it is worked out and drawn in the view open + mx.cmd_set(['missionpath', 'flown']) + assert view.styles == ['geometry', 'flown'] + assert view.missions[-1] is not None + assert len(flights) == 1 + # and only the once + mx.cmd_set(['missionpath', 'plain']) + mx.cmd_set(['missionpath', 'flown']) + assert len(flights) == 1 + def test_the_control_on_the_map_draws_the_style_it_picks(self, map3d_frame): em = self.elements() @@ -2134,6 +3206,32 @@ def test_the_live_map_takes_the_style_picked_on_it(self): # and the mission goes again, with the path flown worked out for it assert module.sent[-1] is not None + def test_mavexplorer_takes_the_style_picked_on_a_view(self, monkeypatch): + from types import SimpleNamespace + mx = TestDrawnTrack().explorer() + styles = [] + drawn = [] + view = SimpleNamespace( + is_alive=lambda: True, + check_events=lambda: [('mission_style', 'flown')], + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_mission_style=styles.append, + set_mission=lambda items, track=None: drawn.append(track), + mission_to_fly=(['an item'], lambda: ['a path flown'])) + settings = SimpleNamespace(showdirection=True, showlabels=False, + labelsize=14, missionpath='geometry') + monkeypatch.setattr(mx, 'mestate', SimpleNamespace(settings=settings), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', [view]) + mx.poll_map3d_views() + assert settings.missionpath == 'flown' + assert styles == ['flown'] + # the path flown is worked out only now that it is drawn, and once + assert drawn == [['a path flown']] + assert view.mission_to_fly is None + def unflyable(self, module): '''give the live module's mission a NAV_DELAY, which cannot be flown through''' @@ -2199,6 +3297,69 @@ def test_the_live_map_says_when_asked_for_a_path_it_cannot_draw( module.idle_task() assert 'only a plane' in capsys.readouterr().out + def test_mavexplorer_says_when_a_mission_has_no_path_flown( + self, monkeypatch, capsys): + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + drawn = TestDrawnTrack() + mx = drawn.explorer() + views = [] + + class Viewer(object): + def __init__(self, title=None): + self.missions = [] + views.append(self) + + def is_alive(self): + return True + + def set_mission(self, items, track=None): + self.missions.append(track) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + flights = [] + monkeypatch.setattr(mx, 'plane_mission_track', + lambda *args, **kwargs: flights.append(args)) + + class Settings(SimpleNamespace): + def command(self, args): + setattr(self, args[0], args[1]) + + def open_view(mav_type, style): + log = drawn.log(*([drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.message('MSG', Message='Mission: 1 Takeoff'), + drawn.pos(40, 0, 30), drawn.pos(80, 0, 60)])) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mav_type + settings = Settings(condition=None, showdirection=True, + showlabels=False, labelsize=14, + sync_xmap=False, missionpath=style) + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=settings), raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + return views[-1] + + # a plane's mission which cannot be flown through + view = open_view(mavlink.MAV_TYPE_FIXED_WING, 'flown') + assert view.missions == [None] + assert 'could not work out the path' in capsys.readouterr().out + # and which is not flown again for nothing + assert len(flights) == 1 + mx.cmd_set(['showdirection', 'true']) + assert len(flights) == 1 + # a copter's, left as it starts, says nothing + open_view(mavlink.MAV_TYPE_QUADROTOR, 'flown') + assert capsys.readouterr().out == '' + # but asked for, it does + open_view(mavlink.MAV_TYPE_QUADROTOR, 'geometry') + mx.cmd_set(['missionpath', 'flown']) + assert 'could not work out the path' in capsys.readouterr().out + class TestApproachCourse(object): """where the course of a VTOL landing approach comes from: the wind the @@ -2288,3 +3449,72 @@ def recording(*args, **kwargs): self.wind(module, 200.0) module.send_mission() assert flights == [None] + + def test_mavexplorer_takes_the_course_the_log_says(self, monkeypatch): + from MAVProxy.modules.mavproxy_map3d import map3d + drawn = TestDrawnTrack() + mx = drawn.explorer() + messages = [drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.pos(40, 0, 30), + drawn.message('MSG', Message='Selected an approach path of 45.5'), + drawn.message('MSG', Message='Selected an approach path of -120.0'), + drawn.pos(80, 0, 60)] + result = mx.mission_from_log(drawn.log(*messages)) + assert result[-1] == -120.0 + # none said, none known + messages = [drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.pos(40, 0, 30)] + assert mx.mission_from_log(drawn.log(*messages))[-1] is None + # and it is what the path is flown with + from types import SimpleNamespace + flown_with = [] + + class Viewer(object): + def __init__(self, title=None): + pass + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + monkeypatch.setattr( + mx, 'plane_mission_track', + lambda *args, **kwargs: flown_with.append(args[-1])) + messages = [drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.message('MSG', Message='Selected an approach path of 30.0'), + drawn.pos(40, 0, 30), drawn.pos(80, 0, 60)] + log = drawn.log(*messages) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mavlink.MAV_TYPE_FIXED_WING + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=SimpleNamespace( + condition=None, showdirection=True, showlabels=False, + labelsize=14, sync_xmap=False, missionpath='flown')), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + assert flown_with == [30.0] + + def test_mavexplorer_flies_the_course_it_is_given(self): + # the log's mission, landing on an approach, flown both ways + mx = TestDrawnTrack().explorer() + land = offset(0, 0, 60) + far = offset(0, 2000, 150) + cmds = { + 0: (HOME[0], HOME[1], HOME[2], 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)), + 1: (far[0], far[1], far[2], 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 1, (0, 0, 0, 0)), + 2: (land[0], land[1], land[2], 0, + mavlink.MAV_CMD_NAV_VTOL_LAND, 2, (1, 0, 0, 0))} + params = dict(PARAMS, Q_ENABLE=1, Q_FW_LND_APR_RAD=250) + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], params, + mavlink.MAV_TYPE_FIXED_WING) + for approach in (0.0, 90.0): + track = mx.plane_mission_track( + cmds, mission, None, params, mavlink.MAV_TYPE_FIXED_WING, + approach=approach) + assert mp_util.wrap_180(self.final_course(track) - approach) == \ + pytest.approx(0, abs=45) From feef7c62ad652ee2a5105308ae5df1091dd02071 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 3 Oct 2026 12:42:19 +1000 Subject: [PATCH 17/22] wp: say how many items to expect when a mission is loaded wp ftp, wp ftpload and wp show fill the loader without saying how many items to expect, so what they load looks like part of a download which stopped: the wp module asks the vehicle for items it already has, and anything else watching the loader waits for the rest of a mission which is all there. Say the count, as loading from a file already does. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/modules/lib/mission_item_protocol.py | 5 ++ tests/test_plane_track.py | 50 +++++++++++++++++++ 2 files changed, 55 insertions(+) diff --git a/MAVProxy/modules/lib/mission_item_protocol.py b/MAVProxy/modules/lib/mission_item_protocol.py index 9aa2f22dde..4bd3c97f8f 100644 --- a/MAVProxy/modules/lib/mission_item_protocol.py +++ b/MAVProxy/modules/lib/mission_item_protocol.py @@ -884,6 +884,7 @@ def cmd_show(self, args): print("usage: wp show ") return self.wploader.load(args[0]) + self.wploader.expected_count = self.wploader.count() def cmd_update(self, args): if not self.check_have_list(): @@ -1099,6 +1100,9 @@ def ftp_callback(self, fh, callback=None): w = mavmsg(*t) w = self.wp_from_mission_item_int(w) self.wploader.add(w) + # the whole of it arrived, however many a MAVLink download before it + # was expecting + self.wploader.expected_count = self.wploader.count() self.show_and_save(self.target_system) if callback is not None: callback(self.wploader) @@ -1144,6 +1148,7 @@ def wp_ftp_upload(self, args, callback=None): callback(None) return print("Loaded %u %s from %s" % (self.wploader.count(), self.itemstype(), filename)) + self.wploader.expected_count = self.wploader.count() self.ftp_upload(self.wploader, callback=callback) def ftp_upload(self, loader, callback=None): diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index 6e9e6ed95e..7b17f060d1 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -1691,6 +1691,56 @@ def test_the_idle_task_draws_the_path_flown(self): module.idle_task() assert module.sent[-1] is not None + def wp_module(self): + """the wp module, with only what loading a mission into it reads""" + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_wp + from pymavlink import mavwp + module = mavproxy_wp.WPModule.__new__(mavproxy_wp.WPModule) + module.wploader_by_sysid = {1: mavwp.MAVWPLoader()} + module.mpstate = SimpleNamespace( + settings=SimpleNamespace(target_system=1, target_component=1), + status=SimpleNamespace(logdir=None)) + return module + + def test_the_wp_module_expects_the_mission_it_is_given(self): + """whatever fills the loader says how many items to expect, so what + it holds is not taken for part of a download which stopped: that + would have the 3D map wait for the rest of it for ever, and the wp + module ask the vehicle for items it has""" + import struct + module = self.wp_module() + loader = module.wploader + # a download of four items, which three of arrive + loader.expected_count = 4 + for seq in range(3): + (lat, lon, _) = offset(seq * 100, 0) + loader.add(mavlink.MAVLink_mission_item_message( + 1, 1, seq, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + lat, lon, 100)) + assert loader.count() < loader.expected_count + # and then the whole of one fetched over ftp, which is shorter + items = b'' + for seq in range(2): + (lat, lon, _) = offset(seq * 100, 0) + item = mavlink.MAVLink_mission_item_int_message( + 1, 1, seq, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + int(lat * 1e7), int(lon * 1e7), 100) + item.mission_type = 0 + ordered = [None] * len(item.fieldnames) + for (i, order) in enumerate(item.orders): + ordered[order] = getattr(item, item.fieldnames[i]) + items += item.unpacker.pack(*ordered) + # the items are taken as they come: turning one into a waypoint of + # its own reads a field only a MAVLink2 dialect carries, which these + # tests do not load + module.wp_from_mission_item_int = lambda item: item + from io import BytesIO + module.ftp_callback(BytesIO(struct.pack( + " Date: Sat, 3 Oct 2026 12:43:46 +1000 Subject: [PATCH 18/22] tests: check the drawing against SITL flights of every mission item Ten SITL flights of ArduPilot's autotest missions are recorded here, and the path drawn for each mission is held to the one flown: across the ground, and in height. The flights are of the missions ArduPilot PR 34424 adds -- QuadPlane.KalaupapaCanyonRun, Plane.MissionItemTypes and QuadPlane.VTOLMissionItemTypes -- which between them fly every kind of item ArduPlane flies, every way a mission ends, every Q_RTL_MODE and each way a QuadPlane lands. Each flight is checked three ways: that every item is reached in the order the mission takes them, with about as far flown between each pair as the aircraft flew; that each point flown is near the path drawn, within limits of its own; and that it is at the height drawn, measured to the nearest point of the path so that a spiral is measured against the turn of it the aircraft was on. Half of each flight is drawn within 0.2 to 9m of where it went, and nine tenths within 4 to 28m. A recording says which firmware flew it, since the drawing follows what a version of ArduPlane does, and record_flight.py makes another from a log: what MAVExplorer hands the drawing, and the path flown once a second from the end of a QuadPlane's transition or a plane's takeoff to the start of its landing. Co-Authored-By: Claude Opus 5 (1M context) --- tests/missions/kalaupapa-canyon-run.json | 1 + tests/missions/record_flight.py | 222 ++++++++++++ .../missions/sitl-plane-loiter-unlimited.json | 1 + tests/missions/sitl-plane-mission-items.json | 1 + tests/missions/sitl-plane-rtl-rally.json | 1 + .../sitl-quadplane-rtl-q-rtl-mode-1.json | 1 + .../sitl-quadplane-rtl-q-rtl-mode-2.json | 1 + .../sitl-quadplane-rtl-q-rtl-mode-3.json | 1 + .../sitl-quadplane-vtol-land-approach.json | 1 + .../sitl-quadplane-vtol-land-q-options.json | 1 + tests/missions/sitl-quadplane-vtol-land.json | 1 + tests/test_plane_track.py | 334 ++++++++++++++++++ 12 files changed, 566 insertions(+) create mode 100644 tests/missions/kalaupapa-canyon-run.json create mode 100644 tests/missions/record_flight.py create mode 100644 tests/missions/sitl-plane-loiter-unlimited.json create mode 100644 tests/missions/sitl-plane-mission-items.json create mode 100644 tests/missions/sitl-plane-rtl-rally.json create mode 100644 tests/missions/sitl-quadplane-rtl-q-rtl-mode-1.json create mode 100644 tests/missions/sitl-quadplane-rtl-q-rtl-mode-2.json create mode 100644 tests/missions/sitl-quadplane-rtl-q-rtl-mode-3.json create mode 100644 tests/missions/sitl-quadplane-vtol-land-approach.json create mode 100644 tests/missions/sitl-quadplane-vtol-land-q-options.json create mode 100644 tests/missions/sitl-quadplane-vtol-land.json diff --git a/tests/missions/kalaupapa-canyon-run.json b/tests/missions/kalaupapa-canyon-run.json new file mode 100644 index 0000000000..1c7354b0c1 --- /dev/null +++ b/tests/missions/kalaupapa-canyon-run.json @@ -0,0 +1 @@ +{"home":[21.165642,-156.8868012,165.31],"items":[[84,21.1656423,-156.8868016,215.31,[0.0,0.0,0.0,0.0]],[31,21.1773205,-156.8829482,600.0,[0.0,200.0,0.0,0.0]],[31,21.1719305,-156.8800583,180.0,[0.0,-150.0,0.0,0.0]],[178,0.0,0.0,0.0,[0.0,22.0,-1.0,0.0]],[16,21.1602524,-156.8810218,200.0,[0.0,0.0,0.0,0.0]],[16,21.1575574,-156.8810219,200.0,[0.0,0.0,0.0,0.0]],[16,21.1548624,-156.8790954,200.0,[0.0,0.0,0.0,0.0]],[16,21.1521675,-156.8776506,220.0,[0.0,0.0,0.0,0.0]],[16,21.1494725,-156.8757242,230.0,[0.0,0.0,0.0,0.0]],[16,21.1467776,-156.8757243,250.0,[0.0,0.0,0.0,0.0]],[16,21.1440826,-156.8776508,280.0,[0.0,0.0,0.0,0.0]],[16,21.1413876,-156.8762061,300.0,[0.0,0.0,0.0,0.0]],[16,21.135,-156.8768,420.0,[0.0,0.0,0.0,0.0]],[16,21.1285,-156.8735,555.0,[0.0,0.0,0.0,0.0]],[16,21.124,-156.8715,645.0,[0.0,0.0,0.0,0.0]],[31,21.118,-156.871,1120.0,[0.0,150.0,0.0,0.0]],[178,0.0,0.0,0.0,[0.0,25.0,-1.0,0.0]],[16,21.1175,-156.86,1120.0,[0.0,0.0,0.0,0.0]],[16,21.119,-156.847,900.0,[0.0,0.0,0.0,0.0]],[16,21.12,-156.835,730.0,[0.0,0.0,0.0,0.0]],[16,21.1205,-156.8268,570.0,[0.0,0.0,0.0,0.0]],[16,21.128,-156.8272,450.0,[0.0,0.0,0.0,0.0]],[16,21.1345,-156.8305,380.0,[0.0,0.0,0.0,0.0]],[16,21.1405,-156.8351999,330.0,[0.0,0.0,0.0,0.0]],[16,21.148125,-156.8357494,300.0,[0.0,0.0,0.0,0.0]],[16,21.153515,-156.8357485,250.0,[0.0,0.0,0.0,0.0]],[16,21.1589049,-156.8333394,220.0,[0.0,0.0,0.0,0.0]],[16,21.1692356,-156.8338192,200.0,[0.0,0.0,0.0,0.0]],[16,21.1737272,-156.8579016,180.0,[0.0,0.0,0.0,0.0]],[16,21.1728288,-156.8868016,235.31,[0.0,0.0,0.0,0.0]],[85,21.1656423,-156.8868016,165.31,[0.0,0.0,0.0,0.0]]],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[21.1669346,-156.886454,230.3],[21.1671238,-156.8863907,232.7],[21.1673443,-156.8863173,233.6],[21.16758,-156.886239,236.2],[21.1678016,-156.8861652,240.1],[21.1680504,-156.8860826,244.8],[21.1682773,-156.886007,249.0],[21.1685231,-156.8859249,253.3],[21.1687676,-156.8858434,257.2],[21.1689883,-156.8857698,260.8],[21.1692284,-156.8856894,265.1],[21.1694665,-156.8856098,269.5],[21.1697027,-156.8855306,274.1],[21.1699376,-156.885452,278.9],[21.1701718,-156.8853736,283.8],[21.1704051,-156.8852956,288.9],[21.1706374,-156.8852177,294.0],[21.1708703,-156.8851392,299.3],[21.1711038,-156.885061,304.6],[21.171317,-156.8849899,309.4],[21.1715525,-156.8849111,314.5],[21.1717879,-156.8848321,319.6],[21.1720018,-156.8847604,324.3],[21.1722365,-156.884682,329.5],[21.1724496,-156.8846102,334.3],[21.1726832,-156.8845312,339.6],[21.1729168,-156.8844525,344.9],[21.1731298,-156.8843809,349.6],[21.1733639,-156.8843023,354.9],[21.1735985,-156.8842234,360.2],[21.1738329,-156.8841443,365.4],[21.1740678,-156.8840654,370.6],[21.1742815,-156.8839936,375.3],[21.174515,-156.8839147,380.6],[21.1747485,-156.8838349,385.9],[21.174962,-156.8837615,390.7],[21.175199,-156.8836889,396.0],[21.1754222,-156.8836617,401.0],[21.1756631,-156.8837185,406.7],[21.1758627,-156.8838777,412.4],[21.1760041,-156.8840675,416.1],[21.1761423,-156.8842905,418.6],[21.1762883,-156.8845073,422.1],[21.1764632,-156.8846992,426.7],[21.1766693,-156.8848506,431.7],[21.1768775,-156.8849458,436.3],[21.177119,-156.8850018,441.3],[21.1773657,-156.8850106,446.2],[21.1775877,-156.8849832,450.6],[21.1778261,-156.8849195,455.7],[21.1780349,-156.8848354,460.5],[21.178253,-156.8847172,465.9],[21.1784577,-156.8845724,471.3],[21.1786294,-156.8844183,476.2],[21.1787989,-156.8842255,481.4],[21.1789436,-156.88401,486.8],[21.1790598,-156.8837759,492.1],[21.1791457,-156.883528,497.5],[21.1791968,-156.8832942,502.3],[21.1792228,-156.88303,507.7],[21.1792173,-156.8827646,513.0],[21.1791848,-156.8825255,517.7],[21.1791186,-156.8822694,523.0],[21.1790317,-156.882046,527.8],[21.1789087,-156.881815,533.1],[21.178759,-156.8816029,538.4],[21.1786017,-156.8814299,543.3],[21.1784084,-156.8812642,548.7],[21.1782165,-156.8811378,553.6],[21.1779921,-156.8810288,558.9],[21.1777559,-156.8809509,564.2],[21.1775333,-156.8809091,569.0],[21.1772854,-156.880895,574.4],[21.177038,-156.8809153,579.8],[21.1768167,-156.8809642,584.6],[21.1765812,-156.88105,589.9],[21.1763748,-156.8811537,593.7],[21.1761589,-156.8812955,596.4],[21.1759604,-156.88147,598.3],[21.1758003,-156.8816558,599.5],[21.1756518,-156.8818853,600.6],[21.1755355,-156.882134,601.2],[21.1754531,-156.8823955,601.5],[21.1754053,-156.8826642,601.7],[21.1753904,-156.8829121,601.6],[21.175405,-156.8831844,601.5],[21.1754522,-156.8834529,601.3],[21.1755228,-156.8836897,601.2],[21.1756298,-156.8839373,601.1],[21.1757527,-156.8841476,601.0],[21.175913,-156.8843574,600.8],[21.1760975,-156.8845432,600.7],[21.1762843,-156.884689,600.6],[21.1765064,-156.8848201,600.5],[21.1767434,-156.8849194,600.4],[21.17699,-156.8849846,600.4],[21.1772431,-156.885014,600.3],[21.1774741,-156.8850096,600.3],[21.1777243,-156.8849702,600.3],[21.1779675,-156.8848962,600.2],[21.1781804,-156.8847991,600.1],[21.1784001,-156.8846615,600.1],[21.1785839,-156.8845101,600.1],[21.1787638,-156.8843185,600.1],[21.1789183,-156.8841037,600.0],[21.1790351,-156.8838905,600.1],[21.1791361,-156.8836402,600.1],[21.1792057,-156.8833781,600.0],[21.1792425,-156.8831076,600.0],[21.1792457,-156.8828344,600.1],[21.1792193,-156.8825885,600.1],[21.1791589,-156.8823254,600.1],[21.1790665,-156.8820724,600.0],[21.1789563,-156.8818549,600.0],[21.1788083,-156.8816337,600.0],[21.1786512,-156.8814521,600.0],[21.1784557,-156.8812768,600.0],[21.1782409,-156.8811311,600.0],[21.1780317,-156.8810262,600.1],[21.1777911,-156.880943,600.0],[21.1775401,-156.8809093,599.3],[21.1772859,-156.8809355,597.1],[21.1770361,-156.8809965,593.1],[21.1768131,-156.8810689,589.0],[21.1765693,-156.8811569,584.4],[21.1763305,-156.8812454,579.6],[21.1761124,-156.8813262,575.0],[21.1758742,-156.8814129,569.8],[21.1756503,-156.8814569,565.2],[21.1753992,-156.8814365,560.6],[21.1751574,-156.881373,555.3],[21.1749434,-156.8812961,550.1],[21.1746891,-156.8811922,544.0],[21.1744768,-156.8811022,539.2],[21.1742447,-156.8810036,534.0],[21.1740117,-156.8809057,528.8],[21.1738016,-156.8808186,524.0],[21.1735664,-156.8807342,518.8],[21.1733203,-156.8807001,513.9],[21.1730962,-156.8807482,509.8],[21.1728839,-156.8808908,505.4],[21.1727223,-156.8810601,500.3],[21.172555,-156.8812508,494.1],[21.1723671,-156.8814245,488.7],[21.1721703,-156.8815445,484.5],[21.1719299,-156.8816192,480.0],[21.1717015,-156.8816292,475.8],[21.171456,-156.881578,470.9],[21.1712295,-156.8814703,465.7],[21.1710434,-156.8813313,460.8],[21.1708676,-156.881145,455.5],[21.1707209,-156.8809291,450.2],[21.1706165,-156.8807146,445.4],[21.1705352,-156.8804612,440.1],[21.1704955,-156.8802234,435.3],[21.1704915,-156.8799566,430.1],[21.1705293,-156.8796957,425.0],[21.1705992,-156.8794663,420.3],[21.1707115,-156.8792337,415.1],[21.1708612,-156.8790218,409.8],[21.1710428,-156.8788399,404.6],[21.1712509,-156.8786944,399.3],[21.1714576,-156.8785976,394.5],[21.1716984,-156.8785331,389.2],[21.1719431,-156.8785152,384.0],[21.1721657,-156.8785385,379.4],[21.1724048,-156.8786057,374.2],[21.1726106,-156.878703,369.5],[21.1728184,-156.8788488,364.3],[21.1729994,-156.8790307,359.1],[21.1731359,-156.8792227,354.3],[21.1732509,-156.8794571,349.0],[21.1733262,-156.8797085,343.6],[21.1733601,-156.8799708,338.2],[21.1733518,-156.8802348,332.9],[21.1733075,-156.8804707,328.2],[21.1732207,-156.8807175,323.0],[21.1730959,-156.8809452,317.8],[21.1729531,-156.881129,313.0],[21.1727674,-156.8813007,307.8],[21.1725759,-156.8814258,302.9],[21.172347,-156.8815242,297.5],[21.1721058,-156.8815784,292.1],[21.1718814,-156.8815876,287.3],[21.1716365,-156.8815537,282.1],[21.1714022,-156.8814758,276.9],[21.1711832,-156.8813558,271.5],[21.1709706,-156.8811824,265.7],[21.1708179,-156.881006,260.9],[21.1706823,-156.8807863,255.5],[21.1705836,-156.8805438,250.2],[21.1705264,-156.8802888,244.9],[21.1705112,-156.8800266,239.6],[21.1705339,-156.8797881,234.9],[21.1705987,-156.8795334,229.6],[21.1707148,-156.8792747,223.8],[21.1708445,-156.8790808,219.0],[21.1710184,-156.8788957,213.6],[21.1712013,-156.8787575,208.9],[21.1714207,-156.8786443,203.8],[21.171655,-156.878572,198.6],[21.1718751,-156.8785441,193.8],[21.1721216,-156.8785568,188.8],[21.1723402,-156.8786132,185.4],[21.1725649,-156.8787202,183.0],[21.1727676,-156.8788636,181.3],[21.1729308,-156.879022,180.1],[21.1730839,-156.8792242,179.2],[21.1732037,-156.879452,178.6],[21.1732801,-156.8796769,178.4],[21.1733245,-156.8799361,178.3],[21.1733273,-156.8801788,178.2],[21.173288,-156.8804413,178.4],[21.1732043,-156.8806934,178.6],[21.1730928,-156.8809053,178.7],[21.1729349,-156.8811117,178.9],[21.1727459,-156.881285,179.0],[21.1725305,-156.8814198,179.1],[21.1722977,-156.8815116,179.3],[21.1720747,-156.8815557,179.3],[21.1718322,-156.881551,179.5],[21.1716157,-156.8814489,181.4],[21.1714515,-156.8813114,184.1],[21.1712894,-156.8811537,187.8],[21.1711443,-156.8810096,191.7],[21.1709833,-156.8808519,195.6],[21.1708185,-156.8806935,198.0],[21.1706684,-156.8805511,199.4],[21.1704974,-156.8804028,200.4],[21.1703076,-156.8802837,201.0],[21.1700997,-156.8802091,201.4],[21.1698839,-156.8801793,201.4],[21.1696854,-156.880183,201.2],[21.1694691,-156.8802091,200.9],[21.169253,-156.8802457,200.8],[21.1690572,-156.8802799,200.7],[21.1688413,-156.8803134,200.6],[21.1686451,-156.8803385,200.5],[21.1684278,-156.8803605,200.4],[21.1682089,-156.8803782,200.4],[21.1680107,-156.8803922,200.4],[21.1677935,-156.8804069,200.3],[21.1675749,-156.8804221,200.2],[21.167356,-156.8804386,200.2],[21.1671376,-156.8804563,200.1],[21.1669381,-156.880473,200.1],[21.1667197,-156.8804917,200.1],[21.1665021,-156.8805101,200.1],[21.1663034,-156.8805267,200.1],[21.1660847,-156.8805446,200.1],[21.1658857,-156.8805607,200.1],[21.1656669,-156.8805784,200.1],[21.1654474,-156.8805963,200.1],[21.1652486,-156.8806125,200.0],[21.1650291,-156.8806306,200.0],[21.1648111,-156.8806486,200.0],[21.1645923,-156.8806668,200.0],[21.1643725,-156.8806852,200.0],[21.1641736,-156.8807018,200.0],[21.1639543,-156.8807201,200.0],[21.1637358,-156.8807383,200.0],[21.1635367,-156.8807549,200.0],[21.1633194,-156.8807724,200.0],[21.1631202,-156.8807887,199.9],[21.1629006,-156.8808067,200.0],[21.1626812,-156.8808247,200.0],[21.1624615,-156.8808428,200.0],[21.1622432,-156.8808605,200.1],[21.1620465,-156.8808764,200.0],[21.1618287,-156.8808939,199.9],[21.1616092,-156.8809118,200.0],[21.1614097,-156.8809282,200.0],[21.16119,-156.8809466,200.0],[21.1609701,-156.8809649,200.1],[21.1607724,-156.8809812,200.0],[21.1605552,-156.8809989,200.0],[21.1603559,-156.8810152,200.0],[21.1601366,-156.8810331,200.0],[21.1599173,-156.8810458,200.0],[21.1597162,-156.8810485,200.0],[21.1594979,-156.8810446,200.1],[21.1592975,-156.881038,200.0],[21.1590789,-156.8810304,200.0],[21.1588604,-156.8810244,200.0],[21.158662,-156.8810208,200.0],[21.1584424,-156.8810189,199.9],[21.1582214,-156.8810186,200.0],[21.1580204,-156.8810194,200.0],[21.1578018,-156.8810205,200.0],[21.1576028,-156.8810195,200.0],[21.1573874,-156.8809786,200.2],[21.1571863,-156.8808832,200.5],[21.15702,-156.8807634,200.3],[21.1568511,-156.8806128,200.1],[21.1566874,-156.8804557,199.9],[21.1565219,-156.8803018,199.8],[21.156351,-156.880155,199.8],[21.1561896,-156.8800276,199.8],[21.156008,-156.8798942,199.9],[21.1558241,-156.8797654,199.9],[21.1556561,-156.8796504,199.9],[21.1554706,-156.8795238,199.9],[21.1553027,-156.8794079,199.9],[21.1551198,-156.87928,199.9],[21.1549377,-156.8791508,199.9],[21.1547725,-156.8790324,199.9],[21.1545849,-156.8789095,200.1],[21.1543899,-156.878801,200.7],[21.1541923,-156.8787035,201.6],[21.1539932,-156.8786098,202.6],[21.153813,-156.8785246,203.6],[21.1536149,-156.8784279,204.8],[21.1534189,-156.8783281,206.1],[21.1532413,-156.8782344,207.4],[21.1530465,-156.8781293,208.8],[21.1528697,-156.878033,210.1],[21.1526747,-156.8779266,211.6],[21.1524799,-156.8778207,213.1],[21.1523019,-156.8777246,214.5],[21.1521058,-156.8776195,216.0],[21.1519129,-156.8775078,218.2],[21.1517155,-156.8773711,221.9],[21.1515416,-156.8772382,224.8],[21.1513835,-156.8771136,226.3],[21.1512071,-156.8769749,227.5],[21.1510259,-156.8768354,228.5],[21.1508427,-156.8766983,229.4],[21.1506406,-156.8765511,230.0],[21.1504722,-156.8764308,230.4],[21.1502865,-156.8762999,230.6],[21.1501013,-156.8761702,230.7],[21.1499329,-156.876052,230.8],[21.1497484,-156.875922,230.7],[21.1495812,-156.8758032,230.6],[21.1493897,-156.8756874,230.6],[21.1491774,-156.8756244,231.0],[21.1489779,-156.875613,231.5],[21.1487602,-156.875633,232.0],[21.1485638,-156.8756641,232.7],[21.1483484,-156.875698,233.8],[21.1481324,-156.8757229,235.1],[21.1479349,-156.8757359,236.4],[21.1477173,-156.8757407,237.9],[21.1474995,-156.875739,239.5],[21.1473014,-156.8757345,241.0],[21.1470829,-156.875729,242.7],[21.1468844,-156.8757253,244.2],[21.1466672,-156.875749,246.3],[21.1464607,-156.8758281,248.2],[21.1462901,-156.8759396,249.7],[21.1461199,-156.8760877,251.2],[21.1459569,-156.8762469,252.8],[21.1457931,-156.8764042,254.7],[21.1456248,-156.876553,256.8],[21.1454659,-156.8766807,258.6],[21.1452861,-156.8768135,260.7],[21.1451027,-156.8769412,262.7],[21.1449348,-156.8770551,264.6],[21.14475,-156.8771802,266.6],[21.1445824,-156.8772948,268.5],[21.144399,-156.8774226,270.6],[21.1442107,-156.8775412,272.9],[21.1440196,-156.877594,275.9],[21.1438019,-156.8775877,278.2],[21.1435913,-156.8775204,279.6],[21.1433966,-156.8774077,280.7],[21.1432152,-156.8772734,281.9],[21.143053,-156.8771463,283.1],[21.1428712,-156.8770114,284.7],[21.1426837,-156.8768868,286.2],[21.1425086,-156.8767831,287.6],[21.1423124,-156.8766773,289.1],[21.1421318,-156.8765856,290.4],[21.1419326,-156.8764868,291.9],[21.1417337,-156.8763874,293.4],[21.1415534,-156.8762954,294.7],[21.1413527,-156.8761991,296.3],[21.1411384,-156.8761472,298.2],[21.1409202,-156.8761472,300.3],[21.1407043,-156.8761837,302.9],[21.1405134,-156.8762301,305.5],[21.1403038,-156.8762813,308.8],[21.1400733,-156.876328,312.7],[21.1398606,-156.8763601,316.5],[21.1396466,-156.8763835,320.4],[21.1394509,-156.8764001,324.0],[21.1392364,-156.8764159,328.1],[21.1390205,-156.8764313,332.1],[21.1388227,-156.8764461,335.9],[21.1386057,-156.8764633,340.1],[21.1384089,-156.8764805,343.9],[21.1381927,-156.8765008,348.0],[21.1379565,-156.8765239,352.5],[21.1377593,-156.8765433,356.2],[21.1375429,-156.8765646,360.4],[21.1373462,-156.8765837,364.1],[21.1371287,-156.8766044,368.2],[21.1369104,-156.8766249,372.3],[21.1367135,-156.8766428,376.1],[21.1364958,-156.8766623,380.2],[21.1362799,-156.8766817,384.3],[21.1360824,-156.8766997,388.0],[21.1358657,-156.8767197,392.1],[21.135667,-156.8767381,395.9],[21.1354503,-156.8767586,399.9],[21.1352323,-156.8767788,404.0],[21.1350351,-156.8767959,407.8],[21.1348186,-156.876781,412.3],[21.1346095,-156.8767147,416.5],[21.1344132,-156.8766105,420.3],[21.1342271,-156.8764857,424.0],[21.1340615,-156.8763684,427.6],[21.1338764,-156.8762429,431.7],[21.1336882,-156.8761268,435.7],[21.1335119,-156.8760286,439.4],[21.1333146,-156.8759269,443.4],[21.133134,-156.8758379,447.2],[21.1329347,-156.8757422,451.3],[21.1327351,-156.8756463,455.4],[21.1325545,-156.8755582,459.1],[21.1323563,-156.8754596,463.2],[21.1321579,-156.8753589,467.3],[21.1319611,-156.8752581,471.4],[21.1317632,-156.8751565,475.6],[21.1315835,-156.8750642,479.3],[21.1313857,-156.874963,483.4],[21.1311877,-156.8748629,487.5],[21.1310083,-156.874772,491.3],[21.1308104,-156.8746719,495.4],[21.1306308,-156.8745813,499.1],[21.1304328,-156.8744815,503.2],[21.1302342,-156.8743813,507.4],[21.1300536,-156.8742895,511.1],[21.1298562,-156.8741889,515.2],[21.129658,-156.8740878,519.3],[21.1294604,-156.8739872,523.4],[21.1292617,-156.873886,527.6],[21.1290817,-156.8737947,531.3],[21.1288835,-156.8736942,535.4],[21.1286853,-156.8735937,539.5],[21.1285046,-156.8735026,543.2],[21.128306,-156.873403,547.3],[21.1281222,-156.8733161,551.1],[21.1279191,-156.8732265,555.2],[21.1277145,-156.8731399,559.3],[21.1275277,-156.8730618,563.0],[21.1273227,-156.8729751,567.0],[21.127137,-156.872895,570.8],[21.1269336,-156.8728057,574.8],[21.1267313,-156.8727157,578.9],[21.1265473,-156.8726335,582.6],[21.1263435,-156.8725431,586.6],[21.1261396,-156.8724529,590.7],[21.1259544,-156.8723707,594.4],[21.1257514,-156.8722806,598.5],[21.1255657,-156.872198,602.2],[21.125362,-156.8721074,606.2],[21.1251591,-156.8720164,610.3],[21.1249743,-156.8719334,614.0],[21.1247709,-156.8718426,618.1],[21.1245667,-156.8717517,622.1],[21.1243448,-156.8716532,626.6],[21.1241415,-156.871563,630.6],[21.1239558,-156.8714818,634.3],[21.1237437,-156.8714154,638.7],[21.1235254,-156.8713861,643.4],[21.1233264,-156.8713818,647.7],[21.1231087,-156.8713873,652.6],[21.1229111,-156.8713923,657.3],[21.1226942,-156.8713921,662.5],[21.1224781,-156.871384,667.8],[21.1222797,-156.8713709,672.6],[21.1220607,-156.8713523,677.7],[21.1218613,-156.8713343,682.4],[21.1216421,-156.8713148,687.6],[21.1214233,-156.8712961,692.8],[21.121225,-156.8712794,697.6],[21.1210071,-156.8712616,702.9],[21.1207883,-156.8712437,708.2],[21.1205896,-156.8712269,713.1],[21.1203707,-156.8712083,718.3],[21.1201716,-156.8711916,723.0],[21.1199521,-156.8711729,728.2],[21.1197343,-156.8711436,733.5],[21.119545,-156.8710768,738.5],[21.1193737,-156.8709297,744.3],[21.1192738,-156.8707192,750.5],[21.1192192,-156.8704914,755.2],[21.1191688,-156.8702621,759.5],[21.1191013,-156.8700611,764.1],[21.1189912,-156.8698596,769.5],[21.1188405,-156.8696866,774.8],[21.1186738,-156.8695637,779.5],[21.1184697,-156.8694718,784.5],[21.1182734,-156.8694247,789.1],[21.1180527,-156.8694097,794.3],[21.1178343,-156.8694281,799.5],[21.1176396,-156.8694719,804.2],[21.1174317,-156.8695492,809.6],[21.1172371,-156.8696555,815.0],[21.1170567,-156.8697917,820.3],[21.1168975,-156.8699535,825.6],[21.1167736,-156.8701221,830.4],[21.1166646,-156.8703259,835.7],[21.1165858,-156.8705457,841.1],[21.1165413,-156.8707555,845.9],[21.1165233,-156.8709917,851.2],[21.1165355,-156.8712064,856.1],[21.1165795,-156.8714376,861.4],[21.1166552,-156.8716596,866.7],[21.1167503,-156.8718483,871.5],[21.1168812,-156.8720373,876.9],[21.1170375,-156.872204,882.2],[21.117215,-156.8723446,887.6],[21.1174284,-156.872465,893.3],[21.1176186,-156.8725361,898.2],[21.1178357,-156.872581,903.6],[21.1180568,-156.8725904,908.9],[21.1182769,-156.872564,914.3],[21.118491,-156.8725031,919.7],[21.1186765,-156.8724184,924.6],[21.1188661,-156.872295,929.9],[21.1190511,-156.8721277,935.6],[21.1191832,-156.871966,940.5],[21.119304,-156.8717675,945.9],[21.1193899,-156.8715717,950.7],[21.1194555,-156.8713442,956.0],[21.1194889,-156.8711084,961.3],[21.1194904,-156.8708916,966.0],[21.11946,-156.8706557,971.3],[21.1194039,-156.8704475,976.1],[21.1193133,-156.8702319,981.5],[21.1191938,-156.8700328,986.8],[21.1190619,-156.8698694,991.6],[21.1188946,-156.8697127,997.0],[21.1187073,-156.8695836,1002.2],[21.1185228,-156.8694929,1007.0],[21.1183094,-156.8694261,1012.3],[21.1181099,-156.8693964,1017.1],[21.1178876,-156.869397,1022.4],[21.1176675,-156.8694323,1027.8],[21.117473,-156.8694941,1032.7],[21.1172703,-156.8695925,1037.9],[21.1170827,-156.8697226,1043.2],[21.1169154,-156.8698809,1048.4],[21.1167724,-156.8700644,1053.7],[21.116666,-156.8702498,1058.5],[21.1165769,-156.870469,1063.8],[21.1165187,-156.8707,1069.1],[21.1164937,-156.8709164,1073.8],[21.1164975,-156.8711565,1078.9],[21.1165303,-156.8713726,1083.8],[21.1165968,-156.8716003,1089.0],[21.1166942,-156.8718158,1094.2],[21.1168079,-156.8719965,1099.0],[21.1169577,-156.8721751,1104.3],[21.1171303,-156.8723287,1109.5],[21.1173223,-156.8724571,1113.8],[21.117532,-156.8725584,1116.5],[21.1177373,-156.87262,1118.3],[21.1179715,-156.8726484,1119.7],[21.1182075,-156.8726342,1120.8],[21.1184162,-156.8725858,1121.4],[21.1186347,-156.8724964,1121.8],[21.118819,-156.8723861,1121.9],[21.1190018,-156.8722365,1121.8],[21.1191618,-156.8720597,1121.7],[21.1192851,-156.8718784,1121.5],[21.119393,-156.8716611,1121.4],[21.1194642,-156.8714505,1121.2],[21.1195106,-156.8712101,1121.1],[21.1195229,-156.8709655,1121.0],[21.1195018,-156.87074,1120.8],[21.1193996,-156.8705031,1122.4],[21.1192411,-156.8702971,1123.5],[21.1190837,-156.8701255,1122.3],[21.1189052,-156.869948,1119.3],[21.1187333,-156.8697846,1117.3],[21.1185374,-156.8696002,1116.9],[21.1183215,-156.8693944,1117.4],[21.1181514,-156.8692098,1118.0],[21.1179982,-156.8689771,1118.8],[21.1179008,-156.8687417,1119.3],[21.1178386,-156.8684663,1119.7],[21.1178131,-156.8681851,1119.6],[21.1178101,-156.8679288,1119.5],[21.1178163,-156.8676479,1119.5],[21.1178229,-156.8673674,1119.6],[21.1178249,-156.8671127,1119.7],[21.1178211,-156.8668332,1119.9],[21.1178124,-156.8665793,1119.9],[21.1177988,-156.8663006,1120.0],[21.1177832,-156.8660222,1120.0],[21.1177688,-156.8657688,1120.0],[21.1177534,-156.8654901,1120.0],[21.1177394,-156.8652103,1120.0],[21.1177263,-156.8649306,1120.1],[21.1177137,-156.8646523,1120.1],[21.1177023,-156.8643997,1120.1],[21.1176899,-156.8641211,1120.0],[21.1176775,-156.8638421,1120.0],[21.1176663,-156.8635877,1120.1],[21.1176538,-156.8633076,1120.1],[21.1176423,-156.8630533,1120.1],[21.1176293,-156.8627739,1120.1],[21.1176165,-156.862495,1120.1],[21.117605,-156.8622419,1120.0],[21.1175921,-156.8619641,1120.0],[21.1175791,-156.8616847,1120.0],[21.1175663,-156.8614057,1120.0],[21.1175537,-156.8611261,1120.0],[21.1175424,-156.8608722,1120.0],[21.1175299,-156.8605923,1120.0],[21.1175171,-156.8603127,1120.0],[21.1175053,-156.8600597,1120.0],[21.1174952,-156.8597818,1119.9],[21.1175026,-156.8595273,1119.3],[21.1175299,-156.8592481,1117.3],[21.1175722,-156.8589688,1114.3],[21.1176172,-156.8587158,1111.1],[21.117667,-156.8584387,1107.3],[21.1177128,-156.8581631,1103.1],[21.1177535,-156.8578872,1098.8],[21.1177891,-156.8576117,1094.3],[21.1178181,-156.8573615,1090.0],[21.1178478,-156.857087,1085.3],[21.1178769,-156.8568123,1080.5],[21.1179035,-156.8565628,1076.1],[21.1179334,-156.8562892,1071.3],[21.1179614,-156.8560405,1067.0],[21.1179928,-156.8557666,1062.2],[21.1180247,-156.855493,1057.5],[21.1180536,-156.8552446,1053.2],[21.1180852,-156.8549719,1048.5],[21.1181166,-156.8546988,1043.7],[21.1181478,-156.854426,1039.0],[21.1181789,-156.8541548,1034.4],[21.1182073,-156.8539072,1030.1],[21.1182387,-156.8536337,1025.5],[21.1182702,-156.8533598,1020.8],[21.1182991,-156.8531107,1016.6],[21.1183307,-156.8528388,1012.0],[21.1183595,-156.8525919,1007.8],[21.1183908,-156.8523195,1003.1],[21.1184221,-156.8520465,998.5],[21.1184505,-156.8517982,994.3],[21.1184816,-156.8515254,989.7],[21.11851,-156.8512771,985.5],[21.1185411,-156.8510061,980.9],[21.1185724,-156.8507345,976.2],[21.1186041,-156.850462,971.6],[21.1186357,-156.850189,967.0],[21.1186671,-156.8499159,962.4],[21.1186955,-156.849668,958.2],[21.1187266,-156.8493961,953.6],[21.1187549,-156.8491488,949.4],[21.118786,-156.8488774,944.8],[21.1188169,-156.8486072,940.1],[21.1188453,-156.8483601,935.9],[21.1188769,-156.8480873,931.3],[21.1189058,-156.8478391,927.3],[21.1189377,-156.8475666,922.7],[21.1189692,-156.8472957,918.1],[21.1189978,-156.8470499,913.9],[21.1190288,-156.8467789,909.2],[21.1190563,-156.8465059,904.7],[21.119077,-156.8462573,900.9],[21.1190966,-156.8459836,896.8],[21.1191133,-156.8457354,893.1],[21.1191319,-156.8454643,889.1],[21.1191517,-156.8451926,885.1],[21.1191711,-156.8449442,881.6],[21.1191935,-156.8446711,877.7],[21.1192167,-156.8443972,873.9],[21.1192399,-156.8441244,870.0],[21.1192632,-156.8438514,866.1],[21.1192842,-156.843604,862.6],[21.1193072,-156.8433325,858.8],[21.1193301,-156.8430601,854.9],[21.1193508,-156.8428119,851.4],[21.1193735,-156.842539,847.6],[21.1193939,-156.8422908,844.1],[21.1194166,-156.8420182,840.3],[21.1194392,-156.8417458,836.4],[21.1194596,-156.8414982,832.9],[21.119482,-156.8412246,829.1],[21.1195042,-156.8409551,825.2],[21.1195267,-156.8406815,821.3],[21.1195492,-156.8404113,817.5],[21.1195698,-156.8401644,814.0],[21.1195926,-156.839892,810.1],[21.1196155,-156.8396194,806.3],[21.1196363,-156.8393733,802.9],[21.119659,-156.8391025,799.0],[21.1196798,-156.8388549,795.5],[21.1197025,-156.8385828,791.6],[21.119725,-156.8383108,787.8],[21.1197457,-156.8380634,784.3],[21.1197686,-156.8377913,780.5],[21.1197914,-156.8375192,776.6],[21.1198137,-156.8372474,772.7],[21.1198378,-156.8369511,768.5],[21.1198581,-156.8367042,765.0],[21.1198807,-156.8364328,761.2],[21.1199036,-156.8361612,757.3],[21.1199264,-156.8358899,753.5],[21.1199489,-156.8356197,749.6],[21.1199692,-156.8353731,746.1],[21.1199917,-156.8351018,742.3],[21.1200145,-156.83483,738.4],[21.1200338,-156.834583,734.8],[21.1200531,-156.8342873,730.0],[21.1200667,-156.8340404,725.7],[21.1200803,-156.833769,720.8],[21.1200938,-156.8334977,715.8],[21.1201068,-156.8332512,711.2],[21.1201217,-156.832981,706.0],[21.120136,-156.8327355,701.2],[21.1201525,-156.8324657,695.9],[21.1201696,-156.8321961,690.6],[21.1201852,-156.8319512,685.8],[21.1202024,-156.8316832,680.5],[21.1202193,-156.8314156,675.2],[21.1202344,-156.8311711,670.3],[21.1202508,-156.8309015,665.1],[21.1202654,-156.8306563,660.3],[21.1202812,-156.8303878,655.1],[21.1202969,-156.8301209,649.8],[21.1203114,-156.8298772,645.0],[21.1203276,-156.8296087,639.7],[21.1203441,-156.8293398,634.5],[21.1203608,-156.8290717,629.2],[21.1203777,-156.8288038,624.0],[21.1203929,-156.8285598,619.2],[21.1204096,-156.8282913,614.0],[21.1204261,-156.8280228,608.8],[21.1204408,-156.8277804,604.0],[21.1204566,-156.8275139,598.7],[21.1204813,-156.827271,593.8],[21.1205828,-156.8270222,589.2],[21.1207636,-156.8268305,584.6],[21.1209704,-156.8267249,580.0],[21.1212155,-156.8266833,574.4],[21.1214618,-156.8267024,568.7],[21.1217045,-156.8267546,563.1],[21.1219467,-156.8268157,558.0],[21.12217,-156.8268652,554.0],[21.1224173,-156.8269062,549.9],[21.1226663,-156.826933,546.0],[21.1228927,-156.826948,542.4],[21.1231426,-156.8269581,538.4],[21.1233698,-156.8269647,534.8],[21.1236208,-156.8269719,530.8],[21.1238716,-156.8269807,526.9],[21.1240988,-156.8269906,523.3],[21.124348,-156.8270031,519.3],[21.1245985,-156.8270171,515.3],[21.1248491,-156.8270315,511.4],[21.1250997,-156.8270459,507.5],[21.1253278,-156.8270588,503.9],[21.1255789,-156.8270723,499.9],[21.1258295,-156.8270854,496.0],[21.1260575,-156.8270972,492.4],[21.1263068,-156.8271104,488.4],[21.1265333,-156.8271225,484.7],[21.1267834,-156.8271359,480.7],[21.127057,-156.8271504,476.4],[21.127285,-156.8271623,472.8],[21.1275349,-156.8271753,468.8],[21.127761,-156.8271873,465.1],[21.1280101,-156.827201,461.1],[21.1282579,-156.8272427,457.0],[21.1284742,-156.8273252,454.0],[21.1286976,-156.8274491,450.9],[21.1289114,-156.8275886,447.7],[21.129104,-156.8277178,445.0],[21.1293184,-156.8278555,442.3],[21.1295174,-156.8279739,440.0],[21.12974,-156.8280966,437.5],[21.1299657,-156.8282132,435.1],[21.1301716,-156.8283157,432.9],[21.1303988,-156.8284267,430.5],[21.1306063,-156.8285281,428.3],[21.130835,-156.8286407,426.0],[21.1310632,-156.8287546,423.7],[21.1312702,-156.828859,421.5],[21.1314968,-156.8289744,419.1],[21.1317234,-156.8290899,416.6],[21.1319294,-156.8291947,414.5],[21.1321558,-156.82931,412.0],[21.1323622,-156.8294148,409.8],[21.1325894,-156.8295302,407.4],[21.1328166,-156.8296454,405.0],[21.1330235,-156.8297501,402.7],[21.1332505,-156.8298649,400.3],[21.1334778,-156.8299799,397.9],[21.1337031,-156.8300944,395.5],[21.1339292,-156.8302096,393.0],[21.1341352,-156.8303145,390.8],[21.1343622,-156.8304297,388.4],[21.1345886,-156.8305448,385.9],[21.1347896,-156.8306623,383.7],[21.1349972,-156.8308138,381.7],[21.1351764,-156.8309651,379.8],[21.135367,-156.8311364,377.9],[21.1355577,-156.8313088,376.0],[21.1357337,-156.8314632,374.3],[21.1359315,-156.83163,372.6],[21.136132,-156.8317921,370.9],[21.1363346,-156.8319509,369.2],[21.1365379,-156.832108,367.5],[21.1367233,-156.8322507,366.0],[21.1369274,-156.8324077,364.3],[21.1371311,-156.8325651,362.6],[21.137316,-156.8327087,361.1],[21.1375189,-156.8328672,359.4],[21.1377033,-156.8330114,357.9],[21.1379046,-156.833169,356.3],[21.1381067,-156.8333275,354.6],[21.1382912,-156.8334723,353.1],[21.1384938,-156.8336316,351.4],[21.1386966,-156.8337905,349.8],[21.1388995,-156.8339493,348.1],[21.1391024,-156.834108,346.4],[21.1392867,-156.8342522,344.9],[21.1394882,-156.8344098,343.2],[21.1396901,-156.8345678,341.4],[21.1398747,-156.8347121,339.9],[21.1400781,-156.8348707,338.3],[21.1402625,-156.835015,336.8],[21.1404654,-156.8351713,335.0],[21.1406896,-156.8352902,333.2],[21.1409129,-156.8353452,332.2],[21.1411633,-156.835361,330.8],[21.141414,-156.8353514,329.2],[21.1416873,-156.8353327,327.8],[21.1419377,-156.8353209,326.7],[21.1421648,-156.8353188,325.8],[21.1424139,-156.8353265,324.9],[21.1426635,-156.8353421,323.9],[21.1429132,-156.8353624,322.9],[21.1431626,-156.8353843,322.1],[21.1433898,-156.8354043,321.2],[21.1436398,-156.8354253,320.2],[21.1438897,-156.835445,319.3],[21.1441393,-156.8354636,318.3],[21.1443881,-156.8354811,317.4],[21.1446153,-156.8354968,316.5],[21.144866,-156.8355142,315.5],[21.1451168,-156.835532,314.5],[21.1453448,-156.8355486,313.7],[21.1455941,-156.8355669,312.7],[21.1458205,-156.8355835,311.8],[21.1460703,-156.8356016,310.8],[21.1463204,-156.8356198,309.8],[21.1465478,-156.835636,309.0],[21.1467987,-156.8356536,308.0],[21.1470491,-156.8356712,307.0],[21.1472755,-156.8356876,306.1],[21.1475244,-156.8357059,305.1],[21.1477517,-156.8357227,304.2],[21.1480013,-156.8357409,303.2],[21.1482514,-156.8357592,302.3],[21.1484789,-156.8357714,301.3],[21.14873,-156.835775,299.8],[21.1489813,-156.8357708,298.0],[21.1492326,-156.8357631,296.0],[21.1494836,-156.8357556,294.0],[21.1497123,-156.8357503,292.1],[21.1499628,-156.8357466,289.9],[21.1502132,-156.8357448,287.6],[21.1504404,-156.8357445,285.5],[21.1506899,-156.8357453,283.2],[21.1509146,-156.8357462,281.1],[21.1511647,-156.8357472,278.7],[21.1514132,-156.835748,276.3],[21.1516403,-156.8357485,274.2],[21.1518882,-156.8357487,271.9],[21.152137,-156.8357486,269.5],[21.1523866,-156.8357486,267.2],[21.1526369,-156.835748,264.9],[21.1528637,-156.8357472,262.8],[21.153113,-156.8357466,260.4],[21.1533622,-156.8357462,258.1],[21.1535877,-156.8357431,255.8],[21.1538335,-156.8357006,253.6],[21.1540482,-156.8356201,252.0],[21.1542725,-156.8355035,250.2],[21.1544901,-156.8353744,248.4],[21.1546876,-156.835256,246.9],[21.1549076,-156.8351315,245.5],[21.1551112,-156.8350253,244.3],[21.1553386,-156.8349162,243.0],[21.1555689,-156.834813,241.8],[21.1557796,-156.8347223,240.7],[21.1560113,-156.8346239,239.4],[21.156243,-156.8345249,238.2],[21.1564532,-156.8344336,237.0],[21.1566836,-156.8343321,235.8],[21.1568932,-156.8342387,234.6],[21.1571233,-156.8341356,233.3],[21.1573533,-156.8340319,232.1],[21.1575625,-156.8339374,231.0],[21.1577923,-156.8338337,229.7],[21.1580224,-156.8337301,228.4],[21.1582514,-156.8336279,227.2],[21.1584809,-156.833526,225.9],[21.1586905,-156.8334331,224.7],[21.1589221,-156.8333326,223.4],[21.1591653,-156.8332681,222.3],[21.1593929,-156.8332553,221.6],[21.1596409,-156.8332765,220.6],[21.1598633,-156.8333122,219.7],[21.1601087,-156.8333558,218.9],[21.1603551,-156.8333944,218.3],[21.1605803,-156.8334216,217.8],[21.1608282,-156.8334422,217.3],[21.1610777,-156.8334552,216.8],[21.1613267,-156.8334637,216.3],[21.1615762,-156.8334705,215.8],[21.1618028,-156.8334768,215.4],[21.1620512,-156.8334849,215.0],[21.1623002,-156.8334947,214.6],[21.1625275,-156.8335051,214.1],[21.1627774,-156.8335176,213.7],[21.1630045,-156.8335292,213.3],[21.1632519,-156.8335416,212.9],[21.1635003,-156.8335533,212.3],[21.163727,-156.8335636,211.9],[21.1639774,-156.833575,211.4],[21.1642271,-156.8335862,211.0],[21.1644766,-156.8335975,210.6],[21.1647255,-156.8336091,210.1],[21.1649524,-156.8336198,209.6],[21.1652012,-156.8336313,209.1],[21.1654505,-156.8336426,208.6],[21.165677,-156.8336531,208.2],[21.1659267,-156.8336646,207.7],[21.1661535,-156.833675,207.3],[21.1664015,-156.8336867,206.8],[21.1666504,-156.8336988,206.3],[21.1668775,-156.8337101,205.9],[21.1671273,-156.8337221,205.4],[21.1673773,-156.8337337,205.0],[21.1676265,-156.8337451,204.5],[21.1678753,-156.8337563,204.0],[21.168101,-156.8337667,203.6],[21.1683482,-156.833778,203.1],[21.1685965,-156.8337892,202.5],[21.1688234,-156.8338021,202.0],[21.1690653,-156.833872,201.6],[21.1692518,-156.8340147,202.0],[21.169398,-156.8342316,202.0],[21.1694893,-156.8345038,201.1],[21.1695241,-156.8347425,199.9],[21.1695382,-156.8350095,199.0],[21.1695461,-156.8352528,198.6],[21.1695621,-156.8355194,198.4],[21.1695906,-156.8357846,198.3],[21.1696268,-156.8360238,198.2],[21.1696747,-156.8362846,198.1],[21.1697276,-156.8365448,198.0],[21.1697772,-156.8367813,197.8],[21.1698316,-156.8370416,197.6],[21.1698797,-156.8372789,197.5],[21.1699306,-156.8375403,197.4],[21.1699796,-156.8378022,197.2],[21.1700233,-156.8380409,197.0],[21.1700715,-156.8383032,196.8],[21.1701155,-156.8385417,196.6],[21.170164,-156.8388042,196.4],[21.1702126,-156.8390653,196.2],[21.1702574,-156.8393025,196.0],[21.1703066,-156.8395637,195.8],[21.1703558,-156.8398259,195.6],[21.1704005,-156.8400642,195.4],[21.1704494,-156.8403266,195.2],[21.1704936,-156.8405651,195.0],[21.1705425,-156.8408276,194.8],[21.1705916,-156.84109,194.6],[21.1706363,-156.8413288,194.4],[21.1706854,-156.8415905,194.2],[21.1707347,-156.8418529,193.9],[21.1707835,-156.8421151,193.7],[21.1708318,-156.8423781,193.5],[21.1708758,-156.8426167,193.3],[21.1709246,-156.8428788,193.1],[21.1709738,-156.8431406,192.9],[21.1710184,-156.8433775,192.7],[21.1710674,-156.8436382,192.4],[21.171112,-156.8438765,192.2],[21.171161,-156.8441389,192.0],[21.1712101,-156.8444023,191.8],[21.1712545,-156.8446412,191.7],[21.1713033,-156.8449031,191.4],[21.1713517,-156.8451638,191.2],[21.1714003,-156.8454253,190.9],[21.1714491,-156.8456872,190.7],[21.1714933,-156.8459259,190.6],[21.1715421,-156.8461886,190.3],[21.1715909,-156.846451,190.1],[21.1716356,-156.8466893,190.0],[21.1716849,-156.8469508,189.7],[21.1717297,-156.8471888,189.5],[21.1717791,-156.8474511,189.3],[21.1718284,-156.8477137,189.1],[21.1718728,-156.8479523,188.9],[21.1719212,-156.8482142,188.7],[21.1719697,-156.8484765,188.4],[21.1720181,-156.8487383,188.2],[21.1720665,-156.8489988,188.0],[21.1721113,-156.8492366,187.8],[21.1721608,-156.849499,187.5],[21.1722103,-156.8497619,187.3],[21.172255,-156.850001,187.2],[21.1723035,-156.8502622,187.0],[21.1723472,-156.8504993,186.8],[21.1723953,-156.8507611,186.6],[21.1724441,-156.8510233,186.3],[21.172489,-156.8512618,186.1],[21.1725382,-156.8515241,185.9],[21.172587,-156.8517866,185.7],[21.1726398,-156.8520707,185.5],[21.1726885,-156.8523323,185.2],[21.1727329,-156.8525708,185.0],[21.1727819,-156.8528337,184.8],[21.1728308,-156.8530968,184.6],[21.1728797,-156.8533592,184.4],[21.1729287,-156.8536212,184.2],[21.1729734,-156.853859,183.9],[21.1730222,-156.8541194,183.7],[21.1730713,-156.8543808,183.5],[21.1731159,-156.8546193,183.3],[21.1731646,-156.8548819,183.1],[21.1732088,-156.855121,182.9],[21.1732575,-156.8553837,182.7],[21.1733061,-156.8556445,182.5],[21.1733503,-156.8558819,182.3],[21.1733994,-156.8561438,182.1],[21.1734487,-156.8564063,181.8],[21.1734979,-156.8566683,181.6],[21.1735469,-156.8569309,181.4],[21.1735912,-156.8571693,181.2],[21.1736393,-156.8574298,181.0],[21.1736876,-156.8576913,180.8],[21.1737319,-156.8579302,180.6],[21.1737666,-156.8581949,181.4],[21.1737684,-156.8584358,183.8],[21.1737498,-156.8586948,187.1],[21.1737233,-156.8589504,190.9],[21.1736999,-156.8591811,194.6],[21.173677,-156.8594391,196.4],[21.1736584,-156.8597066,195.9],[21.1736468,-156.8599811,196.1],[21.1736402,-156.8602557,197.0],[21.1736358,-156.8605036,197.6],[21.1736309,-156.8607731,198.1],[21.1736253,-156.8610413,198.4],[21.1736193,-156.8612842,198.6],[21.1736118,-156.8615523,198.9],[21.1736042,-156.8617904,199.1],[21.1735951,-156.8620595,199.3],[21.173586,-156.8623244,199.7],[21.173578,-156.8625676,200.0],[21.1735696,-156.8628347,200.4],[21.1735612,-156.8631018,200.7],[21.1735532,-156.863366,201.1],[21.1735454,-156.8636328,201.4],[21.1735384,-156.863876,201.7],[21.1735302,-156.8641446,202.1],[21.173522,-156.8644113,202.5],[21.1735143,-156.8646543,202.9],[21.1735058,-156.8649227,203.3],[21.1734983,-156.8651651,203.6],[21.1734898,-156.8654328,203.9],[21.1734813,-156.8656988,204.3],[21.1734732,-156.8659422,204.6],[21.1734648,-156.8662085,204.9],[21.1734564,-156.8664771,205.3],[21.1734484,-156.8667448,205.7],[21.1734403,-156.8670108,206.1],[21.1734328,-156.8672526,206.4],[21.173425,-156.8675198,206.8],[21.1734164,-156.8678125,207.2],[21.1734084,-156.8680797,207.7],[21.1734003,-156.868348,208.0],[21.1733925,-156.8685899,208.3],[21.1733837,-156.8688578,208.7],[21.1733753,-156.869124,209.1],[21.173368,-156.8693682,209.4],[21.1733598,-156.8696347,209.8],[21.1733522,-156.8698789,210.2],[21.1733437,-156.8701473,210.6],[21.1733353,-156.8704136,211.0],[21.1733277,-156.8706557,211.3],[21.1733196,-156.8709222,211.6],[21.1733117,-156.8711905,212.0],[21.1733039,-156.8714581,212.4],[21.1732955,-156.8717265,212.8],[21.1732878,-156.8719706,213.2],[21.1732793,-156.8722364,213.6],[21.1732708,-156.8725015,213.9],[21.1732631,-156.8727445,214.2],[21.1732551,-156.873013,214.6],[21.1732478,-156.8732557,215.0],[21.1732396,-156.8735234,215.4],[21.1732312,-156.8737895,215.8],[21.1732233,-156.8740323,216.1],[21.1732148,-156.8743001,216.5],[21.1732064,-156.8745679,216.9],[21.1731981,-156.8748347,217.2],[21.1731904,-156.8751014,217.6],[21.1731831,-156.8753452,218.0],[21.1731751,-156.8756131,218.4],[21.1731671,-156.8758803,218.7],[21.1731599,-156.876124,219.0],[21.1731518,-156.8763912,219.4],[21.1731444,-156.8766323,219.8],[21.1731358,-156.8769004,220.1],[21.1731271,-156.8771669,220.5],[21.1731191,-156.8774115,220.9],[21.1731105,-156.8776776,221.3],[21.173102,-156.8779467,221.7],[21.1730935,-156.8782135,222.1],[21.1730851,-156.8784814,222.5],[21.1730778,-156.8787235,222.8],[21.1730698,-156.8789925,223.1],[21.1730617,-156.8792596,223.5],[21.1730542,-156.8795027,223.8],[21.173046,-156.8797696,224.2],[21.1730384,-156.8800132,224.5],[21.17303,-156.8802798,224.9],[21.1730216,-156.880548,225.3],[21.1730142,-156.8807905,225.7],[21.173006,-156.8810577,226.0],[21.1729977,-156.8813267,226.4],[21.1729891,-156.8815955,226.8],[21.1729808,-156.8818604,227.3],[21.1729734,-156.8821024,227.5],[21.172965,-156.8823704,227.9],[21.1729569,-156.8826381,228.3],[21.1729495,-156.8828818,228.6],[21.1729412,-156.8831498,229.1],[21.1729336,-156.8833927,229.4],[21.1729254,-156.88366,229.8],[21.1729174,-156.8839269,230.1],[21.1729104,-156.884171,230.5],[21.1729026,-156.8844389,230.9],[21.1728952,-156.884683,231.2],[21.1728867,-156.8849508,231.6],[21.172877,-156.8852428,232.0],[21.1728689,-156.8854867,232.3],[21.17286,-156.8857545,232.7],[21.1728519,-156.8860223,233.1],[21.1728403,-156.8862608,233.5],[21.1727675,-156.886502,234.2],[21.1726389,-156.886682,235.0],[21.1724462,-156.8868204,235.3],[21.1722262,-156.8868908,235.1],[21.1720205,-156.886906,234.6],[21.1717929,-156.886889,234.0],[21.1715877,-156.8868602,233.8],[21.1713665,-156.8868291,233.7],[21.1711494,-156.8868059,233.7],[21.1709554,-156.8867931,233.7],[21.170747,-156.8867873,233.7],[21.1705449,-156.8867873,233.8],[21.1703631,-156.88679,233.8],[21.1701686,-156.8867938,233.9],[21.1699926,-156.886797,234.0],[21.1698045,-156.8867994,234.1],[21.1696159,-156.8868004,234.3],[21.1694437,-156.8868004,234.5],[21.169256,-156.8868,234.7],[21.1690686,-156.8867994,234.9],[21.1688814,-156.8867991,235.0],[21.1686951,-156.886799,235.1],[21.1685269,-156.8867989,235.2],[21.1683425,-156.8867991,235.3],[21.1681554,-156.8867994,235.4],[21.1679859,-156.8867997,235.5],[21.1677986,-156.8868002,235.5],[21.1676274,-156.8868008,235.6],[21.1674402,-156.8868012,235.6],[21.1672517,-156.8868013,235.6],[21.1670808,-156.8868013,235.7],[21.1668926,-156.8868013,235.7],[21.1667027,-156.8868013,235.7],[21.1665209,-156.8868013,235.7],[21.1663514,-156.8868012,236.0],[21.1662176,-156.8868012,236.4],[21.1660859,-156.8868012,236.1],[21.1659672,-156.8868013,235.9],[21.165872,-156.8868014,236.1],[21.1657836,-156.8868013,236.3],[21.1657189,-156.8868013,236.2],[21.1656873,-156.8868009,235.0],[21.1656823,-156.886801,235.7],[21.1656782,-156.8868011,235.7],[21.1656701,-156.8868011,235.7]],"source":"ArduPilot autotest QuadPlane.KalaupapaCanyonRun (SITL, ArduPlane V4.8.0-dev, ardupilot 3cf2e903d7, from ArduPilot PR 34424), flown track sampled once a second between the end of the transition and the start of the VTOL landing descent"} \ No newline at end of file diff --git a/tests/missions/record_flight.py b/tests/missions/record_flight.py new file mode 100644 index 0000000000..10e92454a4 --- /dev/null +++ b/tests/missions/record_flight.py @@ -0,0 +1,222 @@ +#!/usr/bin/env python3 +''' +record a SITL flight of a mission for the tests which check the path drawn +for it against the path flown + + python3 tests/missions/record_flight.py LOG OUT.json --source "..." + +LOG is the flight's dataflash log. OUT gets what MAVExplorer hands +plane_track.mission_track() for it -- home, the mission, the vehicle's +parameters, and where there are any, the takeoff course, where the flight +started, the rally points, the approach course and the course the takeoff +was flown on -- the firmware which +flew it, since what it does is what the drawing follows, and the path +flown, +once a second, from when the aircraft is flying as a plane -- the end of a +QuadPlane's transition, or the first POS of anything else -- to when it +starts to land: a QuadPlane's landing descent, a plane's flare, or the end +of the log -- or to when anything but the mission takes over, as a test +does to end a mission which circles for ever. --start and --end, in +seconds of the log's TimeUS, override either. + +AP_FLAKE8_CLEAN +''' + +import argparse +import importlib.util +import json +import os + +from pymavlink import mavutil + +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import plane_track + +HERE = os.path.dirname(os.path.abspath(__file__)) +# how the firmware names itself, in a log with no VER message +FIRMWARES = ('ArduPlane', 'ArduCopter', 'ArduRover', 'ArduSub', 'APM:') +# what the autopilot says as a flight starts and stops being fixed-wing +STARTS = ('Transition done',) +ENDS = ('Land descend started', 'Flare ') +# ArduPlane's modes a mission is flown in: AUTO, and the RTL and QRTL a +# return to launch switches to. Anything else, or AUTO again after those, +# is the flight being taken over +AUTO = 10 +RETURNING = (11, 21) + + +def explorer(): + path = os.path.join(HERE, '..', '..', 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def firmware(mlog): + '''the firmware which flew the log, as it names itself: whatever the + drawing is checked against has to follow what that version does, so a + recording says which it was. VER says which commit it was built from, + which is what tells one development build from another, so it is taken + over the message the firmware starts with wherever in the log it comes. + None where the log says neither''' + banner = None + mlog.rewind() + while True: + m = mlog.recv_match(type=['VER', 'MSG']) + if m is None: + return banner + if m.get_type() == 'VER': + # FWS is the firmware's own name for itself, GH its commit + name = getattr(m, 'FWS', None) + if name: + commit = getattr(m, 'GH', None) + if commit: + return '%s (%x)' % (name, commit) + return str(name) + elif banner is None and m.Message.startswith(FIRMWARES): + # a log with no VER says it in the message the firmware starts + # with, which a development build names its commit in too + banner = m.Message + + +def extras(heading, start, rally, approach, takeoff_course=None): + '''what the drawing is given beyond the mission, where there is + anything to say: a course of 0 is north, which is something''' + out = {} + for (name, value) in (('heading', heading), ('start', start), + ('rally', rally), ('approach', approach), + ('takeoff_course', takeoff_course)): + if value is not None and value != []: + out[name] = value + return out + + +def times(mlog): + '''when the aircraft starts flying the mission as a plane -- the end of + a QuadPlane's transition, or arming -- and when it starts to land or + stops flying the mission''' + transition = None + armed = None + first = None + last = None + events = [] + mlog.rewind() + while True: + m = mlog.recv_match(type=['MSG', 'POS', 'MODE', 'ARM']) + if m is None: + break + t = m.TimeUS * 1.0e-6 + kind = m.get_type() + if kind == 'POS': + if first is None: + first = t + last = t + elif kind == 'ARM': + if armed is None and m.ArmState: + armed = t + elif kind == 'MODE': + events.append((t, m.ModeNum)) + elif transition is None and m.Message.startswith(STARTS): + transition = t + elif m.Message.startswith(ENDS): + events.append((t, None)) + start = transition + if start is None: + start = armed if armed is not None else first + end = last + returned = False + for (t, mode) in events: + if t < start: + continue + if mode is None or (mode != AUTO and mode not in RETURNING) or ( + mode == AUTO and returned): + end = t + break + if mode in RETURNING: + returned = True + return (start, end) + + +def record(log, out, source, start=None, end=None): + mx = explorer() + mlog = mavutil.mavlink_connection(log) + (path, mission, cmds, started, rally, origin, + approach) = mx.mission_from_log(mlog) + params = mp_util.log_params(mlog) + ground0 = min(p[2] for p in path) + mission = mx.resolve_mission_amsl(mission, ground0, params, mlog.mav_type) + # what the drawing is given, taken as MAVExplorer gives it + captured = {} + real = plane_track.mission_track + + def capture(home, items, params, heading=None, start=None, rally=None, + approach=None, takeoff_course=None): + captured['home'] = list(home) + captured['items'] = [[c, la, lo, a, list(p)] + for (c, la, lo, a, p) in items] + captured.update(extras(heading, start, rally, approach, + takeoff_course)) + return real(home, items, params, heading, start, rally, approach, + takeoff_course) + plane_track.mission_track = capture + try: + mx.plane_mission_track(cmds, mission, + (path[0][0], path[0][1], ground0), params, + mlog.mav_type, path, started, rally, origin, + approach) + finally: + plane_track.mission_track = real + if 'items' not in captured: + raise ValueError('the log has no mission a plane flies') + names = sorted(set(name for (names, _) in plane_track.PARAMETERS.values() + for (name, _) in names)) + captured['params'] = dict((name, params[name]) for name in names + if name in params) + (flying, landing) = times(mlog) + if start is not None: + flying = start + if end is not None: + landing = end + flown = [] + last = None + mlog.rewind() + while True: + m = mlog.recv_match(type='POS') + if m is None: + break + t = m.TimeUS * 1.0e-6 + if t < flying or t > landing: + continue + if last is not None and t - last < 1.0: + continue + last = t + flown.append([round(m.Lat, 7), round(m.Lng, 7), round(m.Alt, 1)]) + captured['flown'] = flown + version = firmware(mlog) + if version is not None: + captured['firmware'] = version + captured['source'] = source + with open(out, 'w') as f: + json.dump(captured, f, separators=(',', ':')) + return (captured, flying, landing) + + +def main(): + parser = argparse.ArgumentParser(description=__doc__.split('\n\n')[0]) + parser.add_argument('log') + parser.add_argument('out') + parser.add_argument('--source', required=True, + help='where the flight came from, for the record') + parser.add_argument('--start', type=float, default=None) + parser.add_argument('--end', type=float, default=None) + args = parser.parse_args() + (captured, flying, landing) = record(args.log, args.out, args.source, + args.start, args.end) + print('%s: %u items, %u points flown from %.1fs to %.1fs, %u bytes' % ( + args.out, len(captured['items']), len(captured['flown']), flying, + landing, os.path.getsize(args.out))) + + +if __name__ == '__main__': + main() diff --git a/tests/missions/sitl-plane-loiter-unlimited.json b/tests/missions/sitl-plane-loiter-unlimited.json new file mode 100644 index 0000000000..e166e8b97c --- /dev/null +++ b/tests/missions/sitl-plane-loiter-unlimited.json @@ -0,0 +1 @@ +{"home":[-35.3629381,149.165085,585.0999755859375],"items":[[22,-35.361279,149.16423,625.0999755859375,[15.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.0999755859375,[0.0,0.0,0.0,0.0]],[17,-35.3557514,149.178303,685.0999755859375,[0.0,0.0,-120.0,0.0]],[189,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[16,-35.365467,149.164215,640.0999755859375,[0.0,0.0,0.0,0.0]],[16,-35.365009,149.165482,625.0999755859375,[0.0,0.0,0.0,0.0]],[21,-35.363041,149.165222,585.0999755859375,[0.0,0.0,0.0,1.0]]],"takeoff_course":353.9666672289162,"start":[-35.3629381,149.165085,585.0700073242188],"params":{"AIRSPEED_CRUISE":22.0,"AIRSPEED_MAX":30.0,"AIRSPEED_MIN":10.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":15.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":0.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.25,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":100.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":80.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":50.0},"flown":[[-35.3629381,149.165085,585.1],[-35.3629247,149.1650834,585.1],[-35.362823,149.1650703,584.8],[-35.3626503,149.1650477,590.3],[-35.3624278,149.1650183,598.6],[-35.3621831,149.1649865,608.4],[-35.3619268,149.164953,618.8],[-35.3616633,149.1649181,628.9],[-35.3613887,149.1648885,634.9],[-35.3611217,149.1648716,637.7],[-35.3608757,149.1648643,637.9],[-35.3606484,149.1648634,636.3],[-35.3604223,149.1648684,636.0],[-35.3601905,149.1648764,637.2],[-35.3599601,149.1648856,639.1],[-35.3597287,149.1648955,641.2],[-35.3595006,149.1649058,643.4],[-35.359271,149.1649159,645.6],[-35.3590449,149.1649262,647.7],[-35.3588173,149.1649367,649.9],[-35.3585932,149.1649469,652.1],[-35.3583671,149.1649574,654.2],[-35.3581442,149.1649674,656.4],[-35.3579199,149.1649774,658.6],[-35.3576983,149.1649875,660.7],[-35.3574753,149.164998,662.8],[-35.3572548,149.1650088,665.0],[-35.3570326,149.1650202,667.0],[-35.3568123,149.1650315,669.1],[-35.3565913,149.1650426,671.3],[-35.356372,149.1650537,673.3],[-35.3561515,149.1650645,675.4],[-35.3559341,149.165106,677.8],[-35.3557516,149.165274,682.0],[-35.3556076,149.1654866,683.1],[-35.3555283,149.1657445,683.8],[-35.3555058,149.1660172,683.8],[-35.3555263,149.1662879,683.7],[-35.3555705,149.166554,683.7],[-35.3556221,149.1668162,683.8],[-35.355671,149.1670813,684.1],[-35.3557112,149.1673471,684.4],[-35.3557408,149.1676168,684.6],[-35.3557596,149.1678845,684.8],[-35.3557697,149.168155,684.8],[-35.3557731,149.1684235,684.9],[-35.355772,149.168695,685.0],[-35.3557681,149.1689633,685.0],[-35.3557632,149.1692349,685.0],[-35.355758,149.1695034,685.0],[-35.3557536,149.1697751,685.0],[-35.3557504,149.170044,685.1],[-35.3557483,149.1703152,685.1],[-35.355747,149.1705839,685.1],[-35.3557465,149.1708554,685.1],[-35.3557464,149.1711246,685.1],[-35.3557466,149.1713956,685.1],[-35.3557475,149.1716647,685.0],[-35.3557484,149.1719355,685.1],[-35.3557492,149.1722055,685.0],[-35.3557497,149.1724762,685.0],[-35.3557502,149.1727466,685.0],[-35.3557507,149.1730168,685.0],[-35.3557508,149.1732872,685.0],[-35.3557509,149.1735576,685.0],[-35.3557509,149.1738286,685.0],[-35.3557509,149.1740985,685.1],[-35.3557509,149.1743697,685.0],[-35.3557509,149.1746391,685.0],[-35.3557507,149.174911,685.0],[-35.3557505,149.1751804,685.0],[-35.3557504,149.1754522,685.0],[-35.3557503,149.1757208,685.0],[-35.3557503,149.1759926,685.0],[-35.3557512,149.1762618,685.0],[-35.3557743,149.1765331,685.0],[-35.35585,149.1767901,685.3],[-35.3559904,149.1770107,685.8],[-35.3561624,149.1771819,685.6],[-35.356344,149.1773327,684.9],[-35.3565195,149.1774901,684.6],[-35.3566802,149.1776767,684.6],[-35.3568107,149.1778965,684.7],[-35.3569008,149.1781485,684.9],[-35.3569413,149.1784167,685.0],[-35.3569304,149.1786896,685.0],[-35.3568691,149.1789508,684.9],[-35.3567624,149.1791904,684.8],[-35.3566183,149.1793969,684.7],[-35.3564444,149.1795647,684.6],[-35.3562484,149.1796899,684.6],[-35.3560377,149.17977,684.6],[-35.3558178,149.1798035,684.6]],"source":"ArduPilot autotest Plane.MissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot bcb5e0c597, from ArduPilot PR 34424): sitl-plane-loiter-unlimited"} \ No newline at end of file diff --git a/tests/missions/sitl-plane-mission-items.json b/tests/missions/sitl-plane-mission-items.json new file mode 100644 index 0000000000..dcf2ea8065 --- /dev/null +++ b/tests/missions/sitl-plane-mission-items.json @@ -0,0 +1 @@ +{"home":[-35.3625914,149.1651656,584.09],"items":[[22,-35.361279,149.16423,624.09,[15.0,0.0,0.0,0.0]],[178,0.0,0.0,0.0,[0.0,20.0,-1.0,0.0]],[16,-35.3557514,149.165085,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3494631,149.1716937,684.09,[0.0,10.0,0.0,0.0]],[16,-35.3557514,149.178303,684.09,[0.0,0.0,80.0,0.0]],[36,-35.3674296,149.1816087,684.09,[90.0,0.0,0.0,0.0]],[16,-35.362938,149.1849129,694.09,[0.0,0.0,0.0,0.0]],[18,-35.3701245,149.1849138,694.09,[2.0,0.0,100.0,0.0]],[18,-35.3710228,149.1772027,694.09,[1.0,0.0,-80.0,1.0]],[19,-35.3674296,149.1716945,695.0,[22.0,0.0,1.0,0.0]],[31,-35.3737178,149.1683898,834.09,[0.0,120.0,0.0,0.0]],[31,-35.3737178,149.1595768,684.09,[0.0,-120.0,0.0,1.0]],[18,0.0,0.0,684.09,[1.0,0.0,80.0,0.0]],[600,0.0,0.0,0.0,[7.0,0.0,0.0,0.0]],[16,-35.3674296,149.1551707,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3656329,149.1617802,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3710228,149.1595769,684.09,[0.0,0.0,0.0,0.0]],[601,0.0,0.0,0.0,[7.0,1.0,0.0,0.0]],[177,0.0,0.0,0.0,[3.0,0.0,0.0,0.0]],[16,-35.360243,149.1573742,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1617804,674.09,[0.0,0.0,0.0,0.0]],[16,-35.3566497,149.1551713,674.09,[0.0,0.0,0.0,0.0]],[177,0.0,0.0,0.0,[20.0,1.0,0.0,0.0]],[189,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[16,-35.365467,149.164215,639.09,[0.0,0.0,0.0,0.0]],[16,-35.365009,149.165482,624.09,[0.0,0.0,0.0,0.0]],[21,-35.363041,149.165222,584.09,[0.0,0.0,0.0,1.0]]],"takeoff_course":353.99782242424425,"start":[-35.3629381,149.165085,585.239990234375],"params":{"AIRSPEED_CRUISE":22.0,"AIRSPEED_MAX":30.0,"AIRSPEED_MIN":10.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":15.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":0.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.25,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":100.0,"RTL_RADIUS":150.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":80.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":50.0},"flown":[[-35.3629381,149.165085,585.2],[-35.3629302,149.165084,585.0],[-35.36284,149.1650725,584.8],[-35.3626772,149.1650514,589.4],[-35.3624587,149.1650232,597.2],[-35.3622167,149.1649916,606.7],[-35.3619588,149.164958,617.0],[-35.3616969,149.1649242,627.3],[-35.3614222,149.1648928,634.4],[-35.3611553,149.1648756,638.5],[-35.3609047,149.1648677,639.8],[-35.3606811,149.164866,638.5],[-35.3604667,149.1648694,637.2],[-35.3602514,149.1648761,637.6],[-35.3600349,149.1648845,639.1],[-35.3598205,149.1648936,640.9],[-35.3596078,149.1649032,642.9],[-35.3593973,149.1649128,644.9],[-35.359189,149.1649224,647.0],[-35.3589816,149.1649319,649.0],[-35.3587761,149.1649412,651.1],[-35.3585711,149.1649507,653.1],[-35.3583682,149.1649599,655.1],[-35.358165,149.1649691,657.1],[-35.3579635,149.1649783,659.1],[-35.3577614,149.1649873,661.1],[-35.3575618,149.1649962,663.0],[-35.3573604,149.1650055,665.0],[-35.3571618,149.1650151,666.8],[-35.356961,149.1650251,668.7],[-35.3567627,149.1650352,670.6],[-35.3565622,149.1650451,672.5],[-35.3563642,149.1650549,674.4],[-35.3561642,149.1650647,676.2],[-35.3559661,149.1650739,678.0],[-35.3557663,149.165089,680.0],[-35.3555738,149.1651586,682.1],[-35.3553933,149.1652744,683.3],[-35.3552324,149.1654232,684.0],[-35.3550853,149.1655933,684.4],[-35.3549482,149.1657731,684.6],[-35.3548142,149.1659573,684.9],[-35.3546796,149.1661401,685.0],[-35.3545414,149.1663202,685.2],[-35.3543995,149.1664952,685.2],[-35.3542541,149.1666648,685.3],[-35.3541054,149.1668304,685.3],[-35.3539548,149.1669923,685.3],[-35.3538016,149.1671529,685.3],[-35.3536483,149.1673109,685.3],[-35.3534935,149.1674697,685.3],[-35.35334,149.1676265,685.2],[-35.3531855,149.1677853,685.2],[-35.3530326,149.1679432,685.2],[-35.3528787,149.1681031,685.2],[-35.3527269,149.1682615,685.2],[-35.3525735,149.1684222,685.2],[-35.352422,149.1685812,685.2],[-35.3522687,149.1687423,685.2],[-35.3521176,149.1689018,685.2],[-35.3519652,149.169063,685.2],[-35.3518139,149.1692228,685.2],[-35.351661,149.1693838,685.2],[-35.3515094,149.1695433,685.2],[-35.3513568,149.1697043,685.1],[-35.3512052,149.1698638,685.1],[-35.3510524,149.1700246,685.2],[-35.3509002,149.1701844,685.2],[-35.3507475,149.1703447,685.2],[-35.3505954,149.1705046,685.2],[-35.3504431,149.1706649,685.2],[-35.3502906,149.170825,685.2],[-35.350138,149.170985,685.2],[-35.3499853,149.1711453,685.2],[-35.3498336,149.1713048,685.2],[-35.3496812,149.1714652,685.1],[-35.3495291,149.171625,685.2],[-35.3493802,149.1717916,685.1],[-35.349307,149.1720282,686.1],[-35.3493329,149.1722903,687.1],[-35.3494371,149.1725109,686.8],[-35.3496029,149.1726601,686.0],[-35.3497904,149.17274,684.8],[-35.3499873,149.1727771,683.9],[-35.3501825,149.1728091,683.5],[-35.3503791,149.172861,683.6],[-35.3505682,149.1729397,683.8],[-35.3507501,149.1730469,684.1],[-35.3509196,149.1731756,684.3],[-35.3510809,149.1733226,684.5],[-35.3512331,149.1734795,684.6],[-35.3513812,149.1736446,684.7],[-35.3515256,149.1738118,684.8],[-35.3516701,149.1739812,684.8],[-35.3518149,149.1741497,684.9],[-35.3519618,149.1743176,685.0],[-35.3521097,149.1744828,685.0],[-35.352259,149.174646,685.1],[-35.3524097,149.1748072,685.1],[-35.3525616,149.1749672,685.1],[-35.3527145,149.1751265,685.1],[-35.3528672,149.175285,685.1],[-35.3530205,149.1754437,685.2],[-35.3531728,149.1756018,685.2],[-35.3533261,149.1757615,685.2],[-35.353478,149.17592,685.2],[-35.3536313,149.1760802,685.2],[-35.3537831,149.1762388,685.1],[-35.3539365,149.1763996,685.1],[-35.3540882,149.1765588,685.1],[-35.3542417,149.1767198,685.1],[-35.3543932,149.1768789,685.2],[-35.3545462,149.1770403,685.2],[-35.3546975,149.1772001,685.2],[-35.3548505,149.1773616,685.2],[-35.3550021,149.1775211,685.2],[-35.3551552,149.1776818,685.2],[-35.355307,149.1778412,685.2],[-35.3554601,149.1780022,685.2],[-35.3556121,149.1781617,685.2],[-35.3557648,149.1783221,685.2],[-35.3559167,149.1784816,685.2],[-35.3560696,149.178642,685.2],[-35.3562219,149.1788018,685.1],[-35.3563745,149.1789619,685.2],[-35.3565322,149.1791126,686.5],[-35.356694,149.1792502,689.0],[-35.3568578,149.1793834,690.8],[-35.3570233,149.1795166,692.1],[-35.357191,149.1796529,693.1],[-35.3573573,149.179791,693.8],[-35.3575235,149.179933,694.3],[-35.3576873,149.1800765,694.7],[-35.357851,149.1802233,694.9],[-35.3580117,149.1803701,695.1],[-35.3581729,149.1805188,695.2],[-35.3583323,149.180667,695.2],[-35.3584935,149.1808174,695.3],[-35.3586529,149.1809656,695.3],[-35.3588139,149.1811151,695.3],[-35.3589732,149.1812629,695.3],[-35.3591341,149.1814124,695.3],[-35.3592935,149.1815603,695.3],[-35.3594546,149.1817091,695.3],[-35.3596143,149.1818562,695.2],[-35.3597758,149.1820041,695.2],[-35.3599362,149.1821511,695.2],[-35.3600972,149.1822992,695.2],[-35.3602575,149.1824468,695.2],[-35.3604181,149.1825948,695.2],[-35.3605783,149.1827423,695.2],[-35.3607391,149.1828902,695.2],[-35.3608997,149.1830381,695.2],[-35.3610599,149.1831852,695.2],[-35.3612207,149.1833328,695.2],[-35.3613812,149.1834803,695.2],[-35.3615425,149.1836283,695.2],[-35.3617025,149.1837754,695.2],[-35.3618634,149.1839234,695.2],[-35.3620232,149.1840706,695.2],[-35.3621846,149.184219,695.2],[-35.3623445,149.1843661,695.1],[-35.362506,149.1845147,695.2],[-35.3626656,149.1846616,695.2],[-35.3628272,149.1848099,695.2],[-35.3629942,149.1849439,695.1],[-35.3631891,149.1850231,695.5],[-35.3633885,149.185057,695.5],[-35.3635896,149.1850569,695.3],[-35.3637874,149.185037,695.2],[-35.3639863,149.1850073,695.1],[-35.3641839,149.1849765,695.1],[-35.3643828,149.184949,695.1],[-35.3645814,149.1849275,695.1],[-35.3647821,149.1849122,695.1],[-35.3649822,149.184903,695.1],[-35.3651829,149.1848987,695.1],[-35.3653834,149.1848977,695.1],[-35.3655846,149.1848989,695.1],[-35.3657855,149.1849013,695.1],[-35.365986,149.1849044,695.1],[-35.3661872,149.1849073,695.1],[-35.3663871,149.1849096,695.2],[-35.3665882,149.1849117,695.1],[-35.3667883,149.1849131,695.1],[-35.3669897,149.184914,695.2],[-35.3671891,149.1849143,695.1],[-35.3673907,149.1849145,695.1],[-35.3675905,149.1849146,695.2],[-35.3677921,149.1849149,695.2],[-35.3679916,149.1849151,695.2],[-35.368193,149.1849153,695.1],[-35.368393,149.1849156,695.1],[-35.368595,149.1849157,695.1],[-35.368795,149.1849246,695.1],[-35.3689941,149.1849755,695.2],[-35.3691738,149.1850961,695.5],[-35.3693192,149.1852763,695.8],[-35.3694386,149.1854714,695.5],[-35.3695585,149.1856642,695.1],[-35.3696953,149.1858394,695.0],[-35.3698582,149.185985,695.0],[-35.3700438,149.1860839,695.1],[-35.3702441,149.1861256,695.3],[-35.3704456,149.1861051,695.4],[-35.3706356,149.1860249,695.4],[-35.3708043,149.1858925,695.3],[-35.3709442,149.1857177,695.3],[-35.3710513,149.1855108,695.2],[-35.3711223,149.1852829,695.1],[-35.371156,149.1850412,695.0],[-35.3711522,149.1847967,695.1],[-35.3711109,149.1845557,695.1],[-35.371034,149.1843302,695.1],[-35.3709229,149.1841252,695.1],[-35.3707831,149.1839511,695.1],[-35.370617,149.1838113,695.1],[-35.3704339,149.1837148,695.2],[-35.3702368,149.1836629,695.1],[-35.3700377,149.1836601,695.2],[-35.3698397,149.1837048,695.2],[-35.3696542,149.1837948,695.2],[-35.3694852,149.1839287,695.2],[-35.369342,149.1840983,695.2],[-35.3692272,149.1843006,695.2],[-35.3691474,149.1845252,695.2],[-35.3691041,149.1847662,695.2],[-35.3690996,149.1850114,695.2],[-35.3691342,149.1852537,695.2],[-35.3692066,149.1854819,695.2],[-35.3693139,149.1856899,695.2],[-35.3694521,149.1858677,695.2],[-35.3696157,149.1860093,695.2],[-35.3697986,149.1861096,695.1],[-35.3699941,149.1861644,695.2],[-35.3701947,149.186172,695.2],[-35.3703919,149.1861322,695.2],[-35.3705798,149.1860449,695.2],[-35.3707487,149.1859158,695.2],[-35.3708955,149.1857479,695.2],[-35.3710121,149.1855495,695.2],[-35.3710959,149.1853252,695.2],[-35.3711422,149.1850879,695.2],[-35.3711501,149.1848415,695.2],[-35.3711196,149.1846003,695.2],[-35.3710508,149.1843682,695.2],[-35.3709476,149.1841594,695.2],[-35.3708118,149.1839778,695.2],[-35.3706512,149.1838328,695.2],[-35.3704689,149.1837275,695.2],[-35.3702754,149.183668,695.1],[-35.3700746,149.1836557,695.1],[-35.3698772,149.1836912,695.2],[-35.3696876,149.1837737,695.2],[-35.3695161,149.183899,695.1],[-35.3693666,149.1840634,695.1],[-35.3692466,149.1842595,695.1],[-35.3691593,149.1844813,695.1],[-35.3691093,149.1847197,695.1],[-35.3690983,149.1849655,695.2],[-35.3691261,149.1852089,695.2],[-35.3691914,149.1854405,695.2],[-35.3692931,149.185653,695.1],[-35.3694259,149.1858368,695.2],[-35.3695859,149.1859864,695.1],[-35.3697653,149.1860942,695.2],[-35.3699597,149.1861575,695.1],[-35.3701589,149.1861732,695.2],[-35.3703583,149.186141,695.2],[-35.3705469,149.1860623,695.2],[-35.3707215,149.1859388,695.1],[-35.3708713,149.185778,695.2],[-35.3709945,149.1855821,695.1],[-35.3710835,149.1853631,695.2],[-35.3711366,149.1851246,695.2],[-35.3711481,149.1848803,695.1],[-35.3710974,149.1846379,695.6],[-35.3710169,149.1844151,695.3],[-35.3709174,149.1842045,695.1],[-35.3708092,149.1840031,695.0],[-35.3706949,149.1838037,694.9],[-35.3705803,149.1836046,694.9],[-35.3704802,149.1833912,695.0],[-35.3704072,149.1831616,695.1],[-35.3703645,149.1829198,695.1],[-35.3703503,149.1826743,695.2],[-35.3703585,149.1824287,695.1],[-35.3703831,149.1821851,695.1],[-35.3704178,149.1819444,695.1],[-35.3704578,149.1817039,695.1],[-35.3704992,149.1814646,695.1],[-35.3705399,149.1812237,695.1],[-35.3705781,149.1809836,695.1],[-35.3706139,149.1807408,695.1],[-35.3706466,149.1804998,695.1],[-35.3706775,149.1802557,695.2],[-35.3707062,149.1800136,695.1],[-35.3707341,149.1797687,695.1],[-35.3707614,149.1795262,695.1],[-35.3707888,149.179281,695.2],[-35.3708161,149.1790386,695.2],[-35.3708435,149.1787936,695.2],[-35.3708608,149.1785495,695.2],[-35.3708405,149.1783015,695.2],[-35.3707604,149.1780704,695.6],[-35.3706372,149.1778698,695.7],[-35.3705096,149.1776845,695.3],[-35.3703969,149.177484,695.1],[-35.3703182,149.1772584,695.1],[-35.3702865,149.1770119,695.3],[-35.3703121,149.1767649,695.5],[-35.370395,149.1765376,695.6],[-35.3705271,149.1763512,695.6],[-35.370696,149.1762185,695.5],[-35.3708879,149.1761473,695.3],[-35.3710879,149.1761387,695.1],[-35.3712841,149.176191,695.0],[-35.3714637,149.1762972,695.0],[-35.3716196,149.1764522,694.9],[-35.3717419,149.1766459,695.0],[-35.3718257,149.1768702,695.0],[-35.3718649,149.1771096,695.0],[-35.3718585,149.1773557,695.0],[-35.3718064,149.1775916,695.1],[-35.3717096,149.1778086,695.1],[-35.3715754,149.1779894,695.2],[-35.3714083,149.1781277,695.1],[-35.3712218,149.1782126,695.2],[-35.3710219,149.1782415,695.2],[-35.3708242,149.1782121,695.2],[-35.370636,149.1781259,695.2],[-35.3704712,149.1779893,695.2],[-35.3703352,149.1778075,695.2],[-35.3702271,149.1776036,695.1],[-35.3701251,149.1773959,694.9],[-35.3700271,149.1771857,694.8],[-35.3699306,149.1769725,694.8],[-35.3698346,149.1767589,694.8],[-35.3697379,149.1765444,694.8],[-35.3696401,149.1763306,694.9],[-35.3695412,149.1761176,694.9],[-35.3694412,149.1759049,694.9],[-35.3693406,149.1756929,695.0],[-35.3692391,149.1754808,694.9],[-35.3691379,149.1752695,695.0],[-35.3690359,149.175057,695.0],[-35.3689344,149.1748456,695.0],[-35.3688321,149.1746325,695.0],[-35.3687311,149.1744214,695.0],[-35.3686293,149.1742081,695.0],[-35.3685287,149.1739972,695.0],[-35.368427,149.1737837,695.0],[-35.3683262,149.173573,695.0],[-35.3682238,149.1733597,695.0],[-35.3681225,149.1731489,695.0],[-35.3680238,149.1729339,695.0],[-35.3679511,149.1727041,695.0],[-35.3679319,149.1724524,695.3],[-35.3679714,149.1722064,695.7],[-35.3680331,149.1719728,695.3],[-35.3680829,149.1717395,695.0],[-35.3681003,149.1714942,694.9],[-35.3680697,149.171249,695.0],[-35.3679845,149.1710215,695.2],[-35.3678496,149.170835,695.3],[-35.3676762,149.1707045,695.3],[-35.3674812,149.170641,695.3],[-35.3672802,149.1706448,695.1],[-35.3670874,149.1707134,694.9],[-35.3669152,149.1708391,694.8],[-35.3667719,149.1710124,694.7],[-35.3666654,149.1712207,694.7],[-35.3666004,149.1714546,694.7],[-35.3665808,149.1716981,694.8],[-35.366607,149.1719427,694.8],[-35.3666781,149.1721718,694.8],[-35.3667917,149.1723762,694.8],[-35.3669396,149.1725401,694.9],[-35.3671162,149.1726583,694.9],[-35.3673089,149.172722,694.9],[-35.3675108,149.1727287,694.9],[-35.367706,149.1726777,695.0],[-35.3678862,149.1725673,695.1],[-35.3680256,149.1723906,696.3],[-35.3681335,149.1721876,697.7],[-35.3682216,149.1719783,699.8],[-35.368302,149.1717625,702.2],[-35.3683828,149.1715474,704.8],[-35.3684833,149.1713403,707.7],[-35.3686088,149.1711547,710.8],[-35.368757,149.1709939,714.1],[-35.3689205,149.1708604,717.6],[-35.3690936,149.1707493,721.2],[-35.3692725,149.1706541,724.9],[-35.3694549,149.1705683,728.7],[-35.3696392,149.170487,732.6],[-35.3698235,149.1704067,736.6],[-35.3700084,149.1703242,740.5],[-35.3701923,149.1702393,744.7],[-35.3703764,149.1701511,748.7],[-35.3705581,149.1700606,752.8],[-35.3707406,149.1699671,756.8],[-35.3709214,149.1698725,760.9],[-35.371104,149.1697766,764.9],[-35.3712843,149.1696814,769.0],[-35.3714665,149.1695847,773.1],[-35.3716462,149.169489,777.1],[-35.3718283,149.1693918,781.2],[-35.3720085,149.1692955,785.3],[-35.3721907,149.1691984,789.3],[-35.3723757,149.1691188,793.8],[-35.3725731,149.1690906,798.6],[-35.3727679,149.1691422,804.0],[-35.3729437,149.1692579,809.1],[-35.3731005,149.1693967,813.9],[-35.3732549,149.1695364,818.9],[-35.3734185,149.1696599,824.0],[-35.3735986,149.1697618,827.8],[-35.3737953,149.1698249,830.5],[-35.3740024,149.1698363,832.5],[-35.3742069,149.169791,833.9],[-35.3743974,149.1696899,834.8],[-35.3745646,149.1695431,835.3],[-35.374703,149.16936,835.6],[-35.3748113,149.1691491,835.6],[-35.3748881,149.1689212,835.7],[-35.3749344,149.1686793,835.6],[-35.3749495,149.1684332,835.7],[-35.3749318,149.1681856,835.4],[-35.37486,149.1679511,834.8],[-35.3747585,149.1677335,832.5],[-35.374639,149.1675357,829.8],[-35.3745079,149.1673469,826.9],[-35.3743729,149.1671667,823.9],[-35.3742336,149.1669881,820.8],[-35.3740939,149.1668135,817.8],[-35.3739589,149.1666306,814.8],[-35.3738451,149.1664293,811.7],[-35.3737576,149.1662062,808.6],[-35.3736994,149.1659724,805.2],[-35.3736663,149.16573,801.5],[-35.3736538,149.1654878,797.7],[-35.3736555,149.1652444,793.7],[-35.3736655,149.1650026,789.6],[-35.3736791,149.1647593,785.5],[-35.3736928,149.1645165,781.4],[-35.3737048,149.1642732,777.3],[-35.3737142,149.1640299,773.1],[-35.3737208,149.1637869,769.0],[-35.3737246,149.1635434,764.8],[-35.3737264,149.1632999,760.7],[-35.373727,149.1630554,756.5],[-35.3737269,149.1628128,752.4],[-35.3737263,149.1625691,748.2],[-35.3737256,149.1623263,744.0],[-35.3737248,149.1620824,739.9],[-35.3737238,149.1618408,735.7],[-35.3737225,149.1615967,731.5],[-35.3737067,149.161356,727.0],[-35.3736485,149.161119,722.2],[-35.3735325,149.1609143,717.7],[-35.3733757,149.1607561,712.9],[-35.3732118,149.1606329,707.8],[-35.3730481,149.1605147,702.6],[-35.3728935,149.1603826,697.7],[-35.3727522,149.1602195,693.3],[-35.3726406,149.1600217,690.4],[-35.3725621,149.1597987,688.5],[-35.372487,149.159579,686.8],[-35.3723855,149.1593723,685.8],[-35.3722347,149.15921,685.4],[-35.3720347,149.1591442,685.7],[-35.3718323,149.1592089,686.4],[-35.3716781,149.159377,686.5],[-35.3716016,149.1596039,686.2],[-35.3715908,149.1598406,685.1],[-35.371634,149.1600702,684.2],[-35.3717143,149.1602854,683.7],[-35.371824,149.1604855,683.5],[-35.3719578,149.1606634,683.6],[-35.3721153,149.1608145,683.8],[-35.3722918,149.1609278,684.1],[-35.3724854,149.1609968,684.3],[-35.3726851,149.161013,684.5],[-35.3728845,149.1609724,684.7],[-35.3730683,149.1608761,684.8],[-35.3732293,149.1607264,684.9],[-35.3733547,149.1605361,685.0],[-35.3734398,149.160312,685.0],[-35.3734779,149.1600717,685.0],[-35.3734681,149.1598253,685.0],[-35.3734116,149.1595911,685.0],[-35.3733108,149.1593774,685.0],[-35.373173,149.1592005,685.1],[-35.3730043,149.1590673,685.1],[-35.3728161,149.158987,685.1],[-35.3726162,149.1589627,685.1],[-35.3724186,149.1589967,685.1],[-35.372232,149.1590863,685.1],[-35.3720685,149.1592266,685.1],[-35.3719358,149.1594103,685.1],[-35.371842,149.1596274,685.1],[-35.3717922,149.1598653,685.1],[-35.3717894,149.160111,685.1],[-35.3718334,149.1603494,685.2],[-35.3719225,149.1605695,685.1],[-35.3720509,149.1607573,685.2],[-35.3722124,149.1609039,685.1],[-35.3723958,149.1609989,685.2],[-35.3725938,149.161039,685.2],[-35.3727924,149.1610213,685.2],[-35.372984,149.1609462,685.1],[-35.3731542,149.1608194,685.2],[-35.373297,149.1606451,685.1],[-35.3734009,149.1604369,685.2],[-35.373463,149.1602018,685.1],[-35.3734779,149.1599582,685.2],[-35.3734456,149.159715,685.2],[-35.3733683,149.1594903,685.2],[-35.3732487,149.1592919,685.1],[-35.373096,149.1591346,685.2],[-35.3729104,149.1590362,685.3],[-35.3727135,149.1589975,685.3],[-35.3725163,149.1589913,685.1],[-35.3723205,149.1590032,684.9],[-35.3721239,149.1590241,684.9],[-35.3719261,149.1590316,684.9],[-35.3717275,149.159007,685.0],[-35.3715336,149.1589453,685.1],[-35.3713488,149.1588502,685.2],[-35.3711753,149.1587288,685.2],[-35.3710124,149.1585897,685.2],[-35.3708562,149.1584383,685.1],[-35.3707045,149.1582811,685.1],[-35.3705527,149.1581202,685.1],[-35.3704017,149.1579605,685.1],[-35.3702482,149.1578011,685.1],[-35.3700945,149.1576458,685.1],[-35.3699371,149.1574914,685.1],[-35.3697799,149.1573409,685.2],[-35.3696199,149.1571905,685.1],[-35.369461,149.1570431,685.1],[-35.3692996,149.1568947,685.1],[-35.3691396,149.1567482,685.1],[-35.3689782,149.1566,685.1],[-35.3688182,149.156453,685.1],[-35.3686571,149.156305,685.2],[-35.3684974,149.156158,685.2],[-35.3683366,149.1560093,685.2],[-35.3681772,149.1558615,685.2],[-35.3680171,149.1557129,685.2],[-35.3678576,149.1555649,685.2],[-35.3676969,149.1554165,685.2],[-35.3675038,149.1553337,685.4],[-35.3672952,149.1553844,687.0],[-35.3671187,149.1555144,686.9],[-35.3669975,149.1557102,686.5],[-35.3669298,149.1559368,685.7],[-35.3669031,149.1561728,685.0],[-35.3668967,149.1564121,684.5],[-35.3668935,149.1566518,684.3],[-35.3668829,149.1568953,684.3],[-35.3668604,149.1571378,684.4],[-35.3668247,149.1573804,684.6],[-35.366778,149.1576175,684.7],[-35.3667219,149.1578539,684.8],[-35.3666599,149.1580861,684.9],[-35.3665935,149.1583194,684.9],[-35.3665259,149.1585486,685.0],[-35.3664571,149.1587802,685.0],[-35.3663894,149.1590097,685.0],[-35.3663216,149.1592424,685.1],[-35.3662556,149.1594735,685.1],[-35.3661901,149.1597067,685.1],[-35.3661262,149.1599379,685.1],[-35.3660624,149.1601718,685.2],[-35.3659995,149.1604044,685.1],[-35.3659366,149.1606384,685.1],[-35.365874,149.1608715,685.1],[-35.3658113,149.1611051,685.2],[-35.3657483,149.1613381,685.2],[-35.3657206,149.1615838,685.0],[-35.3658169,149.1618176,686.9],[-35.3659775,149.1619843,687.4],[-35.3661755,149.1620462,687.3],[-35.3663712,149.1620084,686.7],[-35.3665424,149.1618939,685.7],[-35.3666879,149.1617407,684.8],[-35.3668249,149.1615731,684.2],[-35.3669672,149.1614096,684.0],[-35.3671239,149.1612575,684.1],[-35.3672912,149.1611254,684.3],[-35.3674698,149.1610117,684.4],[-35.3676526,149.1609164,684.5],[-35.3678419,149.1608329,684.6],[-35.3680314,149.1607591,684.7],[-35.3682243,149.1606892,684.8],[-35.3684154,149.1606219,684.8],[-35.3686082,149.1605538,684.9],[-35.3687994,149.1604847,685.0],[-35.3689918,149.1604129,685.0],[-35.3691823,149.1603397,685.1],[-35.3693734,149.1602644,685.1],[-35.3695629,149.1601883,685.1],[-35.3697534,149.1601104,685.1],[-35.3699432,149.1600322,685.1],[-35.3701333,149.1599534,685.1],[-35.3703227,149.1598744,685.1],[-35.3705129,149.159795,685.2],[-35.3707028,149.1597153,685.2],[-35.3708685,149.1595712,685.3],[-35.3709492,149.1593283,687.1],[-35.370955,149.1590749,687.1],[-35.3708777,149.158846,686.8],[-35.3707464,149.1586675,686.1],[-35.3705836,149.1585369,685.2],[-35.3704098,149.1584357,684.6],[-35.3702304,149.1583383,684.2],[-35.3700537,149.1582318,684.2],[-35.3698806,149.1581078,684.3],[-35.3697173,149.1579686,684.4],[-35.369561,149.1578136,684.5],[-35.3694137,149.1576494,684.7],[-35.3692705,149.1574762,684.8],[-35.3691327,149.1573004,684.8],[-35.3689956,149.1571204,684.9],[-35.3688602,149.1569417,685.0],[-35.3687227,149.1567616,685.0],[-35.3685856,149.156584,685.0],[-35.3684465,149.1564064,685.1],[-35.368307,149.1562312,685.1],[-35.368166,149.1560562,685.1],[-35.3680247,149.1558823,685.1],[-35.3678824,149.1557085,685.1],[-35.3677401,149.1555352,685.1],[-35.3675868,149.1553767,685.0],[-35.3673788,149.1553404,686.6],[-35.3671751,149.1554077,687.1],[-35.367016,149.1555649,687.1],[-35.3669179,149.1557752,686.5],[-35.3668755,149.1560085,685.6],[-35.3668698,149.1562454,684.8],[-35.3668775,149.1564846,684.3],[-35.3668824,149.1567246,684.2],[-35.3668755,149.1569694,684.2],[-35.3668533,149.1572115,684.4],[-35.3668157,149.157454,684.5],[-35.3667663,149.1576905,684.6],[-35.366707,149.1579263,684.7],[-35.3666423,149.1581572,684.8],[-35.3665733,149.1583894,684.8],[-35.3665038,149.1586188,684.9],[-35.366434,149.1588504,685.0],[-35.3663657,149.1590801,685.0],[-35.3662978,149.159312,685.0],[-35.366232,149.1595431,685.1],[-35.366167,149.1597763,685.1],[-35.3661033,149.1600087,685.1],[-35.36604,149.160242,685.2],[-35.3659775,149.1604745,685.1],[-35.3659149,149.1607085,685.2],[-35.3658527,149.1609422,685.1],[-35.3657905,149.1611757,685.1],[-35.3657295,149.1614097,685.1],[-35.3657384,149.1616598,685.5],[-35.3658592,149.1618778,687.1],[-35.3660349,149.1620143,687.4],[-35.3662365,149.1620432,687.2],[-35.3664252,149.1619792,686.4],[-35.3665878,149.1618495,685.4],[-35.3667285,149.1616902,684.6],[-35.3668668,149.1615223,684.1],[-35.3670127,149.1613629,684.0],[-35.3671734,149.1612171,684.1],[-35.3673438,149.1610915,684.3],[-35.3675248,149.1609835,684.4],[-35.3677093,149.1608923,684.6],[-35.3678988,149.1608119,684.7],[-35.3680889,149.1607397,684.8],[-35.368282,149.1606709,684.8],[-35.3684739,149.1606039,684.9],[-35.368667,149.1605356,685.0],[-35.3688579,149.1604661,685.0],[-35.36905,149.1603936,685.0],[-35.3692403,149.1603195,685.1],[-35.3694309,149.1602432,685.1],[-35.3696204,149.160166,685.1],[-35.3698105,149.1600876,685.1],[-35.3700005,149.1600086,685.1],[-35.3701905,149.1599294,685.2],[-35.3703805,149.1598504,685.2],[-35.3705698,149.159772,685.2],[-35.3707587,149.1596891,685.2],[-35.3709077,149.1595155,685.8],[-35.3709684,149.1592628,687.2],[-35.3709384,149.1590111,687.5],[-35.3708176,149.1588093,687.4],[-35.3706384,149.1587024,687.0],[-35.3704416,149.1586914,686.0],[-35.3702561,149.158747,684.9],[-35.3700796,149.1588436,684.1],[-35.3699104,149.1589587,683.8],[-35.3697331,149.159068,683.8],[-35.3695462,149.1591505,683.9],[-35.3693484,149.1592004,684.2],[-35.3691492,149.159217,684.3],[-35.368948,149.1592063,684.5],[-35.3687508,149.159175,684.6],[-35.3685538,149.1591301,684.6],[-35.3683597,149.1590783,684.8],[-35.3681646,149.1590233,684.8],[-35.3679701,149.1589686,684.9],[-35.3677741,149.1589154,685.0],[-35.3675786,149.1588656,685.0],[-35.3673822,149.158819,685.1],[-35.3671857,149.1587753,685.1],[-35.3669884,149.1587336,685.1],[-35.3667909,149.1586934,685.1],[-35.3665935,149.1586542,685.2],[-35.3663957,149.1586155,685.2],[-35.3661984,149.1585769,685.2],[-35.3659998,149.1585381,685.2],[-35.3658024,149.1584992,685.1],[-35.3656036,149.1584595,685.1],[-35.3654069,149.1584201,685.2],[-35.3652081,149.1583801,685.2],[-35.365011,149.1583402,685.2],[-35.3648121,149.1582993,685.2],[-35.3646155,149.1582588,685.2],[-35.3644163,149.1582179,685.2],[-35.3642192,149.1581779,685.2],[-35.3640205,149.1581375,685.2],[-35.3638237,149.1580974,685.2],[-35.3636251,149.1580572,685.2],[-35.3634281,149.1580174,685.1],[-35.3632294,149.1579772,685.2],[-35.3630323,149.1579373,685.2],[-35.3628341,149.157897,685.2],[-35.3626367,149.1578571,685.2],[-35.362438,149.1578172,685.2],[-35.3622405,149.1577771,685.1],[-35.3620425,149.1577368,685.2],[-35.3618451,149.1576965,685.2],[-35.3616476,149.1576561,685.2],[-35.3614495,149.1576157,685.1],[-35.3612518,149.157575,685.2],[-35.3610533,149.1575344,685.2],[-35.3608561,149.1574942,685.2],[-35.3606578,149.1574537,685.2],[-35.3604606,149.1574135,685.2],[-35.360261,149.1573834,684.5],[-35.3600581,149.1574181,682.8],[-35.3598655,149.1575132,680.5],[-35.3597001,149.1576566,679.0],[-35.3595539,149.1578241,677.7],[-35.3594209,149.1579999,676.7],[-35.3592892,149.1581811,676.0],[-35.3591566,149.1583589,675.6],[-35.3590176,149.1585346,675.3],[-35.3588747,149.158703,675.1],[-35.3587256,149.1588673,675.0],[-35.3585738,149.1590251,675.0],[-35.3584176,149.1591808,675.0],[-35.3582612,149.1593327,674.9],[-35.3581028,149.1594841,675.0],[-35.3579447,149.1596344,675.0],[-35.3577863,149.1597851,675.0],[-35.3576286,149.1599361,675.0],[-35.3574707,149.1600886,675.0],[-35.3573133,149.1602415,675.0],[-35.3571565,149.1603945,675.1],[-35.3569998,149.1605479,675.1],[-35.3568433,149.1607014,675.1],[-35.3566867,149.1608555,675.1],[-35.3565311,149.1610089,675.1],[-35.3563748,149.1611632,675.1],[-35.3562191,149.1613167,675.1],[-35.3560618,149.1614716,675.1],[-35.3558908,149.161601,674.9],[-35.3556767,149.1615897,676.8],[-35.355489,149.1614721,677.4],[-35.3553671,149.1612664,677.5],[-35.3553354,149.1610247,677.1],[-35.3553844,149.1607898,676.1],[-35.3554847,149.1605895,675.1],[-35.3556128,149.1604129,674.2],[-35.3557435,149.1602387,673.8],[-35.3558653,149.1600472,673.8],[-35.3559682,149.1598386,674.0],[-35.3560509,149.1596142,674.2],[-35.3561131,149.1593823,674.4],[-35.3561593,149.1591432,674.5],[-35.3561933,149.1589033,674.7],[-35.35622,149.1586613,674.8],[-35.356243,149.1584192,674.8],[-35.3562651,149.1581756,674.9],[-35.356288,149.1579326,674.9],[-35.3563127,149.1576894,675.0],[-35.3563396,149.157446,675.0],[-35.3563685,149.1572042,675.1],[-35.3563996,149.1569612,675.1],[-35.3564317,149.1567192,675.1],[-35.3564651,149.1564759,675.1],[-35.3564991,149.1562349,675.1],[-35.3565333,149.1559918,675.2],[-35.356567,149.1557506,675.2],[-35.3566058,149.1555085,675.4],[-35.3567323,149.1553152,678.1],[-35.3569245,149.1552207,681.7],[-35.3571216,149.1551934,682.8],[-35.3573159,149.1552483,683.5],[-35.3574904,149.1553579,683.7],[-35.3576489,149.1555024,683.7],[-35.3577971,149.1556604,683.8],[-35.357947,149.1558221,684.0],[-35.3581007,149.1559772,684.3],[-35.3582623,149.1561239,684.6],[-35.3584285,149.156259,684.8],[-35.3586009,149.1563849,685.0],[-35.3587764,149.1565017,685.0],[-35.3589552,149.1566129,685.1],[-35.3591351,149.1567197,685.2],[-35.3593157,149.1568244,685.2],[-35.3594966,149.1569284,685.2],[-35.3596778,149.1570327,685.2],[-35.3598588,149.1571384,685.2],[-35.3600335,149.1572581,684.3],[-35.3601521,149.1574746,683.5],[-35.3601894,149.1577327,681.8],[-35.3601258,149.1579764,680.6],[-35.3599991,149.1581627,679.0],[-35.3598352,149.1582884,677.2],[-35.3596563,149.1583693,675.8],[-35.35947,149.158435,674.8],[-35.3592845,149.1585059,674.4],[-35.3590992,149.1585946,674.3],[-35.3589213,149.1587009,674.3],[-35.3587483,149.1588265,674.4],[-35.3585843,149.1589643,674.5],[-35.3584247,149.1591136,674.6],[-35.358271,149.1592675,674.7],[-35.3581181,149.1594265,674.8],[-35.3579672,149.1595859,674.8],[-35.3578152,149.1597468,674.9],[-35.3576635,149.1599057,674.9],[-35.3575099,149.1600637,675.0],[-35.3573559,149.1602191,675.0],[-35.3571998,149.160374,675.1],[-35.3570438,149.1605273,675.1],[-35.3568872,149.1606803,675.1],[-35.3567305,149.1608328,675.1],[-35.3565733,149.1609856,675.1],[-35.3564162,149.1611384,675.1],[-35.3562596,149.161291,675.2],[-35.3561025,149.1614446,675.2],[-35.3559397,149.1615862,675.0],[-35.3557308,149.1616091,676.4],[-35.3555331,149.1615144,677.3],[-35.3553931,149.1613258,677.5],[-35.3553405,149.1610883,677.2],[-35.3553745,149.1608483,676.5],[-35.3554654,149.1606406,675.4],[-35.3555893,149.1604592,674.4],[-35.355719,149.1602855,673.9],[-35.3558419,149.1600963,673.8],[-35.3559468,149.1598899,673.9],[-35.3560317,149.1596657,674.2],[-35.3560957,149.1594343,674.4],[-35.356143,149.1591955,674.5],[-35.3561778,149.1589561,674.6],[-35.3562052,149.1587127,674.7],[-35.3562285,149.1584704,674.8],[-35.3562507,149.1582266,674.8],[-35.3562733,149.1579839,674.9],[-35.3562979,149.15774,675.0],[-35.3563246,149.1574968,675.0],[-35.3563539,149.1572537,675.1],[-35.356385,149.1570115,675.1],[-35.3564176,149.15677,675.1],[-35.3564515,149.1565281,675.1],[-35.3564859,149.1562864,675.1],[-35.3565205,149.1560437,675.1],[-35.3565549,149.1558023,675.1],[-35.3565895,149.1555597,675.1],[-35.3566833,149.1553388,675.1],[-35.3568704,149.1552095,676.9],[-35.3570753,149.1551743,676.9],[-35.3572703,149.1552439,676.4],[-35.3574269,149.1553875,675.3],[-35.3575454,149.1555777,674.0],[-35.3576378,149.155785,672.8],[-35.3577263,149.1559998,671.9],[-35.3578228,149.1562094,671.4],[-35.3579347,149.1564133,670.9],[-35.3580598,149.156603,670.6],[-35.3581979,149.1567818,670.1],[-35.3583433,149.156948,669.6],[-35.3584957,149.1571072,669.2],[-35.3586509,149.1572603,668.6],[-35.358809,149.1574119,668.1],[-35.358967,149.1575618,667.5],[-35.3591252,149.1577127,667.0],[-35.3592821,149.1578642,666.4],[-35.3594385,149.1580178,665.8],[-35.3595936,149.1581727,665.2],[-35.3597477,149.1583289,664.6],[-35.3599014,149.1584862,664.0],[-35.3600542,149.1586438,663.4],[-35.3602077,149.1588028,662.8],[-35.3603603,149.1589611,662.2],[-35.3605139,149.1591206,661.6],[-35.3606659,149.1592783,661.0],[-35.3608196,149.1594373,660.4],[-35.3609724,149.1595946,659.8],[-35.3611266,149.1597535,659.1],[-35.3612793,149.1599105,658.5],[-35.3614339,149.1600692,658.0],[-35.3615867,149.1602258,657.4],[-35.3617412,149.1603842,656.8],[-35.3618941,149.1605407,656.1],[-35.3620484,149.1606987,655.5],[-35.3622009,149.1608554,654.9],[-35.3623551,149.1610137,654.3],[-35.3625083,149.161171,653.7],[-35.3626623,149.161329,653.1],[-35.3628151,149.161486,652.5],[-35.3629692,149.1616444,651.8],[-35.3631222,149.1618018,651.2],[-35.3632757,149.1619598,650.6],[-35.3634287,149.1621169,650.0],[-35.3635822,149.1622745,649.4],[-35.3637359,149.1624324,648.8],[-35.3638896,149.16259,648.2],[-35.3640435,149.1627476,647.5],[-35.3641968,149.1629045,647.0],[-35.3643506,149.1630622,646.4],[-35.3645039,149.1632193,645.8],[-35.3646579,149.1633773,645.1],[-35.3648107,149.1635345,644.5],[-35.3649647,149.1636929,643.9],[-35.3651174,149.1638504,643.3],[-35.3652718,149.1640097,642.6],[-35.3653918,149.164208,641.9],[-35.3654417,149.1644567,642.0],[-35.3654408,149.1647034,640.8],[-35.3653921,149.1649423,638.9],[-35.3653108,149.1651623,636.7],[-35.3652094,149.1653686,633.5],[-35.3650683,149.1655252,628.5],[-35.3648689,149.1656247,623.9],[-35.3646454,149.1656466,620.5],[-35.3644306,149.1656273,615.7],[-35.3642223,149.1655746,610.9],[-35.3640292,149.1655118,604.9]],"source":"ArduPilot autotest Plane.MissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot bcb5e0c597, from ArduPilot PR 34424): sitl-plane-mission-items"} \ No newline at end of file diff --git a/tests/missions/sitl-plane-rtl-rally.json b/tests/missions/sitl-plane-rtl-rally.json new file mode 100644 index 0000000000..9992bc1b62 --- /dev/null +++ b/tests/missions/sitl-plane-rtl-rally.json @@ -0,0 +1 @@ +{"home":[-35.3629381,149.165085,585.0999755859375],"items":[[22,-35.361279,149.16423,625.0999755859375,[15.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.0999755859375,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.0999755859375,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[189,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[16,-35.365467,149.164215,640.0999755859375,[0.0,0.0,0.0,0.0]],[16,-35.365009,149.165482,625.0999755859375,[0.0,0.0,0.0,0.0]],[21,-35.363041,149.165222,585.0999755859375,[0.0,0.0,0.0,1.0]]],"takeoff_course":354.0334197600823,"start":[-35.3629381,149.165085,585.0599975585938],"rally":[[-35.3683278,149.1716947,665.0999755859375]],"params":{"AIRSPEED_CRUISE":22.0,"AIRSPEED_MAX":30.0,"AIRSPEED_MIN":10.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":15.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":0.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.25,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":100.0,"RTL_RADIUS":150.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":80.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":50.0},"flown":[[-35.3629381,149.165085,585.0],[-35.3629306,149.1650841,585.0],[-35.3628442,149.165073,584.8],[-35.3626795,149.1650518,589.3],[-35.3624628,149.1650237,597.1],[-35.3622193,149.1649921,606.6],[-35.3619649,149.1649589,617.0],[-35.3617015,149.164924,627.2],[-35.3614287,149.164892,634.3],[-35.3611572,149.1648728,637.8],[-35.3609061,149.1648635,638.6],[-35.3606756,149.1648614,637.1],[-35.3604506,149.1648652,636.4],[-35.3602189,149.1648729,637.4],[-35.3599891,149.1648818,639.2],[-35.3597573,149.1648916,641.2],[-35.3595293,149.1649019,643.3],[-35.3592996,149.164912,645.5],[-35.3590734,149.1649225,647.7],[-35.3588461,149.164933,649.9],[-35.3586217,149.1649433,652.0],[-35.3583958,149.1649539,654.2],[-35.3581727,149.164964,656.3],[-35.357949,149.1649742,658.5],[-35.3577269,149.1649845,660.6],[-35.3575046,149.1649952,662.7],[-35.3572835,149.1650063,664.8],[-35.357062,149.1650178,666.9],[-35.356841,149.1650292,669.0],[-35.3566208,149.1650405,671.1],[-35.3564008,149.1650516,673.1],[-35.3561811,149.1650625,675.2],[-35.3559619,149.1650917,677.5],[-35.3557729,149.1652442,681.6],[-35.3556217,149.1654525,683.0],[-35.3555335,149.1657049,683.8],[-35.3555041,149.1659782,683.8],[-35.3555201,149.1662483,683.7],[-35.3555627,149.1665154,683.7],[-35.3556142,149.1667768,683.8],[-35.3556644,149.1670422,684.1],[-35.3557059,149.1673068,684.3],[-35.3557371,149.1675764,684.6],[-35.3557573,149.1678437,684.7],[-35.3557688,149.1681142,684.8],[-35.3557731,149.1683829,684.9],[-35.3557726,149.168654,685.0],[-35.3557691,149.168923,685.0],[-35.3557641,149.1691937,685.0],[-35.3557588,149.1694627,685.0],[-35.3557543,149.169734,685.0],[-35.3557508,149.1700035,685.0],[-35.3557485,149.1702739,685.1],[-35.3557471,149.1705433,685.1],[-35.3557465,149.1708141,685.1],[-35.3557463,149.1710842,685.1],[-35.3557466,149.1713543,685.1],[-35.3557475,149.1716243,685.0],[-35.3557485,149.1718942,685.1],[-35.3557492,149.1721651,685.1],[-35.3557496,149.1724349,685.0],[-35.3557502,149.1727063,685.0],[-35.3557506,149.1729755,685.0],[-35.3557508,149.1732469,685.0],[-35.3557509,149.1735163,685.0],[-35.3557509,149.1737881,685.0],[-35.3557509,149.1740572,685.0],[-35.3557509,149.1743289,685.0],[-35.3557509,149.1745979,685.0],[-35.3557508,149.1748702,685.0],[-35.3557506,149.1751396,685.0],[-35.3557506,149.1754111,685.0],[-35.3557507,149.1756799,685.0],[-35.3557507,149.1759517,685.0],[-35.3557506,149.1762213,685.0],[-35.3557506,149.1764928,685.0],[-35.3557506,149.176762,685.0],[-35.3557505,149.1770335,685.1],[-35.3557503,149.177303,685.0],[-35.3557503,149.177574,685.0],[-35.3557503,149.1778442,685.0],[-35.3557503,149.1781147,685.0],[-35.3557504,149.1783851,685.0],[-35.3557503,149.1786558,685.0],[-35.3557503,149.1789265,685.0],[-35.3557503,149.1791963,685.0],[-35.3557504,149.1794673,685.0],[-35.3557508,149.1797376,685.0],[-35.3557512,149.1800091,685.0],[-35.3557515,149.1802783,685.0],[-35.3557516,149.1805493,685.1],[-35.3557515,149.1808185,685.0],[-35.3557512,149.1810907,685.0],[-35.3557686,149.1813598,684.8],[-35.3559111,149.181586,686.6],[-35.3561164,149.18172,687.6],[-35.3563391,149.1817639,686.6],[-35.3565541,149.1817256,685.0],[-35.3567584,149.1816308,683.8],[-35.3569503,149.1815071,683.1],[-35.3571378,149.1813666,682.8],[-35.3573208,149.1812194,682.7],[-35.3575033,149.1810667,682.7],[-35.3576832,149.1809131,682.6],[-35.3578634,149.1807577,682.5],[-35.3580425,149.1806029,682.4],[-35.3582226,149.1804469,682.2],[-35.3584022,149.1802916,682.0],[-35.3585822,149.180136,681.8],[-35.358762,149.179981,681.5],[-35.3589427,149.179826,681.3],[-35.3591235,149.1796711,681.0],[-35.3593039,149.1795163,680.8],[-35.3594849,149.1793606,680.5],[-35.3596647,149.1792058,680.2],[-35.3598458,149.1790502,679.9],[-35.3600258,149.1788953,679.6],[-35.3602071,149.1787392,679.3],[-35.3603865,149.1785848,679.0],[-35.3605678,149.1784286,678.7],[-35.3607473,149.1782738,678.5],[-35.3609287,149.1781173,678.1],[-35.3611081,149.1779628,677.8],[-35.3612896,149.1778069,677.5],[-35.3614694,149.1776523,677.2],[-35.3616508,149.1774956,677.0],[-35.3618304,149.1773405,676.6],[-35.3620112,149.1771839,676.3],[-35.3621903,149.1770284,676.0],[-35.362371,149.1768716,675.8],[-35.3625505,149.1767159,675.5],[-35.3627309,149.1765595,675.2],[-35.3629101,149.176404,674.9],[-35.3630901,149.1762479,674.6],[-35.3632703,149.1760921,674.2],[-35.3634508,149.1759354,674.0],[-35.3636309,149.1757788,673.7],[-35.3638107,149.1756229,673.4],[-35.363991,149.1754668,673.1],[-35.3641712,149.1753112,672.8],[-35.364352,149.1751552,672.4],[-35.3645316,149.1749997,672.2],[-35.3647121,149.1748433,671.9],[-35.3648915,149.1746873,671.6],[-35.3650724,149.1745304,671.3],[-35.3652515,149.1743753,671.0],[-35.3654328,149.1742186,670.7],[-35.3656118,149.1740638,670.4],[-35.3657928,149.1739069,670.1],[-35.3659719,149.1737518,669.8],[-35.3661528,149.173595,669.5],[-35.3663315,149.1734396,669.2],[-35.3665123,149.1732825,668.9],[-35.3666914,149.1731269,668.5],[-35.3668795,149.1729828,667.3],[-35.3670901,149.1728914,666.7],[-35.3673182,149.1728869,666.7],[-35.3675342,149.1729607,666.5],[-35.3677339,149.1730713,665.7],[-35.3679271,149.17319,665.1],[-35.368127,149.1732964,664.7],[-35.3683378,149.1733725,664.7],[-35.3685577,149.1734063,664.8],[-35.3687789,149.1733895,664.9],[-35.3689926,149.17332,665.0],[-35.3691911,149.1732018,665.0],[-35.3693672,149.1730411,665.0],[-35.3695183,149.1728444,665.0],[-35.3696399,149.1726207,665.0],[-35.3697318,149.172375,664.9],[-35.3697922,149.1721182,665.0],[-35.3698222,149.1718505,664.9],[-35.369821,149.1715824,664.9],[-35.3697884,149.1713146,665.0],[-35.3697258,149.1710578,664.9],[-35.3696323,149.1708116,665.0],[-35.3695123,149.1705872,665.0],[-35.3693653,149.1703842,665.0],[-35.3691975,149.1702116,665.0],[-35.3690092,149.1700698,665.0],[-35.368808,149.1699643,665.0],[-35.3685942,149.1698955,665.0],[-35.3683762,149.1698663,665.0],[-35.3681558,149.1698771,665.0]],"source":"ArduPilot autotest Plane.MissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot bcb5e0c597, from ArduPilot PR 34424): sitl-plane-rtl-rally"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-rtl-q-rtl-mode-1.json b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-1.json new file mode 100644 index 0000000000..461d0ea851 --- /dev/null +++ b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-1.json @@ -0,0 +1 @@ +{"home":[-35.3629376,149.1650845,585.0499877929688],"items":[[84,-35.3629376,149.1650846,615.0499877929688,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.0499877929688,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.0499877929688,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]]],"heading":131.4656109106957,"start":[-35.3629376,149.1650846,585.0700073242188],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":1.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3624932,149.1662196,618.2],[-35.3622365,149.1661816,618.2],[-35.362006,149.1661368,618.2],[-35.3617507,149.166085,619.1],[-35.3614935,149.166035,620.8],[-35.3612595,149.1659919,622.9],[-35.3610033,149.1659467,625.4],[-35.3607482,149.1659029,628.1],[-35.3605176,149.1658641,630.5],[-35.3602655,149.165823,633.1],[-35.3600339,149.1657855,635.5],[-35.3597822,149.1657444,638.2],[-35.3595297,149.1657027,640.8],[-35.3593015,149.1656647,643.1],[-35.3590491,149.1656233,645.7],[-35.3587985,149.1655825,648.2],[-35.3585475,149.1655417,650.8],[-35.3582958,149.1655008,653.2],[-35.3580678,149.1654637,655.5],[-35.3578156,149.1654225,658.0],[-35.3575655,149.1653815,660.4],[-35.3573373,149.165344,662.7],[-35.3570876,149.1653028,665.1],[-35.3568591,149.1652651,667.3],[-35.3566072,149.1652237,669.8],[-35.3563546,149.1651834,672.3],[-35.356124,149.1651921,675.1],[-35.3558993,149.1653336,679.9],[-35.3557333,149.1655678,683.5],[-35.3556283,149.1658494,684.5],[-35.3555922,149.1661591,684.3],[-35.3556091,149.1664465,684.1],[-35.3556551,149.1667588,684.0],[-35.3557052,149.1670701,684.3],[-35.3557416,149.1673535,684.7],[-35.3557666,149.1676662,685.1],[-35.3557765,149.1679502,685.3],[-35.355777,149.1682627,685.5],[-35.3557711,149.1685748,685.5],[-35.3557637,149.1688583,685.5],[-35.3557563,149.16917,685.5],[-35.3557512,149.1694821,685.5],[-35.355749,149.1697939,685.4],[-35.3557488,149.1701058,685.4],[-35.3557494,149.1703898,685.3],[-35.3557506,149.1707021,685.3],[-35.355752,149.1710145,685.3],[-35.3557531,149.1712989,685.2],[-35.3557539,149.1716116,685.2],[-35.3557541,149.1718961,685.2],[-35.3557538,149.1722068,685.2],[-35.3557527,149.1725466,685.2],[-35.3557518,149.1728311,685.1],[-35.3557511,149.1731447,685.1],[-35.3557506,149.1734301,685.1],[-35.3557506,149.1737437,685.1],[-35.3557508,149.1740554,685.1],[-35.3557513,149.174339,685.1],[-35.3557518,149.1746524,685.1],[-35.355752,149.1749659,685.1],[-35.3557521,149.1752508,685.1],[-35.3557521,149.1755641,685.1],[-35.3557521,149.1758491,685.1],[-35.3557519,149.1761618,685.1],[-35.3557514,149.1764744,685.1],[-35.3557512,149.1767577,685.0],[-35.3557509,149.1770696,685.0],[-35.3557508,149.1773542,685.0],[-35.3557507,149.1776677,685.1],[-35.3557505,149.1779814,685.1],[-35.3557506,149.1782661,685.1],[-35.3557508,149.1785769,685.0],[-35.3557515,149.1788892,685.0],[-35.3557519,149.1791745,685.0],[-35.3557523,149.1794884,685.1],[-35.3557525,149.1797733,685.1],[-35.3557525,149.180086,685.1],[-35.3557522,149.1803986,685.0],[-35.355752,149.1806835,685.0],[-35.3557599,149.1809956,685.0],[-35.3558678,149.1812814,685.8],[-35.3560884,149.1814461,687.7],[-35.3563474,149.181442,689.1],[-35.3565486,149.1812987,689.3],[-35.3566774,149.1810327,688.6],[-35.3567078,149.1807283,686.8],[-35.356684,149.1804528,683.8],[-35.3566236,149.1801529,681.0],[-35.356556,149.179879,679.7],[-35.3564959,149.1795734,679.2],[-35.3564809,149.17926,679.0],[-35.3565134,149.1789794,678.8],[-35.3565935,149.1786853,678.1],[-35.3567075,149.1784081,676.9],[-35.3568428,149.1781436,675.6],[-35.3569866,149.1778859,674.3],[-35.3571176,149.1776518,673.2],[-35.3572569,149.1773927,671.9],[-35.3573901,149.1771291,670.7],[-35.3575069,149.1768846,669.4],[-35.357632,149.1766117,668.1],[-35.3577437,149.1763624,666.9],[-35.3578655,149.1760883,665.5],[-35.3579878,149.1758145,664.1],[-35.3580998,149.1755666,662.9],[-35.358224,149.1752953,661.5],[-35.3583495,149.1750239,660.1],[-35.3584755,149.1747531,658.7],[-35.3586015,149.1744819,657.3],[-35.3587162,149.174235,656.0],[-35.3588421,149.1739636,654.6],[-35.358968,149.1736923,653.3],[-35.3590823,149.1734449,652.0],[-35.3592071,149.173173,650.6],[-35.3593207,149.1729257,649.3],[-35.3594457,149.1726537,647.9],[-35.3595703,149.1723833,646.5],[-35.3596835,149.1721371,645.2],[-35.3598086,149.1718658,643.8],[-35.3599342,149.1715939,642.5],[-35.3600709,149.1712979,641.0],[-35.3601966,149.1710267,639.5],[-35.360311,149.1707798,638.3],[-35.3604366,149.1705081,636.9],[-35.3605622,149.1702358,635.5],[-35.3606874,149.169964,634.1],[-35.3608123,149.1696925,632.7],[-35.3609261,149.1694453,631.4],[-35.3610519,149.1691731,630.0],[-35.3611779,149.1689014,628.6],[-35.3612922,149.1686544,627.3],[-35.3614174,149.1683829,626.0],[-35.3615422,149.1681114,624.6],[-35.3616663,149.1678416,623.1],[-35.361791,149.1675707,621.7],[-35.3619051,149.1673234,620.5],[-35.3620308,149.167051,619.1],[-35.362145,149.1668037,617.9],[-35.3622705,149.1665322,616.5],[-35.3623888,149.1662759,616.6],[-35.3624809,149.1660763,618.9],[-35.3625671,149.1658902,619.9],[-35.3626447,149.1657225,619.1],[-35.3627097,149.1655817,618.7],[-35.3627733,149.1654433,618.9],[-35.362823,149.1653343,619.4],[-35.3628678,149.1652353,620.0],[-35.3629023,149.1651585,619.8],[-35.3629172,149.1651245,616.4],[-35.3629188,149.1651208,614.0],[-35.3629187,149.1651212,612.1],[-35.3629205,149.1651184,609.8],[-35.3629249,149.1651099,607.6]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-rtl-q-rtl-mode-1"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-rtl-q-rtl-mode-2.json b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-2.json new file mode 100644 index 0000000000..dbfe736987 --- /dev/null +++ b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-2.json @@ -0,0 +1 @@ +{"home":[-35.3629371,149.1650848,585.010009765625],"items":[[84,-35.3629371,149.1650848,615.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.010009765625,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]]],"heading":257.1149493194462,"start":[-35.3629371,149.1650848,585.0399780273438],"approach":-153.7,"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":2.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3621436,149.1641285,619.9],[-35.361899,149.1641717,620.5],[-35.3616726,149.1642121,620.3],[-35.3614178,149.1642552,621.2],[-35.3611813,149.1642928,623.0],[-35.3609215,149.1643331,625.6],[-35.360662,149.1643721,628.5],[-35.3604276,149.1644066,631.0],[-35.360171,149.1644441,633.9],[-35.3599392,149.164478,636.4],[-35.359685,149.1645154,639.1],[-35.3594319,149.1645521,641.7],[-35.3592018,149.1645855,644.1],[-35.3589489,149.1646221,646.7],[-35.3586965,149.1646585,649.2],[-35.3584685,149.1646914,651.5],[-35.3582173,149.1647279,654.0],[-35.357988,149.1647614,656.2],[-35.3577355,149.1647983,658.8],[-35.3574829,149.1648344,661.3],[-35.3572542,149.1648672,663.6],[-35.3570036,149.1649031,666.0],[-35.3567517,149.1649392,668.4],[-35.3564995,149.1649755,670.8],[-35.3562254,149.1650244,673.7],[-35.3560148,149.1651392,677.2],[-35.3558316,149.165352,681.3],[-35.3557032,149.1656197,683.5],[-35.3556397,149.1658941,684.1],[-35.3556259,149.1662078,684.4],[-35.3556468,149.1664934,684.4],[-35.3556856,149.1668063,684.5],[-35.3557235,149.1671193,684.9],[-35.3557494,149.1674045,685.2],[-35.3557661,149.1677178,685.4],[-35.3557718,149.1680024,685.5],[-35.355771,149.1683126,685.5],[-35.3557661,149.1686244,685.5],[-35.3557601,149.1689368,685.4],[-35.3557548,149.1692493,685.4],[-35.355751,149.169562,685.4],[-35.3557491,149.1698744,685.4],[-35.3557488,149.1701868,685.3],[-35.3557489,149.1704712,685.3],[-35.3557492,149.1707841,685.3],[-35.3557499,149.1710971,685.3],[-35.3557505,149.1713815,685.2],[-35.3557509,149.1716923,685.2],[-35.355751,149.1719758,685.1],[-35.355751,149.1722892,685.1],[-35.355751,149.1726028,685.1],[-35.3557507,149.172888,685.1],[-35.3557502,149.1732005,685.1],[-35.35575,149.1735137,685.1],[-35.35575,149.1738269,685.1],[-35.35575,149.17414,685.1],[-35.35575,149.1744239,685.0],[-35.3557499,149.1747348,685.0],[-35.35575,149.1750473,685.0],[-35.3557501,149.1753329,685.0],[-35.3557501,149.1756466,685.0],[-35.3557503,149.1759323,685.1],[-35.3557505,149.1762433,685.1],[-35.3557506,149.1765558,685.0],[-35.3557506,149.1768405,685.0],[-35.3557508,149.1771542,685.0],[-35.3557512,149.1774678,685.0],[-35.3557516,149.1777811,685.0],[-35.355752,149.1780922,685.0],[-35.3557519,149.1783762,685.0],[-35.3557517,149.1786885,685.0],[-35.3557511,149.1790015,685.0],[-35.3557505,149.1792856,685.0],[-35.3557501,149.1795991,685.0],[-35.3557503,149.1798814,685.0],[-35.3557507,149.1801957,685.0],[-35.355751,149.1805075,685.0],[-35.355751,149.1807927,685.0],[-35.3557741,149.1811015,685.0],[-35.355916,149.181363,686.3],[-35.3561538,149.1814881,688.3],[-35.3564085,149.181447,689.3],[-35.3566038,149.1812976,688.9],[-35.3567686,149.1810695,686.5],[-35.356895,149.1808059,683.7],[-35.3569931,149.1805495,681.9],[-35.3570964,149.1802612,680.8],[-35.357191,149.1799988,680.3],[-35.3572965,149.1797123,679.6],[-35.3574035,149.1794281,678.8],[-35.3575015,149.1791707,677.8],[-35.3576098,149.1788891,676.7],[-35.3577181,149.1786087,675.4],[-35.3578266,149.1783277,674.1],[-35.3579353,149.1780457,672.8],[-35.3580342,149.1777887,671.6],[-35.3581427,149.1775057,670.3],[-35.358251,149.1772235,668.9],[-35.35835,149.176966,667.6],[-35.3584686,149.1766573,666.1],[-35.3585673,149.1764012,664.9],[-35.3586753,149.1761206,663.5],[-35.3587838,149.175839,662.0],[-35.3588827,149.1755822,660.7],[-35.3589916,149.1752999,659.3],[-35.3590909,149.1750432,658.1],[-35.3592001,149.1747606,656.7],[-35.359309,149.1744782,655.3],[-35.3594082,149.1742214,654.1],[-35.3595171,149.1739391,652.6],[-35.3596262,149.1736572,651.3],[-35.3597257,149.1734003,650.0],[-35.3598349,149.1731186,648.6],[-35.3599338,149.1728636,647.3],[-35.3600425,149.1725827,645.9],[-35.3601514,149.1723011,644.5],[-35.3602509,149.1720442,643.3],[-35.3603602,149.171762,641.9],[-35.3604596,149.171505,640.6],[-35.3605688,149.1712227,639.2],[-35.3606784,149.1709401,637.8],[-35.3607777,149.1706843,636.5],[-35.3608865,149.1704038,635.2],[-35.3609954,149.1701228,633.7],[-35.3610948,149.1698661,632.5],[-35.3612043,149.1695833,631.1],[-35.3613038,149.169326,629.9],[-35.3614127,149.1690442,628.5],[-35.3615211,149.1687635,627.1],[-35.3616195,149.1685082,625.8],[-35.3617287,149.1682255,624.3],[-35.361838,149.1679433,623.0],[-35.3619471,149.1676621,621.6],[-35.3620558,149.1673814,620.3],[-35.3621546,149.1671254,619.0],[-35.3622631,149.1668435,617.6],[-35.3623715,149.1665615,616.2],[-35.3624708,149.1663051,614.9],[-35.3625955,149.1660337,612.4],[-35.3627489,149.1658192,609.4],[-35.3629759,149.1656652,606.6],[-35.3632201,149.1655655,604.4],[-35.3634139,149.1654142,602.8],[-35.3635465,149.1651503,602.4],[-35.363565,149.1648406,602.6],[-35.3634744,149.1645534,602.2],[-35.3632939,149.1643404,601.3],[-35.3630797,149.1642442,600.5],[-35.36283,149.164255,599.8],[-35.3625997,149.1643782,599.3],[-35.3624327,149.1645725,598.9],[-35.3623212,149.1648521,598.9],[-35.3622992,149.1651341,599.0],[-35.3623665,149.1654326,599.3],[-35.3625262,149.1656723,599.4],[-35.3627484,149.1657541,600.5],[-35.3629924,149.1656632,602.2],[-35.3631649,149.1654552,602.7],[-35.3632765,149.1652242,602.0],[-35.3633706,149.1650232,603.2],[-35.3634469,149.1648672,604.0],[-35.363526,149.1647244,604.8],[-35.3635995,149.164612,605.9],[-35.3636581,149.1645348,606.8],[-35.3637111,149.1644745,606.9],[-35.3637375,149.1644481,603.6],[-35.363737,149.1644572,601.1],[-35.3637292,149.164479,600.1],[-35.3637024,149.1645026,599.7],[-35.3636544,149.1645385,600.0],[-35.3635988,149.1645804,600.2],[-35.3635449,149.1646226,600.2],[-35.3634955,149.164662,600.0],[-35.3634547,149.1646946,600.0],[-35.3634132,149.1647271,600.0],[-35.3633735,149.1647575,600.0],[-35.3633372,149.1647847,600.0],[-35.3632966,149.1648147,600.0],[-35.3632588,149.1648427,600.0],[-35.3632164,149.164874,600.0],[-35.3631737,149.1649059,600.0],[-35.3631348,149.164935,600.0],[-35.3630927,149.1649667,600.0],[-35.3630542,149.1649959,600.0],[-35.3630216,149.1650206,600.0],[-35.3629931,149.1650422,600.0],[-35.3629715,149.1650586,600.0],[-35.3629563,149.1650701,600.0]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-rtl-q-rtl-mode-2"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-rtl-q-rtl-mode-3.json b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-3.json new file mode 100644 index 0000000000..a5035c3d16 --- /dev/null +++ b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-3.json @@ -0,0 +1 @@ +{"home":[-35.3629365,149.1650852,585.1199951171875],"items":[[84,-35.3629365,149.1650852,615.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.1199951171875,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]]],"heading":22.350890943519392,"start":[-35.3629365,149.1650852,585.0800170898438],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":3.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":60.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3615629,149.1652298,625.1],[-35.3613209,149.1652224,625.8],[-35.361093,149.1652165,625.5],[-35.3608337,149.1652108,626.5],[-35.3605697,149.1652042,628.7],[-35.360329,149.1651976,631.2],[-35.3600658,149.1651902,634.1],[-35.3598046,149.165183,637.0],[-35.3595471,149.1651764,639.9],[-35.3592911,149.1651703,642.6],[-35.3590583,149.1651646,645.0],[-35.358803,149.1651586,647.7],[-35.3585483,149.1651524,650.3],[-35.3583171,149.1651469,652.6],[-35.3580631,149.1651409,655.2],[-35.357832,149.1651355,657.5],[-35.3575782,149.1651294,660.0],[-35.357324,149.1651232,662.6],[-35.357093,149.1651173,664.9],[-35.3568393,149.1651111,667.4],[-35.3565859,149.1651053,669.9],[-35.3563549,149.1651001,672.2],[-35.3561071,149.1651461,675.4],[-35.3559114,149.1652929,679.7],[-35.355747,149.1655291,683.0],[-35.3556423,149.1658136,684.2],[-35.3556061,149.1660964,684.4],[-35.3556187,149.1664135,684.3],[-35.3556557,149.1666986,684.3],[-35.3557022,149.1670108,684.5],[-35.35574,149.1673234,684.9],[-35.3557622,149.1676077,685.2],[-35.3557736,149.1679182,685.5],[-35.3557747,149.1682302,685.6],[-35.3557708,149.1685136,685.6],[-35.3557642,149.1688253,685.6],[-35.3557583,149.1691085,685.6],[-35.3557531,149.1694211,685.6],[-35.3557501,149.1697332,685.5],[-35.3557494,149.1700172,685.5],[-35.3557499,149.1703293,685.4],[-35.3557512,149.1706419,685.4],[-35.3557529,149.1709542,685.4],[-35.3557541,149.1712649,685.4],[-35.3557546,149.1715487,685.3],[-35.3557546,149.1718612,685.3],[-35.355754,149.1721741,685.3],[-35.3557533,149.1724585,685.3],[-35.3557523,149.1727713,685.3],[-35.3557516,149.1730562,685.2],[-35.355751,149.1733697,685.2],[-35.3557509,149.173712,685.2],[-35.3557508,149.173996,685.2],[-35.3557509,149.1743072,685.2],[-35.3557511,149.1745918,685.2],[-35.3557511,149.1749051,685.2],[-35.3557512,149.1752186,685.2],[-35.3557513,149.1755038,685.2],[-35.3557514,149.1758156,685.1],[-35.3557518,149.1761268,685.1],[-35.355752,149.1764113,685.1],[-35.3557522,149.1767251,685.1],[-35.3557521,149.1770105,685.1],[-35.3557517,149.1773238,685.2],[-35.3557512,149.1776351,685.1],[-35.3557507,149.1779188,685.1],[-35.3557504,149.1782312,685.1],[-35.3557502,149.1785444,685.1],[-35.3557502,149.1788578,685.1],[-35.3557505,149.1791713,685.1],[-35.355751,149.1794563,685.1],[-35.3557516,149.1797697,685.1],[-35.3557519,149.1800834,685.1],[-35.355752,149.1803669,685.1],[-35.355752,149.1806784,685.1],[-35.3557574,149.1809632,685.1],[-35.3558541,149.1812557,685.7],[-35.3560694,149.1814333,687.6],[-35.3563056,149.181448,689.1],[-35.3565389,149.1813234,689.3],[-35.3567208,149.1811157,687.5],[-35.3568604,149.1808644,684.6],[-35.356975,149.1805874,682.5],[-35.3570714,149.1803268,681.6],[-35.357176,149.1800389,681.2],[-35.3572805,149.179754,680.8],[-35.3573767,149.1794954,680.4],[-35.357483,149.1792122,679.8],[-35.3575801,149.1789547,679.2],[-35.3576872,149.1786718,678.4],[-35.3577948,149.178389,677.5],[-35.3578927,149.1781322,676.7],[-35.3580003,149.1778498,675.8],[-35.3581081,149.1775674,674.8],[-35.3582165,149.1772851,673.8],[-35.3583242,149.1770049,672.7],[-35.3584216,149.1767518,671.6],[-35.3585289,149.1764732,670.5],[-35.3586359,149.1761958,669.7],[-35.3587323,149.1759458,669.0],[-35.358837,149.1756743,668.2],[-35.3589312,149.1754303,667.5],[-35.3590342,149.1751631,666.7],[-35.3591363,149.1748983,665.8],[-35.3592284,149.1746597,665.1],[-35.3593287,149.1744002,664.2],[-35.3594184,149.1741683,663.3],[-35.359516,149.1739154,662.4],[-35.359613,149.173664,661.4],[-35.3597008,149.1734364,660.4],[-35.3597966,149.1731874,659.5],[-35.3598917,149.1729406,658.5],[-35.3599775,149.1727181,657.7],[-35.3600711,149.1724754,656.7],[-35.3601556,149.1722563,655.8],[-35.3602478,149.1720175,654.9],[-35.3603394,149.1717809,654.0],[-35.3604221,149.171567,653.2],[-35.3605125,149.1713332,652.3],[-35.3606021,149.1711014,651.4],[-35.3606986,149.1708514,650.5],[-35.360786,149.1706251,649.6],[-35.3608648,149.1704214,648.8],[-35.3609508,149.1701987,648.0],[-35.3610363,149.1699774,647.2],[-35.3611133,149.1697778,646.4],[-35.3611976,149.1695594,645.2],[-35.3612748,149.1693598,643.7],[-35.3613597,149.1691406,641.9],[-35.3614439,149.1689227,639.7],[-35.3615205,149.1687247,637.5],[-35.3616046,149.1685075,635.0],[-35.3616805,149.1683114,632.6],[-35.3617642,149.1680954,629.9],[-35.361847,149.1678817,627.3],[-35.3619227,149.1676865,624.8],[-35.3620061,149.1674714,622.2],[-35.3620896,149.1672565,619.5],[-35.362165,149.1670629,617.1],[-35.362248,149.1668498,614.5],[-35.3623236,149.1666557,612.1],[-35.362407,149.166442,609.5],[-35.3624894,149.1662303,607.3],[-35.3625591,149.1660509,606.9],[-35.3626279,149.1658742,606.5],[-35.3626918,149.1657105,605.5],[-35.3627503,149.1655609,605.3],[-35.3628019,149.1654287,605.5],[-35.3628426,149.1653245,605.8],[-35.3628801,149.1652284,606.0],[-35.3629084,149.1651552,605.0],[-35.3629173,149.1651318,602.3],[-35.3629189,149.1651286,600.6],[-35.36292,149.1651264,600.1],[-35.3629228,149.1651183,600.2],[-35.3629277,149.1651044,600.1]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-rtl-q-rtl-mode-3"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-vtol-land-approach.json b/tests/missions/sitl-quadplane-vtol-land-approach.json new file mode 100644 index 0000000000..0ffb1054de --- /dev/null +++ b/tests/missions/sitl-quadplane-vtol-land-approach.json @@ -0,0 +1 @@ +{"home":[-35.362938,149.165085,584.9500122070312],"items":[[84,-35.362938,149.165085,614.9500122070312,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,684.9500122070312,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.178303,684.9500122070312,[0.0,0.0,0.0,0.0]],[85,-35.362938,149.165085,664.9500122070312,[1.0,0.0,0.0,0.0]]],"heading":355.1348879668895,"start":[-35.362938,149.165085,585.0700073242188],"approach":168.6,"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":150.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.361542,149.1650471,625.4],[-35.3613051,149.1650476,625.9],[-35.36108,149.1650485,625.3],[-35.3608211,149.1650494,626.1],[-35.360556,149.1650503,628.3],[-35.3603147,149.1650512,631.0],[-35.3600517,149.1650524,634.0],[-35.3597901,149.1650534,637.0],[-35.3595534,149.1650547,639.7],[-35.3592946,149.165056,642.5],[-35.3590607,149.1650568,645.0],[-35.3588051,149.1650581,647.6],[-35.3585503,149.1650594,650.3],[-35.3583196,149.1650606,652.6],[-35.358067,149.1650622,655.2],[-35.3578129,149.1650638,657.7],[-35.3575584,149.1650656,660.3],[-35.3573038,149.1650675,662.8],[-35.3570726,149.1650697,665.1],[-35.3568191,149.1650721,667.6],[-35.3565658,149.1650743,670.1],[-35.356335,149.165077,672.4],[-35.3560895,149.1651413,675.7],[-35.3558996,149.1652996,679.9],[-35.3557419,149.1655427,683.0],[-35.3556433,149.1658307,684.0],[-35.3556119,149.1661144,684.3],[-35.3556263,149.1664302,684.3],[-35.355666,149.1667427,684.3],[-35.3557093,149.1670539,684.5],[-35.3557433,149.1673657,684.9],[-35.3557627,149.1676505,685.1],[-35.3557718,149.167963,685.3],[-35.3557717,149.1682758,685.4],[-35.355767,149.1685597,685.4],[-35.3557606,149.1688724,685.4],[-35.3557555,149.1691561,685.4],[-35.3557515,149.1694675,685.4],[-35.355749,149.1697785,685.3],[-35.355748,149.1700623,685.3],[-35.355748,149.1703745,685.2],[-35.3557491,149.1706873,685.2],[-35.3557502,149.1710003,685.2],[-35.355751,149.1713133,685.2],[-35.3557515,149.1715982,685.2],[-35.3557515,149.1719113,685.1],[-35.3557514,149.1722244,685.1],[-35.3557511,149.1725076,685.1],[-35.3557505,149.172819,685.1],[-35.3557501,149.1731035,685.0],[-35.35575,149.1734164,685.0],[-35.3557501,149.1737296,685.0],[-35.35575,149.1740147,685.0],[-35.3557498,149.1743286,685.0],[-35.3557498,149.1746421,685.1],[-35.35575,149.1749529,685.0],[-35.3557503,149.1752644,684.9],[-35.3557505,149.175549,684.9],[-35.3557509,149.1758625,684.9],[-35.3557511,149.1761767,685.0],[-35.3557513,149.1764612,685.0],[-35.3557513,149.176773,685.0],[-35.3557513,149.1770565,685.0],[-35.355751,149.1773977,684.9],[-35.3557586,149.177709,684.8],[-35.3558501,149.1779722,685.2],[-35.3560627,149.1781534,687.1],[-35.3562965,149.1781737,688.6],[-35.3565335,149.1780601,688.7],[-35.3567246,149.1778666,686.6],[-35.3568647,149.1776501,683.9],[-35.3569958,149.1773874,681.9],[-35.3571202,149.1771135,681.2],[-35.3572336,149.1768634,681.2],[-35.3573601,149.1765906,681.3],[-35.3574764,149.1763443,681.3],[-35.3576051,149.1760744,681.2],[-35.3577343,149.1758058,681.1],[-35.3578524,149.1755617,680.9],[-35.3579815,149.175295,680.6],[-35.3580997,149.1750514,680.3],[-35.3582299,149.1747827,680.0],[-35.3583607,149.1745131,679.7],[-35.3584793,149.1742689,679.4],[-35.3586092,149.1740011,679.0],[-35.3587385,149.1737337,678.7],[-35.3588569,149.1734897,678.3],[-35.3589872,149.1732216,677.9],[-35.3591061,149.1729767,677.6],[-35.3592368,149.1727076,677.2],[-35.3593677,149.1724382,676.8],[-35.3594863,149.1721943,676.5],[-35.3596164,149.1719265,676.1],[-35.3597467,149.1716584,675.7],[-35.3598773,149.1713898,675.3],[-35.3600078,149.1711212,674.9],[-35.3601268,149.1708768,674.5],[-35.360258,149.1706081,674.2],[-35.360389,149.1703393,673.8],[-35.360508,149.170095,673.4],[-35.3606381,149.1698279,673.0],[-35.3607568,149.1695844,672.6],[-35.3608881,149.1693156,672.2],[-35.3610194,149.1690464,671.8],[-35.3611384,149.1688014,671.5],[-35.3612683,149.1685335,671.1],[-35.3613983,149.1682652,670.7],[-35.3615286,149.1679965,670.3],[-35.3616593,149.1677276,669.9],[-35.3617782,149.167483,669.6],[-35.3619094,149.1672135,669.2],[-35.3620507,149.1669526,668.0],[-35.3622155,149.1667508,667.1],[-35.362451,149.1666229,667.1],[-35.3626855,149.1666107,667.4],[-35.3629369,149.1666501,666.4],[-35.3631868,149.1666876,664.9],[-35.3634177,149.1666842,664.4],[-35.3636668,149.1666159,664.5],[-35.3638952,149.1664759,664.8],[-35.3641039,149.1662526,665.0],[-35.3642496,149.1659992,664.9],[-35.364343,149.1657402,664.7],[-35.3644031,149.1654366,664.5],[-35.3644201,149.1651244,664.5],[-35.3643958,149.1648134,664.6],[-35.3643234,149.1644871,664.7],[-35.364228,149.1642301,664.7],[-35.3640875,149.1639705,664.7],[-35.3639131,149.163743,664.8],[-35.3637287,149.1635703,664.8],[-35.363503,149.1634242,664.9],[-35.3632831,149.1633353,665.0],[-35.3630331,149.1632883,665.0],[-35.3627792,149.1632954,664.9],[-35.3625517,149.1633484,664.9],[-35.3623129,149.1634567,664.9],[-35.3621116,149.163598,664.9],[-35.3619142,149.1637961,664.9],[-35.361748,149.1640328,664.9],[-35.3616277,149.1642754,664.9],[-35.3615336,149.164565,665.0],[-35.3614841,149.1648707,664.9],[-35.3615363,149.1651483,665.4],[-35.3617286,149.1653583,667.6],[-35.3619568,149.1654249,669.0],[-35.3622055,149.1653979,669.8],[-35.3624082,149.1653306,673.2],[-35.3625447,149.165259,677.3],[-35.3626613,149.1651872,679.9],[-35.3627525,149.1651318,681.4],[-35.3628257,149.1650963,681.7],[-35.3628799,149.1650784,679.9],[-35.3628979,149.165083,678.8],[-35.3629099,149.1650977,679.4],[-35.3629196,149.1651059,679.2],[-35.3629272,149.1651033,679.2]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-vtol-land-approach"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-vtol-land-q-options.json b/tests/missions/sitl-quadplane-vtol-land-q-options.json new file mode 100644 index 0000000000..87b190eafc --- /dev/null +++ b/tests/missions/sitl-quadplane-vtol-land-q-options.json @@ -0,0 +1 @@ +{"home":[-35.3629385,149.1650851,585.1199951171875],"items":[[22,-35.3629385,149.1650851,615.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.178303,685.1199951171875,[0.0,0.0,0.0,0.0]],[85,-35.362938,149.165085,645.1199951171875,[0.0,0.0,0.0,0.0]]],"heading":251.93458190675557,"start":[-35.3629385,149.1650851,585.0499877929688],"approach":169.5,"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":-120.0,"Q_OPTIONS":16.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3622274,149.1640704,619.5],[-35.36198,149.1641154,620.0],[-35.3617544,149.1641587,619.8],[-35.3615,149.1642052,620.5],[-35.3612411,149.1642494,622.5],[-35.3610044,149.1642874,624.8],[-35.3607463,149.1643271,627.7],[-35.360513,149.1643624,630.3],[-35.3602568,149.1644009,633.1],[-35.3600012,149.1644394,635.9],[-35.3597695,149.1644739,638.3],[-35.359516,149.1645123,641.0],[-35.3592864,149.1645474,643.4],[-35.3590355,149.1645856,646.0],[-35.3587837,149.1646243,648.5],[-35.3585551,149.1646593,650.8],[-35.3583029,149.164698,653.3],[-35.35805,149.1647368,655.8],[-35.3577987,149.164775,658.3],[-35.3575463,149.1648132,660.8],[-35.3573185,149.1648475,663.0],[-35.3570669,149.1648858,665.4],[-35.3568147,149.1649241,667.9],[-35.3565855,149.164959,670.1],[-35.3563339,149.1649975,672.6],[-35.3561114,149.1650701,675.3],[-35.3559048,149.1652471,679.8],[-35.3557518,149.165492,682.9],[-35.3556622,149.1657546,684.0],[-35.3556241,149.1660663,684.4],[-35.3556354,149.1663815,684.5],[-35.355668,149.1666671,684.5],[-35.3557086,149.1669793,684.7],[-35.3557391,149.1672623,685.1],[-35.3557612,149.1675746,685.3],[-35.3557711,149.1678885,685.5],[-35.3557719,149.1681731,685.6],[-35.3557675,149.1684859,685.6],[-35.3557612,149.1687985,685.6],[-35.3557553,149.1691094,685.5],[-35.3557509,149.1694215,685.5],[-35.3557486,149.1697057,685.5],[-35.355748,149.1700185,685.5],[-35.3557487,149.170332,685.4],[-35.3557498,149.1706162,685.4],[-35.3557512,149.1709287,685.4],[-35.3557522,149.1712128,685.3],[-35.3557528,149.1715253,685.3],[-35.3557529,149.1718379,685.3],[-35.3557527,149.1721226,685.2],[-35.355752,149.1724353,685.2],[-35.3557512,149.1727472,685.2],[-35.3557505,149.1730598,685.2],[-35.35575,149.1733721,685.2],[-35.3557496,149.1736557,685.2],[-35.3557494,149.1739687,685.2],[-35.3557495,149.1742822,685.1],[-35.3557497,149.1745666,685.2],[-35.3557498,149.1748796,685.1],[-35.35575,149.1751646,685.2],[-35.3557503,149.1754767,685.2],[-35.3557506,149.1757889,685.1],[-35.3557507,149.1760742,685.1],[-35.3557507,149.1763869,685.2],[-35.3557509,149.1767004,685.1],[-35.355751,149.1770132,685.1],[-35.3557511,149.1773261,685.1],[-35.3557526,149.1776103,685.0],[-35.3558217,149.1779104,684.9],[-35.3560392,149.17813,686.6],[-35.356297,149.1781563,688.4],[-35.3565325,149.178041,688.5],[-35.356708,149.1778677,686.5],[-35.3568649,149.1776327,683.3],[-35.3569977,149.1773691,681.0],[-35.357112,149.1771193,680.1],[-35.3572371,149.1768445,679.8],[-35.357352,149.1765962,679.5],[-35.3574796,149.1763257,679.2],[-35.3576203,149.1760318,678.6],[-35.3577381,149.1757872,678.1],[-35.3578673,149.1755198,677.4],[-35.3579861,149.1752747,676.8],[-35.358116,149.1750071,676.1],[-35.3582459,149.174739,675.4],[-35.3583645,149.1744945,674.7],[-35.3584944,149.1742258,674.1],[-35.3586244,149.1739575,673.3],[-35.3587425,149.1737143,672.7],[-35.358873,149.1734468,671.8],[-35.358991,149.173204,671.1],[-35.3591215,149.1729349,670.3],[-35.3592512,149.1726672,669.6],[-35.35937,149.1724226,668.8],[-35.3595004,149.1721548,668.0],[-35.3596321,149.1718847,667.3],[-35.3597632,149.1716157,666.5],[-35.3598933,149.1713479,665.7],[-35.3600116,149.171104,665.0],[-35.3601422,149.1708358,664.2],[-35.3602733,149.170567,663.3],[-35.3603928,149.1703225,662.6],[-35.3605239,149.170054,661.9],[-35.360643,149.16981,661.2],[-35.3607737,149.1695419,660.4],[-35.3609041,149.1692738,659.6],[-35.3610231,149.1690294,658.9],[-35.3611536,149.1687613,658.1],[-35.3612844,149.1684923,657.3],[-35.3614151,149.168224,656.5],[-35.3615451,149.1679574,655.8],[-35.361664,149.1677138,655.0],[-35.361795,149.1674455,654.2],[-35.3619258,149.1671775,653.5],[-35.3620446,149.1669331,652.8],[-35.3621667,149.1666593,651.2],[-35.3622411,149.1663898,649.3],[-35.3622593,149.1660748,649.2],[-35.3622813,149.1657921,651.9],[-35.3622867,149.1655806,654.6],[-35.3622843,149.1653779,653.9],[-35.3622775,149.1651827,653.0],[-35.3622692,149.1650129,654.1],[-35.3622616,149.1648723,655.2],[-35.3622561,149.1647707,656.1],[-35.3622516,149.1646871,655.8],[-35.3622525,149.1646636,653.6],[-35.3622691,149.164679,653.8],[-35.3623002,149.1647074,653.9],[-35.3623369,149.1647379,653.9],[-35.3623813,149.1647701,653.9],[-35.3624273,149.1647989,653.9],[-35.3624693,149.1648224,653.9],[-35.3625148,149.1648465,653.8],[-35.3625555,149.1648677,653.8],[-35.3625999,149.1648915,653.9],[-35.3626483,149.1649185,653.8],[-35.3626887,149.1649418,653.9],[-35.3627333,149.1649681,653.9],[-35.3627771,149.164994,653.9],[-35.3628138,149.1650157,653.9],[-35.3628484,149.165036,653.8],[-35.3628761,149.1650519,653.8],[-35.3629017,149.1650663,653.9],[-35.3629178,149.1650751,653.8]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-vtol-land-q-options"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-vtol-land.json b/tests/missions/sitl-quadplane-vtol-land.json new file mode 100644 index 0000000000..e0b37eaedb --- /dev/null +++ b/tests/missions/sitl-quadplane-vtol-land.json @@ -0,0 +1 @@ +{"home":[-35.3629381,149.1650858,585.010009765625],"items":[[22,-35.3629381,149.1650858,615.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.178303,685.010009765625,[0.0,0.0,0.0,0.0]],[85,-35.362938,149.165085,645.010009765625,[0.0,0.0,0.0,0.0]]],"heading":156.55184140685728,"start":[-35.3629381,149.1650858,585.0899658203125],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3629418,149.1662625,618.1],[-35.3627057,149.1662692,617.5],[-35.362448,149.1662393,615.8],[-35.36219,149.166189,615.6],[-35.3619558,149.1661397,616.7],[-35.3617,149.1660876,618.7],[-35.3614686,149.1660434,620.9],[-35.3612143,149.1659975,623.4],[-35.3609609,149.1659533,626.1],[-35.3607311,149.1659142,628.5],[-35.3604792,149.1658719,631.1],[-35.3602268,149.1658297,633.7],[-35.3599979,149.1657912,636.1],[-35.3597457,149.1657487,638.6],[-35.3595169,149.1657106,641.0],[-35.359267,149.165669,643.5],[-35.3590161,149.165627,646.0],[-35.3587871,149.1655889,648.3],[-35.3585353,149.1655468,650.8],[-35.3583059,149.1655087,653.1],[-35.3580546,149.1654671,655.7],[-35.3578047,149.1654259,658.1],[-35.3575766,149.1653883,660.3],[-35.3573249,149.1653467,662.8],[-35.3570727,149.1653048,665.3],[-35.3568435,149.1652669,667.5],[-35.3565926,149.1652255,670.0],[-35.356365,149.1651879,672.1],[-35.3561123,149.165193,675.1],[-35.3558882,149.1653366,679.9],[-35.3557351,149.1655485,683.2],[-35.3556258,149.1658303,684.5],[-35.3555861,149.166141,684.4],[-35.3556082,149.1664858,684.1],[-35.3556572,149.1667979,684.0],[-35.3557033,149.1670801,684.3],[-35.3557426,149.1673903,684.7],[-35.3557661,149.1677022,685.1],[-35.3557757,149.1680154,685.3],[-35.3557754,149.1683283,685.4],[-35.3557703,149.1686122,685.4],[-35.3557626,149.1689513,685.5],[-35.3557565,149.1692609,685.4],[-35.3557527,149.1695441,685.3],[-35.3557505,149.1698568,685.3],[-35.35575,149.1701416,685.3],[-35.3557506,149.1704548,685.3],[-35.3557518,149.1707659,685.3],[-35.3557527,149.1710489,685.3],[-35.3557534,149.171361,685.2],[-35.3557538,149.1716454,685.2],[-35.3557538,149.1719586,685.1],[-35.3557537,149.1722718,685.2],[-35.3557533,149.1725563,685.1],[-35.3557525,149.1728687,685.1],[-35.3557513,149.173181,685.1],[-35.3557504,149.1734655,685.1],[-35.3557503,149.1737778,685.1],[-35.3557506,149.1740627,685.0],[-35.3557513,149.1743762,685.1],[-35.355752,149.1746902,685.1],[-35.3557524,149.1749749,685.1],[-35.3557524,149.1752876,685.0],[-35.3557521,149.1755987,685.0],[-35.3557515,149.1759112,685.0],[-35.355751,149.1762244,685.0],[-35.3557508,149.1765097,685.0],[-35.3557508,149.1768213,685.0],[-35.3557508,149.1771335,685.0],[-35.3557508,149.1774172,685.0],[-35.3557682,149.1777292,684.8],[-35.3558816,149.1779801,685.6],[-35.3561119,149.1781405,687.1],[-35.3563756,149.1781288,688.2],[-35.3565835,149.1779723,688.3],[-35.356722,149.1776953,687.8],[-35.3567672,149.177379,686.4],[-35.3567689,149.1770659,684.3],[-35.3567757,149.1767546,682.7],[-35.3568117,149.176479,682.3],[-35.3568897,149.1761897,682.4],[-35.3569992,149.1759208,682.5],[-35.3571178,149.1756903,682.7],[-35.3572597,149.175448,682.9],[-35.3573915,149.1752344,683.2],[-35.3575369,149.1749987,683.5],[-35.3576779,149.1747626,683.8],[-35.3578033,149.1745443,684.1],[-35.357936,149.174304,684.3],[-35.3580667,149.1740606,684.4],[-35.3581945,149.1738192,684.5],[-35.3583224,149.1735774,684.6],[-35.3584383,149.1733603,684.7],[-35.3585658,149.173124,684.7],[-35.3587047,149.1728688,684.7],[-35.3588304,149.1726393,684.7],[-35.3589561,149.1724097,684.8],[-35.3590688,149.1722034,684.8],[-35.3591922,149.1719771,684.8],[-35.3593135,149.171754,684.8],[-35.3594235,149.1715513,684.7],[-35.3595426,149.1713319,684.8],[-35.3596508,149.1711327,684.8],[-35.3597682,149.1709165,684.7],[-35.3598947,149.1706833,684.7],[-35.3599994,149.1704909,684.7],[-35.3601137,149.1702815,684.7],[-35.3602169,149.1700924,684.7],[-35.3603291,149.1698862,684.7],[-35.3604408,149.1696806,684.7],[-35.3605412,149.1694959,684.7],[-35.3606505,149.1692943,684.7],[-35.3607594,149.1690938,684.8],[-35.3608583,149.1689113,684.8],[-35.3609669,149.1687109,684.9],[-35.3610657,149.1685289,684.9],[-35.361173,149.1683324,684.9],[-35.3612802,149.1681363,684.9],[-35.3613783,149.1679567,685.0],[-35.361485,149.1677606,685.0],[-35.3615922,149.1675634,685.0],[-35.361699,149.1673666,685.1],[-35.3618053,149.1671704,685.1],[-35.3619023,149.1669911,685.2],[-35.3620079,149.1667962,685.2],[-35.3621149,149.1665993,685.2],[-35.362212,149.1664209,685.2],[-35.3623196,149.1662236,685.2],[-35.3624136,149.166051,685.2],[-35.3625108,149.1658722,685.5],[-35.3625955,149.1657161,685.8],[-35.3626656,149.165587,685.6],[-35.3627346,149.16546,685.5],[-35.362796,149.1653468,685.6],[-35.3628475,149.1652516,685.9],[-35.3628889,149.1651747,685.8],[-35.3629077,149.1651392,684.5],[-35.3629118,149.165132,685.2],[-35.3629154,149.1651261,685.2],[-35.3629202,149.1651168,685.2],[-35.3629271,149.1651029,685.3]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-vtol-land"} \ No newline at end of file diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index 7b17f060d1..435562ff72 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -1,9 +1,11 @@ '''the path plane_track flies a mission along, against ArduPlane's own navigation and a flight flown through it''' +import json import math import os import struct +import types import pytest @@ -1204,6 +1206,338 @@ def test_without_parameters_arduplanes_defaults_fly(self): # the first 30s, which takes in a QuadPlane's transition and a plane's # takeoff; and the 90th percentile and worst of the height of each point # flown above or below the nearest point of the path drawn +FLIGHTS = { + # QuadPlane.KalaupapaCanyonRun: loiters up and down over the sea, a + # winding gorge inland, a spiral up out of the head of the canyon, and + # down with the ground over a ridge and along the valley beyond + 'kalaupapa-canyon-run.json': (2.0, 8.0, 80.0, 30.0, 25.0, 45.0), + # Plane.MissionItemTypes: every kind of item a plane flies on the way, + # and a landing + 'sitl-plane-mission-items.json': (3.0, 16.0, 35.0, 35.0, 7.0, 22.0), + # a return to launch, which a rally point is nearer than home for + 'sitl-plane-rtl-rally.json': (10.0, 40.0, 55.0, 55.0, 11.0, 18.0), + # a loiter for ever + 'sitl-plane-loiter-unlimited.json': (2.0, 16.0, 30.0, 30.0, 12.0, 18.0), + # QuadPlane.VTOLMissionItemTypes: a VTOL landing on a fixed-wing + # approach, asked for by param1 + 'sitl-quadplane-vtol-land-approach.json': (7.0, 28.0, 45.0, 45.0, 7.0, 25.0), + # a VTOL landing straight in + 'sitl-quadplane-vtol-land.json': (2.0, 20.0, 40.0, 40.0, 6.0, 11.0), + # a VTOL landing on an approach, asked for by Q_OPTIONS. The flight + # met its circle already within the 5 degrees of the course it breaks + # out on, and broke out at once, where the path drawn went round once + # more: the approach is not modelled that finely + 'sitl-quadplane-vtol-land-q-options.json': (15.0, 42.0, 90.0, 90.0, 13.0, 16.0), + # a return to launch with each Q_RTL_MODE: switching to QRTL near home, + # landing on an approach, whose circle is flown a little wider than + # drawn, and as QRTL + 'sitl-quadplane-rtl-q-rtl-mode-1.json': (2.0, 15.0, 40.0, 40.0, 7.0, 12.0), + 'sitl-quadplane-rtl-q-rtl-mode-2.json': (13.0, 35.0, 70.0, 70.0, 7.0, 22.0), + 'sitl-quadplane-rtl-q-rtl-mode-3.json': (4.0, 30.0, 45.0, 45.0, 6.0, 11.0), +} + + +class TestFlownMission(object): + """the path worked out for a mission against a flight of it""" + + @pytest.fixture(params=sorted(FLIGHTS)) + def flight(self, request): + path = os.path.join(os.path.dirname(__file__), 'missions', + request.param) + with open(path) as f: + return (json.load(f), FLIGHTS[request.param]) + + @staticmethod + def recorder(): + import importlib.util + path = os.path.join(os.path.dirname(__file__), 'missions', + 'record_flight.py') + spec = importlib.util.spec_from_file_location('record_flight', path) + recorder = importlib.util.module_from_spec(spec) + spec.loader.exec_module(recorder) + return recorder + + def test_the_firmware_which_flew_a_recording_is_kept(self): + '''the drawing follows what a version of ArduPlane does, so a + recording of a flight says which flew it''' + recorder = self.recorder() + + class Log(object): + def __init__(self, *messages): + self.messages = list(messages) + + def rewind(self): + self.left = list(self.messages) + + def recv_match(self, type=None): + while self.left: + m = self.left.pop(0) + if type is None or m.get_type() in type: + return m + return None + + def message(kind, **fields): + m = types.SimpleNamespace(**fields) + m.get_type = lambda: kind + return m + started = message('MSG', Message='ArduPlane V4.8.0-dev (48c91a4c9e)') + other = message('MSG', Message='Frame: QUADPLANE') + version = message('VER', FWS='ArduPlane V4.8.0-dev', GH=0x48c91a4c) + # VER says which commit it was built from, wherever it comes: after + # the message the firmware starts with, as well as before it + for log in (Log(other, version, started), + Log(other, started, version)): + assert recorder.firmware(log) == 'ArduPlane V4.8.0-dev (48c91a4c)' + # an older log has only what the firmware said as it started + assert recorder.firmware(Log(other, started)) == \ + 'ArduPlane V4.8.0-dev (48c91a4c9e)' + # and a VER which does not name a commit is still the version + assert recorder.firmware(Log(message('VER', FWS='ArduPlane V4.8.0'))) \ + == 'ArduPlane V4.8.0' + # and one which says neither says nothing + assert recorder.firmware(Log(other)) is None + + def test_a_course_of_north_is_recorded(self): + recorder = self.recorder() + assert recorder.extras(0.0, None, [], 0.0) == { + 'heading': 0.0, 'approach': 0.0} + assert recorder.extras(None, (1, 2, 3), [(4, 5, 6)], None) == { + 'start': (1, 2, 3), 'rally': [(4, 5, 6)]} + + def test_a_loiter_to_altitude_with_no_radius_is_reached_at_the_default( + self): + # WP_LOITER_RAD 80: a point 100m from the centre is on its circle, + # which with 50 it is not, and one 60m off is + (lat, lon, amsl) = offset(1000, 0, 100) + items = [(mavlink.MAV_CMD_NAV_LOITER_TO_ALT, lat, lon, amsl, + (0, 0, 0, 0))] + path = [offset(0, 0, 100), offset(500, 0, 100), + offset(900, 0, 100), offset(1000, 60, 100)] + assert self.arrivals(path, items, dict(PARAMS, WP_LOITER_RAD=80)) == \ + [(1, 2)] + assert self.arrivals(path, items, dict(PARAMS, WP_LOITER_RAD=-50)) == \ + [(1, 3)] + # and one of no radius at all is ArduPlane's 60m + for none in (0, 1, -1): + assert self.arrivals(path, items, + dict(PARAMS, WP_LOITER_RAD=none)) == [(1, 3)] + + @staticmethod + def drawn(data): + '''the path drawn for a recorded flight's mission, given what + MAVExplorer gave it''' + items = [(c, la, lo, a, tuple(p)) for (c, la, lo, a, p) in data['items']] + rally = [tuple(point) for point in data.get('rally', [])] + start = data.get('start') + return (items, plane_track.mission_track( + tuple(data['home']), items, data['params'], + heading=data.get('heading'), + start=None if start is None else tuple(start), + rally=rally or None, approach=data.get('approach'), + takeoff_course=data.get('takeoff_course'))) + + def distances(self, track, flown): + '''metres from each flown point to the nearest leg of track, in + order of distance''' + return sorted(self.unsorted_distances(track, flown)) + + def unsorted_distances(self, track, flown): + '''metres from each flown point to the nearest leg of track''' + (lat0, lon0) = (track[0][0], track[0][1]) + scale = math.cos(math.radians(lat0)) + + def xy(p): + return ((p[0] - lat0) * 111319.5, (p[1] - lon0) * 111319.5 * scale) + segments = [(xy(a), xy(b)) for (a, b) in zip(track, track[1:])] + out = [] + for p in flown: + (pn, pe) = xy(p) + best = None + for ((an, ae), (bn, be)) in segments: + (dn, de) = (bn - an, be - ae) + squared = dn * dn + de * de + t = 0.0 + if squared > 0: + t = min(max(((pn - an) * dn + (pe - ae) * de) / squared, 0.0), 1.0) + d = math.hypot(pn - (an + t * dn), pe - (ae + t * de)) + if best is None or d < best: + best = d + out.append(best) + return out + + def heights(self, track, flown): + '''metres each flown point is above the nearest point of track, + nearest counting height as well, so a spiral is measured against + the turn of it at the height flown; below is negative''' + (lat0, lon0) = (track[0][0], track[0][1]) + scale = math.cos(math.radians(lat0)) + + def xyz(p): + return ((p[0] - lat0) * 111319.5, + (p[1] - lon0) * 111319.5 * scale, p[2]) + segments = [(xyz(a), xyz(b)) for (a, b) in zip(track, track[1:])] + out = [] + for p in flown: + q = xyz(p) + best = None + for (a, b) in segments: + d = [b[i] - a[i] for i in range(3)] + squared = sum(x * x for x in d) + t = 0.0 + if squared > 0: + t = sum((q[i] - a[i]) * d[i] for i in range(3)) / squared + t = min(max(t, 0.0), 1.0) + nearest = [a[i] + t * d[i] for i in range(3)] + distance = math.dist(q, nearest) + if best is None or distance < best[0]: + best = (distance, q[2] - nearest[2]) + out.append(best[1]) + return out + + # items which end a mission's flight: nothing after one is flown to + ENDINGS = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavlink.MAV_CMD_NAV_LAND, + mavlink.MAV_CMD_NAV_VTOL_LAND) + + def arrivals(self, path, items, flight_params): + '''for each item flown to, in mission order, the index into path of + the first point near it after the item before was reached: a + waypoint's is within the distance a turn is started out, a loiter's + within its circle, which is ArduPlane's default where it gives + none. An item reached out of turn leaves the ones after it + unreached, as None; so does a flight which ends short. + Items with no position, those ArduPlane skips, takeoffs, which the + flight starts from, VTOL landings, and anything after the item which + ends the flight, are left out''' + out = [] + after = 0 + for (seq, (command, lat, lon, amsl, params)) in enumerate(items, 1): + if seq > 1 and items[seq - 2][0] in self.ENDINGS: + break + if ((lat == 0 and lon == 0) or + command in plane_track.SKIPPED_COMMANDS or + command in plane_track.TAKEOFF_COMMANDS or + command == mavlink.MAV_CMD_NAV_VTOL_LAND): + continue + near = 120.0 + if command == mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + radius = abs(params[1] or 0.0) + if radius <= 1: + radius = plane_track.default_loiter_radius( + plane_track.parameter(flight_params, 'WP_LOITER_RAD')) + near = radius * 1.4 + index = None + if after is not None: + for i in range(after, len(path)): + if mp_util.gps_distance(path[i][0], path[i][1], + lat, lon) < near: + index = i + break + out.append((seq, index)) + after = index + return out + + # a fixed-wing launch goes whichever way the aircraft is thrown or the + # runway points. Where the recording says which course it was flown on, + # the launch is drawn on that, and is held to as near as the rest of the + # flight; where it does not, it is drawn towards the first waypoint, + # anywhere within 45 degrees or so of the way flown, so until the + # aircraft gets there it can be that far across from the line drawn + LAUNCH_LIMIT = 10.0 + LAUNCH_SPREAD = math.sin(math.radians(45)) + 0.05 + LAUNCH_SLACK = 30.0 + + def launch(self, data, items, flown): + '''the flight split into its fixed-wing launch, up to where it gets + to the first waypoint after it, and the rest. A VTOL takeoff goes + up where it is, and is left with the rest''' + first = items[0][0] if items else None + if first not in plane_track.TAKEOFF_COMMANDS: + return ([], flown) + flight = plane_track.MissionFlight( + (0.0, 0.0), (0.0, 0.0, 0.0), list(items), data['params']) + if flight.vtol_takeoff(first): + return ([], flown) + arrived = [i for (seq, i) in self.arrivals(flown, items, data['params']) + if i is not None] + if not arrived: + return (flown, []) + return (flown[:arrived[0]], flown[arrived[0]:]) + + def length(self, path, start, end): + return sum(mp_util.gps_distance(a[0], a[1], b[0], b[1]) + for (a, b) in zip(path[start:end], path[start + 1:end + 1])) + + def test_the_path_is_flown_in_the_order_flown(self, flight): + # the distances alone would not notice legs flown in the wrong order, + # backwards or with extra loops, so long as the lines were there + (data, _) = flight + (items, track) = self.drawn(data) + flown = data['flown'] + drawn = self.arrivals(track, items, data['params']) + seen = self.arrivals(flown, items, data['params']) + # the flight kept ends short of the last waypoint + seen = [(seq, i) for (seq, i) in seen if i is not None] + order = [seq for (seq, i) in sorted(seen, key=lambda x: x[1])] + assert order == [seq for (seq, i) in seen] + assert None not in [i for (seq, i) in drawn] + assert ([seq for (seq, i) in sorted(drawn, key=lambda x: x[1])] == + [seq for (seq, i) in drawn]) + # and about as far between each of them as was flown: an extra lap + # of a loiter, or a leg flown twice, would not be + drawn = dict(drawn) + stages = [seq for (seq, i) in seen] + total_drawn = self.length(track, drawn[stages[0]], drawn[stages[-1]]) + total_flown = self.length(flown, dict(seen)[stages[0]], + dict(seen)[stages[-1]]) + assert total_drawn == pytest.approx(total_flown, rel=0.1) + seen = dict(seen) + for (a, b) in zip(stages, stages[1:]): + flown_stage = self.length(flown, seen[a], seen[b]) + if flown_stage < 1000: + continue + assert self.length(track, drawn[a], drawn[b]) == pytest.approx( + flown_stage, rel=0.2), (a, b) + + def test_the_path_is_the_one_flown(self, flight): + (data, (median_limit, p90_limit, worst_limit, later_limit, _, + _)) = flight + (items, track) = self.drawn(data) + assert track is not None + (launch, flown) = self.launch(data, items, data['flown']) + # the launch, drawn on the course flown where that is known, and + # otherwise only going the right general way + start = launch[0] if launch else None + known = data.get('takeoff_course') is not None + for (p, d) in zip(launch, self.unsorted_distances(track, launch)): + if known: + assert d < self.LAUNCH_LIMIT + continue + out = mp_util.gps_distance(start[0], start[1], p[0], p[1]) + assert d < self.LAUNCH_SPREAD * out + self.LAUNCH_SLACK + distances = self.distances(track, flown) + median = distances[len(distances) // 2] + p90 = distances[int(len(distances) * 0.9)] + # the tangent-and-circle drawing this replaced was 6m out at the + # median and 22m at the 90th percentile over the first flight of the + # canyon run + assert median < median_limit + assert p90 < p90_limit + # a QuadPlane climbs out of its VTOL takeoff and transitions on the + # way to the first item, and a plane rolls down the runway, neither + # of which is modelled; after that the path stays closer + assert distances[-1] < worst_limit + later = self.distances(track, flown[30:]) + assert later[-1] < later_limit + + def test_the_path_is_flown_at_the_height_flown(self, flight): + # the distances above are across the ground only + (data, (_, _, _, _, p90_limit, worst_limit)) = flight + (items, track) = self.drawn(data) + heights = sorted(abs(h) for h in self.heights(track, data['flown'])) + assert heights[int(len(heights) * 0.9)] < p90_limit + assert heights[-1] < worst_limit class TestDrawnTrack(object): From a1f99c3cf0c7bdd8705421bf80ae99eaaad6d492 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 7 Oct 2026 17:16:57 +1100 Subject: [PATCH 19/22] plane_track: follow the jumps, and the altitudes, ArduPilot does Two places the mission was read differently from AP_Mission: - Meeting a DO_JUMP twice over ended the drawn mission. AP_Mission gives up that way only while it is looking ahead (AP_Mission::get_next_cmd); advancing, it runs the jump's repeats out and carries on past it, so a loop with nothing to navigate to in it -- a jump to itself, or one over commands which are not flown to -- is flown past rather than ending the mission 3km short of the item after it. - Any negative repeat count was taken as repeating for ever, where AP_MISSION_JUMP_REPEAT_FOREVER is -1 alone and a jump asking for fewer is never followed at all. An item with no position of its own and no altitude either is flown at the altitude the aircraft is at, which Plane::set_next_WP() gives it in whatever frame the zero is in; an absolute zero was drawn as 0m AMSL. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/modules/lib/plane_track.py | 36 +++++++++++++++++++---------- tests/test_plane_track.py | 29 +++++++++++++++++++++++ 2 files changed, 53 insertions(+), 12 deletions(-) diff --git a/MAVProxy/modules/lib/plane_track.py b/MAVProxy/modules/lib/plane_track.py index 4aa2194b7f..662f929c3b 100644 --- a/MAVProxy/modules/lib/plane_track.py +++ b/MAVProxy/modules/lib/plane_track.py @@ -127,6 +127,9 @@ ) # navigation commands numbered past MAV_CMD_NAV_LAST LATE_NAV_COMMANDS = (42702, 42703) # NAV_SCRIPT_TIME, NAV_ATTITUDE_TIME +# AP_MISSION_JUMP_REPEAT_FOREVER: the only repeat count which repeats for +# ever, and the fewest AP_Mission follows at all +JUMP_FOREVER = -1 MAV_CMD_JUMP_TAG = 600 MAV_CMD_DO_JUMP_TAG = 601 # ArduPlane rejects these, and moves straight on to the item after @@ -456,17 +459,19 @@ def jump_target(self, index): return target - 1 def jumps(self, index): - '''whether the jump at index is followed. AP_Mission follows one - its repeat count, or for ever with a count of -1; drawn, each is - followed once, so the path goes round a loop the once''' + '''whether the jump at index is followed. AP_Mission follows one its + repeat count times, or for ever with a count of -1, and never one + asking for fewer than that; drawn, each is followed once, so the path + goes round a loop the once''' repeats = int(self.items[index][4][1] or 0) - return repeats != 0 and index not in self.jumps_taken + return repeats >= JUMP_FOREVER and repeats != 0 and \ + index not in self.jumps_taken def loops_forever(self, index): '''whether the jump at index has already been followed and is followed for ever, so that the mission never gets past it''' repeats = int(self.items[index][4][1] or 0) - return repeats < 0 and index in self.jumps_taken + return repeats == JUMP_FOREVER and index in self.jumps_taken def next_nav_index(self, index, take=False, passed=None): '''AP_Mission::get_next_nav_cmd: the index of the next item after @@ -479,8 +484,14 @@ def next_nav_index(self, index, take=False, passed=None): command = self.items[i][0] if command in (mavutil.mavlink.MAV_CMD_DO_JUMP, MAV_CMD_DO_JUMP_TAG): if i in followed: - # round in a circle without navigating anywhere - return None + # round to this jump again without navigating anywhere. + # AP_Mission gives up only while it is looking ahead + # (AP_Mission::get_next_cmd); advancing, it runs the + # jump's repeats out and carries on past it + if not take: + return None + i += 1 + continue if self.loops_forever(i): # the aircraft goes round this loop for ever, and has # been drawn going round it once @@ -1243,14 +1254,15 @@ def is_navigation_command(command): def positionless_amsl(alt, frame, home_amsl): '''the AMSL altitude an item with no position of its own is flown at, or None for the altitude the aircraft is at. Location::sanitize() puts - such an item where the aircraft is, but keeps its altitude unless that - is a relative 0; a terrain-relative one needs the terrain under wherever - that is, which is not known here''' - if alt is None: + such an item where the aircraft is, and Plane::set_next_WP() keeps the + altitude it has there unless that is zero, in whatever frame, which is + the altitude the aircraft is at as well; a terrain-relative one needs + the terrain under wherever that is, which is not known here''' + if alt is None or alt == 0: return None if frame in (0, 5): return alt - if alt == 0 or frame not in (3, 6) or home_amsl is None: + if frame not in (3, 6) or home_amsl is None: return None return home_amsl + alt diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index 435562ff72..b169fd83b5 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -678,6 +678,35 @@ def test_waypoints_closer_than_a_turn_are_passed_on_the_way_home(self): far = max(math.hypot(*local(p)) for p in track) assert far > PARAMS['WP_LOITER_RAD'] * 0.9 + def test_a_loop_with_nothing_to_navigate_to_is_flown_past(self): + """AP_Mission gives up on meeting a jump twice over only while it is + looking ahead; advancing, it runs the jump's repeats out and carries + on past it, so a loop with no item to navigate to in it does not end + the mission""" + after = (3000, 0) + for loop in ([self.jump(2, 3)], + [change_speed(25), self.jump(2, 3)]): + items = [waypoint(1000, 0)] + loop + [waypoint(*after)] + assert self.visits(fly(items), *after) == 1 + + def test_a_jump_asking_for_fewer_than_for_ever_is_not_followed(self): + """AP_Mission repeats for ever on -1 alone, and never follows a jump + asking for fewer than that""" + items = [waypoint(1000, 0), self.jump(1, -2), waypoint(3000, 0)] + assert self.visits(fly(items), 3000, 0) == 1 + # where -1 is flown round the loop the once and ends there + items = [waypoint(1000, 0), self.jump(1, -1), waypoint(3000, 0)] + assert self.visits(fly(items), 3000, 0) == 0 + + def test_an_item_with_no_altitude_at_all_holds_the_one_flown(self): + """Plane::set_next_WP takes a zero altitude, in whatever frame, as + the altitude the aircraft is at""" + for frame in (0, 3, 10): + assert plane_track.positionless_amsl(0.0, frame, HOME[2]) is None + # where an altitude of its own is flown at + assert plane_track.positionless_amsl(700.0, 0, HOME[2]) == 700.0 + assert plane_track.positionless_amsl(100.0, 3, HOME[2]) == HOME[2] + 100 + def test_a_mission_which_loops_for_ever_does_not_return(self): items = [waypoint(2000, 0), waypoint(2000, 2000), waypoint(0, 2000), self.jump(1, -1)] From 80978e87e4ec60e528e95abf7bebe147d4eaa48c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 7 Oct 2026 17:17:47 +1100 Subject: [PATCH 20/22] map3d: take the rally points from the module which has them The 3D map read them from a rallyloader on the rally module, which has no such thing: it is a MissionItemProtocolModule, and answers for its points itself. So nothing was ever drawn for them, and a return to launch in the live map went to home however near a rally point was. The tests had a double with the attribute the code reached for, which is why they did not notice; they now stand in for what the module really offers. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/modules/mavproxy_map3d/__init__.py | 11 ++++++----- tests/test_plane_track.py | 15 +++++++-------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index ed2bcc0b0b..78bf2b82dc 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -583,8 +583,9 @@ def rally_points(self, home): its own rather than one which moves with home''' from MAVProxy.modules.lib import plane_track try: - loader = self.module('rally').rallyloader - points = [loader.rally_point(i) for i in range(loader.rally_count())] + rally = self.module('rally') + points = [rally.rally_point(i) + for i in range(rally.rally_count())] except Exception: return [] out = [] @@ -763,8 +764,8 @@ def send_rally(self): if self.map is None: return try: - loader = self.module('rally').rallyloader - raw = [loader.rally_point(i) for i in range(loader.rally_count())] + rally = self.module('rally') + raw = [rally.rally_point(i) for i in range(rally.rally_count())] except Exception: return resolved = self.rally_points((0.0, 0.0, self.home_amsl)) @@ -860,7 +861,7 @@ def idle_task(self): except Exception: pass try: - rally_change = self.module('rally').rallyloader.last_change + rally_change = self.module('rally').last_change() if rally_change != self.rally_change_time: self.rally_change_time = rally_change self.send_rally() diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index b169fd83b5..fb783f1d10 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -1846,8 +1846,7 @@ def test_the_live_map_takes_home_for_an_origin_it_has_not_been_told(self): lng=int(HOME[1] * 1e7), alt=100, flags=frame_valid | (2 << 3))] module.mpstate.module = lambda name: SimpleNamespace( - rallyloader=SimpleNamespace(rally_count=lambda: 1, - rally_point=lambda i: rally[0])) + rally_count=lambda: 1, rally_point=lambda i: rally[0]) # ArduPilot logs the EKF origin and home together, and MAVProxy # keeps only the last of each type, so the origin is often unknown: # home stands in for it, and moves the point as home moves @@ -1867,9 +1866,7 @@ def test_the_live_map_takes_a_rally_points_own_altitude_frame(self): module.mpstate.module = lambda name: SimpleNamespace( wploader=module.mpstate.module('wp').wploader if name == 'wp' else None, - rallyloader=SimpleNamespace( - rally_count=lambda: len(rally), - rally_point=lambda i: rally[i])) + rally_count=lambda: len(rally), rally_point=lambda i: rally[i]) module.origin_amsl = 500.0 module.terrain_alt = lambda lat, lon: 300.0 points = module.rally_points((HOME[0], HOME[1], HOME[2])) @@ -1891,8 +1888,7 @@ def test_the_live_map_draws_rally_points_where_they_are_returned_to( flags=frame_valid | (frame << 3)) for frame in (0, 1, 2, 3)] module.mpstate.module = lambda name: SimpleNamespace( - rallyloader=SimpleNamespace(rally_count=lambda: len(rally), - rally_point=lambda i: rally[i])) + rally_count=lambda: len(rally), rally_point=lambda i: rally[i]) drawn = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) drawn.set_home(HOME[2]) module.map = SimpleNamespace( @@ -2211,7 +2207,10 @@ def test_the_live_map_returns_to_its_rally_points(self, monkeypatch): rally = SimpleNamespace(rally_points=[]) rally.rally_count = lambda: len(rally.rally_points) rally.rally_point = lambda i: rally.rally_points[i] - modules = SimpleNamespace(wploader=loader, rallyloader=rally) + modules = SimpleNamespace(wploader=loader, + rally_count=rally.rally_count, + rally_point=rally.rally_point, + last_change=lambda: 0) module.mpstate.module = lambda name: ( modules if name in ('wp', 'rally') else None) module.send_mission() From 5b2ba03049ce8870f8c2df2668cf5547d5cca0f2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 7 Oct 2026 17:18:17 +1100 Subject: [PATCH 21/22] map3d: draw a mission while its terrain is still coming, and by its frames Two things about the path the live map draws: - An item above terrain which has not been fetched yet was flown at whatever height the aircraft happened to be at, 390m out where the real ground was. Stand it above home until the terrain arrives, which is near enough to be worth drawing, and the mission is drawn again as soon as the terrain is known. - The path already drawn is kept against what the mission is, which folded an item's altitude frame away: 100m above a home at 584m and 100m AMSL were the same item, so changing one to the other drew the path worked out for the other. Keep the frame with the altitude. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/modules/mavproxy_map3d/__init__.py | 14 +++++--- tests/test_mission_geometry.py | 14 +++++--- tests/test_plane_track.py | 37 +++++++++++++++++++++ 3 files changed, 56 insertions(+), 9 deletions(-) diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index 78bf2b82dc..9a7c054cdc 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -342,9 +342,13 @@ def item_amsl(self, alt, frame): if frame in (0, 5): # already AMSL return alt if frame in (10, 11): - # still above terrain: send_mission() turns these into AMSL once - # it has the terrain height, and until then nobody knows - return None + # above terrain which has not been fetched yet: send_mission() + # turns these into AMSL once it has the terrain height, and + # until then home's height stands in for the ground's, which is + # nearer the mark than the height the aircraft happens to be at + if self.home_amsl is None: + return None + return self.home_amsl + alt if self.home_amsl is None: # relative to a home we have not been told about yet return None @@ -529,7 +533,9 @@ def flown_track(self, wploader, items, fixed_alt): def keyed_alt(amsl, fixed): if amsl is None or fixed is None: return None - return round(amsl if fixed else amsl - home[2], 2) + # which of the two it is matters as much as the number: 100m + # above home and 100m AMSL are not the same item + return (fixed, round(amsl if fixed else amsl - home[2], 2)) relative = tuple( (command, 'home' if (lat, lon) == home[:2] else (lat, lon), diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index f3bd0917db..3a819bda50 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -780,13 +780,17 @@ def test_frames_resolve_to_amsl(self): for frame in (3, 6): assert module.item_amsl(100.0, frame) == 684.0 - def test_a_terrain_altitude_without_terrain_is_unknown(self): - # send_mission() turns a terrain-frame item into AMSL when it has the - # terrain height; one still in frame 10 or 11 has no known height, - # and home is not a stand-in for the ground under it + def test_a_terrain_altitude_stands_above_home_until_it_is_known(self): + # send_mission() turns a terrain-frame item into AMSL when it has + # the terrain height; until then home's height stands in for the + # ground's, so there is a path to draw, and the mission is drawn + # again once the terrain arrives module = self.module(584.0) for frame in (10, 11): - assert module.item_amsl(100.0, frame) is None + assert module.item_amsl(100.0, frame) == 684.0 + # unless home is not known either + module = self.module(None) + assert module.item_amsl(100.0, 10) is None def test_a_climb_across_two_frames(self): module = self.module(584.0) diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index fb783f1d10..a5bb4b02d9 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -2129,6 +2129,43 @@ def home_position(amsl): home_position(HOME[2] + 100) assert len(module.sent) == drawn + def test_an_items_frame_is_part_of_which_mission_it_is(self): + """100m above home and 100m AMSL are not the same item, so a mission + changed from one to the other is flown again rather than drawn with + the path worked out for the other""" + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + last = loader.wpoints[-1] + (last.frame, last.z) = (3, 100.0) + module.send_mission() + above_home = nearest(module.sent[-1], last.x, last.y)[2] + assert above_home == pytest.approx(HOME[2] + 100, abs=30.0) + (last.frame, last.z) = (0, 100.0) + module.send_mission() + assert module.sent[-1] is not None + # 100m AMSL is a long way below a home at 584m, and the path drawn + # for it dives rather than holding the height of the other + assert nearest(module.sent[-1], last.x, last.y)[2] < above_home - 300 + + def test_a_waypoint_above_terrain_nobody_has_yet_stands_above_home(self): + """there is a path to draw before the terrain arrives: home's height + stands in for the ground's, and the mission is drawn again once the + terrain is known""" + module = self.live_module('plane') + module.terrain_alt = lambda lat, lon: None + loader = module.mpstate.module('wp').wploader + last = loader.wpoints[-1] + (last.frame, last.z) = (10, 100.0) + module.send_mission() + assert module.sent[-1] is not None + assert nearest(module.sent[-1], last.x, last.y)[2] == \ + pytest.approx(HOME[2] + 100, abs=30.0) + # and once it is known, at the height above that + module.terrain_alt = lambda lat, lon: HOME[2] + 300 + module.send_mission() + assert nearest(module.sent[-1], last.x, last.y)[2] == \ + pytest.approx(HOME[2] + 400, abs=30.0) + def test_the_live_map_redraws_for_a_changed_parameter(self): """how the vehicle flies a mission is its parameters' as much as its items': a parameter the path is flown by changing, or arriving for From 6b784a1a4c601f5e69c92078941d9598d64e2042 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 7 Oct 2026 17:19:53 +1100 Subject: [PATCH 22/22] MAVExplorer: know the whole of a rally table, and keep the views current A rally table is only a table to work a return out from when the whole of it is known, and two ways of knowing it were missed: the logger writing out a table with nothing in it -- 'New rally' and no points -- which is the whole of it, and a download which says how many points to expect and ends before any of them arrive, which is none of it. The first left a mission unflown for want of points which had been cleared away, and the second flew it home as though the vehicle held none. The settings an open 3D view is drawn by are also taken each time round the loop rather than only when a set command gives them, so changing the mission path, the labels or their size in the settings dialog reaches the view: choosing 'flown' there drew nothing until some later set command. Co-Authored-By: Claude Opus 5 (1M context) --- MAVProxy/tools/MAVExplorer.py | 43 ++++++++++++++++++++++-------- tests/test_map3d_settings_sync.py | 32 ++++++++++++++++++++++ tests/test_plane_track.py | 44 +++++++++++++++++++++++++++++++ 3 files changed, 108 insertions(+), 11 deletions(-) diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index 02b8f1ad72..f1542b8bee 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -630,6 +630,8 @@ def cmd_graphs(args): grui[-1].set_xlim(xlimits.last_xlim) map_timelim_pipes = [] +# the settings the open 3D views were last given +map3d_pushed = None def cmd_map(args): '''map command''' @@ -949,9 +951,12 @@ def mission_from_log(mlog, condition=None): A telemetry log carries the same things as the MAVLink messages the vehicle sent: its position, home and origin, what it said, and the mission and rally points as they were downloaded or uploaded while the - log was kept. Only a transfer which arrives whole replaces what was - held, so one the log ends part way through leaves the table before it, - and a log which starts part way through one holds no mission at all + log was kept. Only a mission which arrives whole replaces the one held, + so a transfer the log ends part way through leaves the mission before + it, and a log which starts part way through one holds no mission at all; + the rally points which arrive are the table, since what the old protocol + set before them is gone whether or not the new table ever arrives whole, + and they say so ''' path = [] # the mission and the rally points, as whatever kind of log this is @@ -1039,9 +1044,13 @@ def mission_from_log(mlog, condition=None): after = rally_points(rally_log) if after != before or rally_log.arrivals != arrivals: rally = after - # a table which never arrived whole is what is known of the - # one the vehicle holds, and all that is known: the points - # the old protocol set before it are gone + if mtype in ('MISSION_COUNT', 'MISSION_CLEAR_ALL') or \ + after != before: + # what arrived is what is known of the table the vehicle + # holds, and all that is known: the points the old protocol + # set before it are gone. A download which says how many to + # expect and ends before any of them arrive leaves none of + # it, which is not the table either rally_whole = not rally_log.partial() if mtype in log_mission.MISSION_TYPES: # the mission itself, which LogMission keeps: only the last one @@ -1086,8 +1095,10 @@ def mission_from_log(mlog, condition=None): pass elif m.Message == 'New rally': # written before the whole table, even an empty one: a - # cleared table is this and no points at all + # cleared table is this and no points at all, which is the + # whole of it rally = {} + rally_whole = True elif (len(fields) > 1 and fields[0] == 'Mission:' and fields[1].isdigit() and path): flown_from.setdefault(int(fields[1]), (path[-1][0], path[-1][1])) @@ -1412,6 +1423,8 @@ def cmd_set(args): def update_map3d_views(): '''push the settings the open 3D views care about''' + global map3d_pushed + map3d_pushed = map3d_settings() for view in map3d_views: if view.is_alive(): view.set_mission_arrows(mestate.settings.showdirection) @@ -1433,11 +1446,19 @@ def say_unflown(): "finishes an item, uses a command which cannot be flown here, or " "which the log does not hold all of; drawing its geometry") +def map3d_settings(): + '''the settings the open 3D views are drawn by''' + return (mestate.settings.showdirection, mestate.settings.showlabels, + mestate.settings.labelsize, mestate.settings.missionpath) + + def poll_map3d_views(): - '''take what the open 3D views' own controls have been set to. A view - which has died is drained too, so we do not lose the reason it failed - to start''' - changed = False + '''take what the open 3D views' own controls have been set to, and give + them any setting changed since they were last told -- by the settings + dialog as much as by a set command. A view which has died is drained + too, so we do not lose the reason it failed to start''' + global map3d_pushed + changed = map3d_settings() != map3d_pushed for view in map3d_views: for event in view.check_events(): if event[0] == 'startup_error': diff --git a/tests/test_map3d_settings_sync.py b/tests/test_map3d_settings_sync.py index 3e77e8e21a..59c6a05499 100644 --- a/tests/test_map3d_settings_sync.py +++ b/tests/test_map3d_settings_sync.py @@ -192,6 +192,38 @@ def test_mavexplorer_and_its_views_agree(self, map3d_frame, mavexplorer, # view's, which it read after the command assert settings.showlabels is True + def test_mavexplorer_gives_a_view_a_setting_changed_anywhere( + self, map3d_frame, mavexplorer, monkeypatch): + '''the settings dialog writes the setting and says nothing, so what + the views are drawn by is taken each time round rather than only + when a set command gives it''' + from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting + settings = MPSettings([ + MPSetting('showdirection', bool, True), + MPSetting('showlabels', bool, False), + MPSetting('labelsize', int, 14), + MPSetting('missionpath', str, 'flown', + choice=['flown', 'geometry', 'plain'])]) + monkeypatch.setattr(mavexplorer, 'mestate', + SimpleNamespace(settings=settings), raising=False) + (viewer, frame) = connect(map3d_frame) + monkeypatch.setattr(mavexplorer, 'map3d_views', [viewer]) + monkeypatch.setattr(mavexplorer, 'map3d_pushed', None, + raising=False) + mavexplorer.poll_map3d_views() + deliver(viewer, frame) + assert frame.mission_style == 'flown' + # the dialog's own doing: the setting changed, with nothing said + settings.missionpath = 'plain' + settings.labelsize = 20 + mavexplorer.poll_map3d_views() + deliver(viewer, frame) + assert frame.mission_style == 'plain' + assert frame.mission_label_size == 20 + # and nothing more is sent while nothing changes + mavexplorer.poll_map3d_views() + assert viewer.object_queue.empty() + def display(): return bool(os.environ.get('DISPLAY') or os.environ.get('WAYLAND_DISPLAY')) diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py index a5bb4b02d9..9be3c63479 100644 --- a/tests/test_plane_track.py +++ b/tests/test_plane_track.py @@ -2741,6 +2741,50 @@ def legacy(seq, count, north, east): cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, path, started, rally, origin) is not None + def test_mavexplorer_flies_a_mission_once_a_rally_table_is_cleared(self): + """a table the log holds only part of leaves the mission unflown, + but a table cleared away afterwards is the whole of what the vehicle + holds: none of it, and the mission is flown home again""" + mx = self.explorer() + + def legacy(seq, count, north, east): + (lat, lon, _) = offset(north, east) + return self.mavlink(mavlink.MAVLink_rally_point_message( + 255, 1, seq, count, int(lat * 1e7), int(lon * 1e7), 120, + 0, 0, 0), 1) + + def flown(log): + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + return mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) + # one point of a table of two: not a table to work a return out from + assert flown(self.telemetry_mission(legacy(0, 2, -2000, 0))) is None + # and then the logger writing out a table with nothing in it + assert flown(self.telemetry_mission( + legacy(0, 2, -2000, 0), + self.message('MSG', Message='New rally'))) is not None + + def test_mavexplorer_does_not_fly_a_rally_download_which_brought_none( + self): + """a download which says how many points to expect and ends before + any of them arrive leaves nothing known of the table: the mission is + not flown home as though the vehicle held no points""" + mx = self.explorer() + log = self.telemetry_mission( + self.mission_count(255, 2, mavlink.MAV_MISSION_TYPE_RALLY, 1)) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + assert rally == [] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is None + def test_mavexplorer_drops_rally_points_cleared_away(self): """MISSION_CLEAR_ALL for the rally table, and for everything, which takes the mission with it"""