From 741661840aac3c696a0946fc4224745bae171d56 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 30 Jul 2026 16:17:54 +1000 Subject: [PATCH 1/3] bvlos_plan: add BVLOS planning module with return path check Planning a BVLOS mission means knowing that an RTL from anywhere along it returns through airspace that was surveyed and approved. With RTL_AUTOLAND=4 an ArduPlane RTL calls AP_Mission::jump_to_closest_mission_leg(), which makes the end waypoint of the closest leg after a DO_RETURN_PATH_START current, and the aircraft then flies direct to it with no crosstrack. That cut across can leave the mission corridor entirely; ArduPilot puts no distance limit on it and does not test it against a fence. Return Path Check samples the mission every 50m and, for each point, works out what ArduPilot would pick and how far the resulting line gets from the mission path. The allowed distance is the turn radius the vehicle can achieve at its cruise airspeed and bank limit at that altitude, so a return that stays inside it is one the aircraft could have flown as part of the mission. Failing stretches are reported with the leg they are on and highlighted on the map in their own layer, which Clear Highlight removes without disturbing the mission. The return path walk follows AP_Mission: DO_JUMP and DO_JUMP_TAG are followed with a 64 loop cap and a finite jump counter zeroed once its loop completes, the search shares a 1000 command budget and a candidate that exhausts it is rejected rather than used truncated, zero length legs are skipped without advancing prev_loc, the first vertex of a path is measured as a point rather than a leg, ties keep the earliest leg within a path and the last DO_RETURN_PATH_START across paths, the walk stops at a landing or DO_LAND_START inclusive, and because set_current_cmd() runs advance_current_nav_cmd() the target resolves forward to the next navigation command, following a DO_JUMP to reach it. A point whose return path reaches no navigation command is an error, not a pass: set_current_cmd() fails there, so an RTL follows nothing. The corridor itself is the navigation commands only. Commands that merely carry a location, such as DO_SET_HOME, DO_SET_ROI and DO_LAND_START, are never flown to and would invent corridor that does not exist. Geometry runs in a local metric frame using ArduPilot's own scaling so distances match get_distance_NED_alt_frame, projected once for the whole mission rather than per operation, which keeps a 110km mission under a few seconds. Leg selection is 3D as in ArduPilot while the corridor test is horizontal, as it is about lateral containment. Altitudes come from the terrain module where the frame needs it, and terrain that cannot be resolved is reported rather than quietly assumed. mission_model.py has no dependency on MAVProxy state so it can be driven offline. The check is sensitive to the airspeed and bank limit, as it should be, so cruise_airspeed and roll_limit settings can override the vehicle parameters for planning without a vehicle connected. return_path_width instead gives a fixed distance either side of the path, for when the corridor is set by the approval rather than by what the aircraft can fly. Not loaded by default: "module load bvlos_plan". --- .../modules/mavproxy_bvlos_plan/__init__.py | 297 ++++++++++++ .../mavproxy_bvlos_plan/mission_model.py | 443 ++++++++++++++++++ .../mavproxy_bvlos_plan/return_path.py | 342 ++++++++++++++ setup.py | 1 + 4 files changed, 1083 insertions(+) create mode 100644 MAVProxy/modules/mavproxy_bvlos_plan/__init__.py create mode 100644 MAVProxy/modules/mavproxy_bvlos_plan/mission_model.py create mode 100644 MAVProxy/modules/mavproxy_bvlos_plan/return_path.py diff --git a/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py b/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py new file mode 100644 index 0000000000..9acf705b75 --- /dev/null +++ b/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py @@ -0,0 +1,297 @@ +#!/usr/bin/env python3 +''' +BVLOS planning module. + +Assists with planning complex BVLOS missions. Not loaded by default, load it +with "module load bvlos_plan". + +Adds a BVLOS submenu to the map's right click menu when the map is loaded. +The first check is Return Path Check, which verifies that a +DO_RETURN_PATH_START is safe: that wherever an RTL is started along the +mission, the return ArduPilot picks stays within a turn radius of the mission +path. See return_path.py. +''' + +# AP_FLAKE8_CLEAN + +from MAVProxy.modules.lib import mp_module +from MAVProxy.modules.lib import mp_settings +from MAVProxy.modules.lib import mp_util + +from MAVProxy.modules.mavproxy_bvlos_plan import mission_model +from MAVProxy.modules.mavproxy_bvlos_plan import return_path + +if mp_util.has_wxpython: + from MAVProxy.modules.lib.mp_menu import MPMenuItem + from MAVProxy.modules.lib.mp_menu import MPMenuSubMenu + +# our own map layer, so it can be cleared without touching the mission +MAP_LAYER = 'BVLOSReturnPath' + +# colour of the highlighted parts of the mission +FAIL_COLOUR = (255, 0, 0) +FAIL_LINEWIDTH = 4 + +# RTL_AUTOLAND value that makes ArduPlane use a DO_RETURN_PATH_START, see +# RtlAutoland in ArduPlane/defines.h +RTL_AUTOLAND_RETURN_PATH = 4 + + +class BvlosPlanModule(mp_module.MPModule): + def __init__(self, mpstate): + super(BvlosPlanModule, self).__init__(mpstate, "bvlos_plan", + "BVLOS planning") + self.menu_added_map = False + self.menu = None + if mp_util.has_wxpython: + self.menu = MPMenuSubMenu( + 'BVLOS', + items=[ + MPMenuItem('Return Path Check', 'Return Path Check', + '# bvlos_plan returncheck'), + MPMenuItem('Clear Highlight', 'Clear Highlight', + '# bvlos_plan clear'), + ]) + self.bvlos_settings = mp_settings.MPSettings([ + ('granularity', float, 50.0), + # 0 means take it from the vehicle parameters + ('cruise_airspeed', float, 0.0), + ('roll_limit', float, 0.0), + # if set, the distance either side of the mission path that the + # return may use, in metres, instead of the turn radius + ('return_path_width', float, 0.0), + ]) + self.add_command('bvlos_plan', self.cmd_bvlos_plan, + "BVLOS planning", ['returncheck', 'clear', + 'set (BVLOSPLANSETTING)']) + self.add_completion_function('(BVLOSPLANSETTING)', + self.bvlos_settings.completion) + + def usage(self): + return "Usage: bvlos_plan " + + def cmd_bvlos_plan(self, args): + if len(args) == 0: + print(self.usage()) + return + if args[0] == "returncheck": + self.cmd_returncheck() + elif args[0] == "clear": + self.clear_highlight() + elif args[0] == "set": + self.bvlos_settings.command(args[1:]) + else: + print(self.usage()) + + def terrain_function(self): + '''terrain lookup, or None if the terrain module is not loaded. + + The terrain module owns the elevation model and rebuilds it when + the source is changed, so go through the module each time rather + than holding onto the model. + ''' + terrain = self.module('terrain') + if terrain is None: + return None + + def lookup(lat, lon): + return terrain.ElevationModel.GetElevation(lat, lon) + + return lookup + + def cruise_airspeed(self): + '''cruise airspeed as EAS in m/s, or None''' + if self.bvlos_settings.cruise_airspeed > 0: + return self.bvlos_settings.cruise_airspeed + value = self.get_mav_param('AIRSPEED_CRUISE', None) + if value is not None and value > 0: + return float(value) + # renamed in Plane 4.5, older vehicles hold it in cm/s + value = self.get_mav_param('TRIM_ARSPD_CM', None) + if value is not None and value > 0: + return float(value) * 0.01 + return None + + def roll_limit(self): + '''bank angle limit in degrees, or None''' + if self.bvlos_settings.roll_limit > 0: + return self.bvlos_settings.roll_limit + value = self.get_mav_param('ROLL_LIMIT_DEG', None) + if value is not None and value > 0: + return float(value) + # renamed in Plane 4.5, older vehicles hold it in centidegrees + value = self.get_mav_param('LIM_ROLL_CD', None) + if value is not None and value > 0: + return float(value) * 0.01 + return None + + def mission_items(self): + '''the loaded mission, or None with a reason printed''' + wp = self.module('wp') + if wp is None: + print("bvlos_plan: the wp module is not loaded") + return None + loader = wp.wploader + count = loader.count() + if count == 0: + print("bvlos_plan: no mission loaded, try 'wp list'") + return None + # only refuse if the mission we hold is actually incomplete. Note that + # a "wp load" leaves loading_waypoints set while it uploads, which is + # no reason not to check the mission we already have + expected = getattr(loader, 'expected_count', 0) + if expected and count < expected: + print("bvlos_plan: only have %u of %u mission items, still loading" + % (count, expected)) + return None + return [loader.wp(i) for i in range(count)] + + def run_check(self, items): + '''run the return path check, returning (mission, result, cruise, + roll) or None with a reason printed''' + width = self.bvlos_settings.return_path_width + cruise = self.cruise_airspeed() + roll = self.roll_limit() + if width <= 0 and (cruise is None or roll is None): + print("bvlos_plan: need cruise airspeed and bank limit for the " + "turn radius. Connect to a vehicle, set them with " + "'bvlos_plan set cruise_airspeed' and 'bvlos_plan set " + "roll_limit', or give a fixed distance with 'bvlos_plan set " + "return_path_width'") + return None + terrain_fn = self.terrain_function() + mission = mission_model.build_mission(items, terrain_fn=terrain_fn) + result = return_path.check_return_path( + mission, cruise, roll, + granularity=self.bvlos_settings.granularity, + terrain_fn=terrain_fn, width=width) + return (mission, result, cruise, roll) + + def cmd_returncheck(self): + '''check that a DO_RETURN_PATH_START is safe''' + items = self.mission_items() + if items is None: + return + run = self.run_check(items) + if run is None: + return + (mission, result, cruise, roll) = run + self.report(result, mission, cruise, roll) + self.highlight(result, mission) + + def report(self, result, mission, cruise, roll): + '''print the outcome of a check''' + if result.fixed_width is not None: + print("Return path check: allowing %.0fm either side of the " + "mission path, sampled every %.0fm" + % (result.fixed_width, self.bvlos_settings.granularity)) + else: + print("Return path check: turn radius from cruise %.1fm/s and " + "bank limit %.0fdeg, sampled every %.0fm" + % (cruise, roll, self.bvlos_settings.granularity)) + if mission.terrain_missing: + print(" WARNING: %u mission items need terrain that is not " + "available, so their altitudes are approximate. Try " + "'terrain set source SRTM1'" % mission.terrain_missing) + autoland = self.get_mav_param('RTL_AUTOLAND', None) + if autoland is not None and int(autoland) != RTL_AUTOLAND_RETURN_PATH: + print(" WARNING: RTL_AUTOLAND is %u, so an RTL will not use the " + "return path at all (needs %u)" + % (int(autoland), RTL_AUTOLAND_RETURN_PATH)) + for err in result.errors: + print(" ERROR: %s" % err) + if len(result.errors) > 0: + return + + print(" DO_RETURN_PATH_START at %s" % + ', '.join(str(s) for s in result.return_path_starts)) + if result.worst_radius is not None: + allowed = "%.0fm allowed" % result.worst_radius + if result.fixed_width is None: + allowed = "%.0fm turn radius" % result.worst_radius + print(" worst case: %.0fm from the mission path against %s, on " + "the leg %u->%u, rejoining at waypoint %u" + % (result.worst_deviation, allowed, + result.worst_leg[0], result.worst_leg[1], + result.worst_rejoin)) + if result.failed == 0: + print(" PASS: all %u points along the mission return within %s " + "of the mission path" % (result.checked, self.metric_name(result))) + return + print(" FAIL: %u of %u points along the mission would return outside " + "%s of the mission path" + % (result.failed, result.checked, self.metric_name(result))) + for span in result.spans: + legs = sorted(span.legs) + print(" legs %s: worst %.0fm" + % (', '.join("%u->%u" % leg for leg in legs), span.worst)) + + def metric_name(self, result): + '''how the allowed distance was arrived at, for the report''' + if result.fixed_width is not None: + return "the %.0fm return path width" % result.fixed_width + return "a turn radius" + + def maps(self): + '''every loaded map instance''' + return [m for m in self.module_matching('map*')] + + def clear_highlight(self): + '''remove our own map layer, leaving everything else alone''' + if not mp_util.has_wxpython: + return + from MAVProxy.modules.mavproxy_map import mp_slipmap + for m in self.maps(): + m.map.add_object(mp_slipmap.SlipClearLayer(MAP_LAYER)) + + def highlight(self, result, mission): + '''draw the failing parts of the mission path on the map''' + if not mp_util.has_wxpython: + return + maps = self.maps() + if len(maps) == 0: + if result.failed: + print(" (load the map module to see the failing parts " + "highlighted)") + return + from MAVProxy.modules.mavproxy_map import mp_slipmap + self.clear_highlight() + for (i, span) in enumerate(result.spans): + points = [mission.projector.unproject(x, y) for (x, y) in span.points] + for m in maps: + if len(points) < 2: + # one failing sample has no line to draw, so mark it + radius = max(self.bvlos_settings.granularity * 0.5, 25.0) + m.map.add_object(mp_slipmap.SlipCircle( + 'bvlos_return_fail_%u' % i, MAP_LAYER, points[0], + radius, FAIL_COLOUR, linewidth=FAIL_LINEWIDTH)) + continue + m.map.add_object(mp_slipmap.SlipPolygon( + 'bvlos_return_fail_%u' % i, points, + layer=MAP_LAYER, linewidth=FAIL_LINEWIDTH, + colour=FAIL_COLOUR, showcircles=False)) + + def idle_task(self): + '''add our menu to the map, and notice the map going away''' + if self.menu is None: + return + if self.module('map') is not None: + if not self.menu_added_map: + self.menu_added_map = True + self.module('map').add_menu(self.menu) + else: + self.menu_added_map = False + + def unload(self): + '''unload module''' + self.clear_highlight() + if self.menu is not None and self.module('map') is not None: + self.module('map').remove_menu(self.menu) + self.menu_added_map = False + self.remove_command('bvlos_plan') + super(BvlosPlanModule, self).unload() + + +def init(mpstate): + '''initialise module''' + return BvlosPlanModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_bvlos_plan/mission_model.py b/MAVProxy/modules/mavproxy_bvlos_plan/mission_model.py new file mode 100644 index 0000000000..44de305fd9 --- /dev/null +++ b/MAVProxy/modules/mavproxy_bvlos_plan/mission_model.py @@ -0,0 +1,443 @@ +#!/usr/bin/env python3 +''' +Mission geometry for the BVLOS planning checks. + +Deliberately free of any MAVProxy or vehicle state, so a mission can be +checked offline from a file. Mission items are anything with the mavlink +mission item interface (seq, command, frame, x, y, z), which covers both the +wp module's wploader and mavwp.MAVWPLoader reading a mission file. +''' + +# AP_FLAKE8_CLEAN + +import math + +from pymavlink import mavutil + +mavlink = mavutil.mavlink + +# ArduPilot constants, see libraries/AP_Math/definitions.h +GRAVITY_MSS = 9.80665 +SSL_AIR_DENSITY = 1.225 +ISA_LAPSE_RATE = 0.0065 +SSL_TEMPERATURE = 288.15 +# the 1976 standard atmosphere gas constant used by AP_Baro_atmosphere.cpp +R_SPECIFIC = 287.053072 + +# metres per degree of latitude, matching LOCATION_SCALING_FACTOR in +# libraries/AP_Common/Location.h +METRES_PER_DEG = 0.011131884502145034 * 1.0e7 + +# commands carrying a location, mirroring AP_Mission::stored_in_location() +LOCATION_COMMANDS = frozenset([ + mavlink.MAV_CMD_NAV_WAYPOINT, + mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavlink.MAV_CMD_NAV_LOITER_TIME, + mavlink.MAV_CMD_NAV_LAND, + mavlink.MAV_CMD_NAV_TAKEOFF, + mavlink.MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT, + mavlink.MAV_CMD_NAV_LOITER_TO_ALT, + mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT, + mavlink.MAV_CMD_NAV_GUIDED_ENABLE, + mavlink.MAV_CMD_DO_SET_HOME, + mavlink.MAV_CMD_DO_RETURN_PATH_START, + mavlink.MAV_CMD_DO_LAND_START, + mavlink.MAV_CMD_DO_GO_AROUND, + mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + mavlink.MAV_CMD_DO_SET_ROI, + mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, + mavlink.MAV_CMD_NAV_VTOL_LAND, + mavlink.MAV_CMD_NAV_PAYLOAD_PLACE, +] + ([mavlink.MAV_CMD_NAV_ARC_WAYPOINT] + if hasattr(mavlink, 'MAV_CMD_NAV_ARC_WAYPOINT') else [])) + +# commands that end a return path, AP_Mission::is_landing_type_cmd() +LANDING_COMMANDS = frozenset([ + mavlink.MAV_CMD_NAV_LAND, + mavlink.MAV_CMD_NAV_VTOL_LAND, + mavlink.MAV_CMD_DO_PARACHUTE, +]) + +# AP_Mission::is_nav_cmd(): everything up to NAV_LAST, plus these +# smallest sample spacing we will use, so a mistyped setting cannot divide by +# zero or ask for an unbounded amount of work +MIN_SPACING = 1.0 + +NAV_LAST = mavlink.MAV_CMD_NAV_LAST +EXTRA_NAV_COMMANDS = frozenset([ + mavlink.MAV_CMD_NAV_SET_YAW_SPEED, + getattr(mavlink, 'MAV_CMD_NAV_SCRIPT_TIME', 42702), + getattr(mavlink, 'MAV_CMD_NAV_ATTITUDE_TIME', 42703), +]) + +RELATIVE_FRAMES = frozenset([ + mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, +]) +TERRAIN_FRAMES = frozenset([ + mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, + mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT, +]) + + +def is_nav_command(command): + '''AP_Mission::is_nav_cmd()''' + return command <= NAV_LAST or command in EXTRA_NAV_COMMANDS + + +def command_name(command): + '''readable name for a MAV_CMD''' + enums = mavlink.enums['MAV_CMD'] + if command in enums: + return enums[command].name.replace('MAV_CMD_', '') + return str(command) + + +def wrap_180(angle): + '''wrap a longitude difference to -180..180''' + return (angle + 180.0) % 360.0 - 180.0 + + +def eas2tas(alt_amsl): + '''equivalent to true airspeed ratio at an altitude, matching the + gradient layer of AP_Baro::get_air_density_for_alt_amsl()''' + temp = SSL_TEMPERATURE - ISA_LAPSE_RATE * alt_amsl + if temp <= 0: + # far above anywhere an aircraft flies + return 1.0 + exponent = GRAVITY_MSS / (ISA_LAPSE_RATE * R_SPECIFIC) - 1.0 + density = SSL_AIR_DENSITY * (temp / SSL_TEMPERATURE) ** exponent + if density <= 0: + return 1.0 + return math.sqrt(SSL_AIR_DENSITY / density) + + +def turn_radius(cruise_eas, roll_limit_deg, alt_amsl): + '''fixed wing turn radius in metres, from the coordinated turn relation + used by fixedwing_turn_rate() in libraries/AP_Math/AP_Math.cpp''' + bank = max(1.0, min(float(roll_limit_deg), 80.0)) + tas = cruise_eas * eas2tas(alt_amsl) + return (tas * tas) / (GRAVITY_MSS * math.tan(math.radians(bank))) + + +class Projector(object): + '''project lat/lon onto a local metric frame. + + Uses ArduPilot's own equirectangular scaling so distances match + Location::get_distance_NED_alt_frame(), except that the longitude + scale is taken at one reference latitude rather than at the mean + latitude of each pair. Over a mission spanning a degree of latitude + that is a few tenths of a percent, immaterial against a turn radius + tolerance of tens of metres. + ''' + + def __init__(self, lat0, lon0): + self.lat0 = lat0 + self.lon0 = lon0 + self.lon_scale = max(math.cos(math.radians(lat0)), 0.01) + + def project(self, lat, lon): + '''return (east, north) metres from the reference''' + return (wrap_180(lon - self.lon0) * METRES_PER_DEG * self.lon_scale, + (lat - self.lat0) * METRES_PER_DEG) + + def unproject(self, x, y): + '''return (lat, lon) for a point in metres''' + lat = self.lat0 + y / METRES_PER_DEG + lon = self.lon0 + x / (METRES_PER_DEG * self.lon_scale) + return (lat, wrap_180(lon)) + + +def segment_distance(px, py, ax, ay, bx, by): + '''distance from a point to a closed 2D segment''' + vx = bx - ax + vy = by - ay + wx = px - ax + wy = py - ay + d2 = vx * vx + vy * vy + if d2 <= 0.0: + return math.hypot(wx, wy) + t = (vx * wx + vy * wy) / d2 + t = max(0.0, min(1.0, t)) + return math.hypot(wx - t * vx, wy - t * vy) + + +def segment_distance_3d(px, py, pz, ax, ay, az, bx, by, bz): + '''distance from a point to a closed 3D segment, which is what + AP_Mission::distance_to_mission_leg() measures''' + vx = bx - ax + vy = by - ay + vz = bz - az + wx = px - ax + wy = py - ay + wz = pz - az + d2 = vx * vx + vy * vy + vz * vz + if d2 <= 0.0: + return math.sqrt(wx * wx + wy * wy + wz * wz) + t = (vx * wx + vy * wy + vz * wz) / d2 + t = max(0.0, min(1.0, t)) + dx = wx - t * vx + dy = wy - t * vy + dz = wz - t * vz + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +class PathIndex(object): + '''pre-chewed segments of a path, so the distance from a point to the + whole path is a tight loop over plain floats''' + + def __init__(self, points): + self.segments = [] + for i in range(1, len(points)): + a = points[i - 1] + b = points[i] + vx = b.x - a.x + vy = b.y - a.y + d2 = vx * vx + vy * vy + if d2 <= 0.0: + continue + self.segments.append((a.x, a.y, vx, vy, d2)) + self.fallback = (points[0].x, points[0].y) if len(points) else None + + def distance(self, px, py): + '''smallest distance from a point to the path''' + best = None + for (ax, ay, vx, vy, d2) in self.segments: + wx = px - ax + wy = py - ay + t = (vx * wx + vy * wy) / d2 + if t < 0.0: + t = 0.0 + elif t > 1.0: + t = 1.0 + dx = wx - t * vx + dy = wy - t * vy + d = dx * dx + dy * dy + if best is None or d < best: + best = d + if best is None: + if self.fallback is None: + return 0.0 + return math.hypot(px - self.fallback[0], py - self.fallback[1]) + return math.sqrt(best) + + +class MissionPoint(object): + '''a mission item reduced to what the checks need''' + + def __init__(self, seq, command, frame, lat, lon, alt, param1=0, param2=0): + self.seq = seq + self.command = command + self.frame = frame + self.lat = lat + self.lon = lon + # altitude as stored, in its own frame + self.alt = alt + # DO_JUMP uses param1 as the target and param2 as the repeat count + self.param1 = param1 + self.param2 = param2 + # resolved by build_mission() + self.amsl = None + self.ground = None + self.x = None + self.y = None + + def initialised(self): + '''Location::initialised(). A non zero altitude alone makes a + location count as valid, which ArduPilot relies on''' + return self.lat != 0 or self.lon != 0 or self.alt != 0 + + def has_location(self): + return self.command in LOCATION_COMMANDS and self.initialised() + + def is_landing(self): + return self.command in LANDING_COMMANDS + + def is_nav(self): + return is_nav_command(self.command) + + def is_terrain_frame(self): + return self.frame in TERRAIN_FRAMES + + def __str__(self): + return "%u:%s" % (self.seq, command_name(self.command)) + + +class Mission(object): + '''a mission with altitudes resolved to AMSL and positions projected''' + + def __init__(self, points, home_amsl, projector, terrain_missing=0): + self.points = points + self.home_amsl = home_amsl + self.projector = projector + # how many items needed terrain we did not have + self.terrain_missing = terrain_missing + + def count(self): + return len(self.points) + + def point(self, index): + if 0 <= index < len(self.points): + return self.points[index] + return None + + def return_path_starts(self): + return [p.seq for p in self.points + if p.command == mavlink.MAV_CMD_DO_RETURN_PATH_START] + + def flown_path(self): + '''where the aircraft actually goes: navigation items with a location, + after home, up to and including the first landing. + + Only navigation commands count. DO_SET_HOME, DO_SET_ROI, + DO_LAND_START and DO_RETURN_PATH_START carry a location but are + never flown to, and treating them as corridor would invent + corridor that does not exist and could hide an unsafe cut. + + Anything past the landing is not flown either. Both reference + BVLOS missions carry LOITER_TURNS and DO_JUMP pairs after the + landing, which the operator selects rather than flies, and which + ArduPilot's own return path walk never reaches because it stops at + the landing. + ''' + path = [] + for p in self.points[1:]: + if p.is_nav() and p.has_location(): + path.append(p) + if p.is_landing(): + break + return path + + def ends_in_landing(self): + '''True if the flown path finishes at a landing''' + path = self.flown_path() + return len(path) > 0 and path[-1].is_landing() + + +def build_mission(items, terrain_fn=None): + '''build a Mission from mavlink mission items. items[0] is home, whose + altitude is the reference for relative frames''' + points = [MissionPoint(it.seq, it.command, it.frame, it.x, it.y, it.z, + getattr(it, 'param1', 0), getattr(it, 'param2', 0)) + for it in items] + if len(points) == 0: + return Mission([], 0.0, Projector(0.0, 0.0)) + + home_amsl = points[0].alt + located = [p for p in points if p.has_location()] + if len(located) == 0: + return Mission(points, home_amsl, + Projector(points[0].lat, points[0].lon)) + + # reference the projection at the middle of the mission so the longitude + # scale error is spread rather than piling up at one end. Unwrap the + # longitudes about the first point first, or a mission either side of the + # antimeridian would be referenced half way round the world + base_lon = located[0].lon + lons = [base_lon + wrap_180(p.lon - base_lon) for p in located] + lat0 = 0.5 * (min(p.lat for p in located) + max(p.lat for p in located)) + lon0 = wrap_180(0.5 * (min(lons) + max(lons))) + projector = Projector(lat0, lon0) + + terrain_missing = 0 + for p in points: + (p.x, p.y) = projector.project(p.lat, p.lon) + if p.is_terrain_frame(): + p.ground = terrain_fn(p.lat, p.lon) if terrain_fn is not None else None + if p.ground is None: + terrain_missing += 1 + # no terrain, so treat the height above ground as being above + # home. Wrong, but far closer than taking it as AMSL, which + # would put a 90m AGL waypoint underground. The caller reports + # how many items this happened to + p.amsl = home_amsl + p.alt + else: + p.amsl = p.ground + p.alt + elif p.frame in RELATIVE_FRAMES: + p.amsl = home_amsl + p.alt + else: + # anything else is treated as absolute, the likeliest meaning of + # an unexpected frame + p.amsl = p.alt + + return Mission(points, home_amsl, projector, terrain_missing) + + +class PathSample(object): + '''a point along the mission path''' + + def __init__(self, x, y, amsl, leg_from, leg_to, distance): + self.x = x + self.y = y + self.amsl = amsl + # sequence numbers of the items at either end of the leg + self.leg_from = leg_from + self.leg_to = leg_to + # distance along the whole path + self.distance = distance + + +class SampleStats(object): + '''what happened while sampling, so the caller can report it''' + + def __init__(self): + self.terrain_missing = 0 + + +def sample_path(path, spacing, projector=None, terrain_fn=None, stats=None): + '''sample a list of MissionPoints at the given spacing in metres. + + A leg into a terrain frame waypoint follows the terrain, as ArduPilot + interpolates height above ground rather than AMSL for those legs, so + the sample altitude is the ground beneath it plus the interpolated + height above ground. + ''' + samples = [] + spacing = max(float(spacing), MIN_SPACING) + if len(path) == 0: + return samples + samples.append(PathSample(path[0].x, path[0].y, path[0].amsl, + path[0].seq, path[0].seq, 0.0)) + travelled = 0.0 + for i in range(1, len(path)): + a = path[i - 1] + b = path[i] + leg = math.hypot(b.x - a.x, b.y - a.y) + if leg <= 0: + continue + # follow the terrain only if we can look it up and know where the + # leg started in height above ground terms + follow_terrain = (b.is_terrain_frame() and terrain_fn is not None and + projector is not None and b.ground is not None) + start_agl = None + if follow_terrain: + if a.is_terrain_frame() and a.ground is not None: + start_agl = a.alt + elif a.ground is not None: + start_agl = a.amsl - a.ground + else: + ground_a = terrain_fn(a.lat, a.lon) + if ground_a is not None: + start_agl = a.amsl - ground_a + if start_agl is None: + follow_terrain = False + steps = max(1, int(math.ceil(leg / spacing))) + for step in range(1, steps + 1): + frac = float(step) / steps + x = a.x + (b.x - a.x) * frac + y = a.y + (b.y - a.y) * frac + amsl = None + if follow_terrain: + (slat, slon) = projector.unproject(x, y) + ground = terrain_fn(slat, slon) + if ground is not None: + amsl = ground + start_agl + (b.alt - start_agl) * frac + elif stats is not None: + stats.terrain_missing += 1 + if amsl is None: + amsl = a.amsl + (b.amsl - a.amsl) * frac + samples.append(PathSample(x, y, amsl, a.seq, b.seq, + travelled + leg * frac)) + travelled += leg + return samples diff --git a/MAVProxy/modules/mavproxy_bvlos_plan/return_path.py b/MAVProxy/modules/mavproxy_bvlos_plan/return_path.py new file mode 100644 index 0000000000..2c32ee0e55 --- /dev/null +++ b/MAVProxy/modules/mavproxy_bvlos_plan/return_path.py @@ -0,0 +1,342 @@ +#!/usr/bin/env python3 +''' +DO_RETURN_PATH_START safety check for BVLOS missions. + +With RTL_AUTOLAND=4 an ArduPlane RTL calls +AP_Mission::jump_to_closest_mission_leg(), which finds the closest leg of the +mission after a DO_RETURN_PATH_START and makes that leg's end waypoint the +current command. The aircraft then flies direct to that waypoint, with no +crosstrack on that first leg, so the return can cut across ground the mission +never covers. ArduPilot puts no distance limit on this and does not check it +against a fence. + +This checks that for every point along the mission, that cut-across stays +within one turn radius of the mission path, so an RTL from anywhere keeps the +aircraft inside the corridor the mission already surveys. The turn radius is +what the vehicle can achieve at its cruise airspeed and bank limit, at the +altitude of that point of the mission. + +The model of the ArduPilot behaviour follows +libraries/AP_Mission/AP_Mission.cpp jump_to_closest_mission_leg() and +distance_to_mission_leg(). +''' + +# AP_FLAKE8_CLEAN + +import math + +from pymavlink import mavutil + +from MAVProxy.modules.mavproxy_bvlos_plan import mission_model + +mavlink = mavutil.mavlink + +# AP_Mission::jump_to_closest_mission_leg() budget, shared across all of the +# DO_RETURN_PATH_START candidates +SEARCH_BUDGET = 1000 + +# max_loops in AP_Mission::get_next_cmd() +MAX_JUMP_LOOPS = 64 + +# AP_MISSION_JUMP_REPEAT_FOREVER +JUMP_REPEAT_FOREVER = -1 + + +class ReturnPath(object): + '''the located points reached from one DO_RETURN_PATH_START, in order''' + + def __init__(self, start_seq, points): + self.start_seq = start_seq + self.points = points + + +def next_command(mission, index, jump_counts): + '''AP_Mission::get_next_cmd(): the next non jump command at or after + index, following DO_JUMP. Returns None at the end of the mission or on + a bad jump''' + loops = MAX_JUMP_LOOPS + total = mission.count() + while 0 <= index < total: + point = mission.point(index) + command = point.command + if command == mavlink.MAV_CMD_DO_JUMP: + target = int(point.param1) + elif command == getattr(mavlink, 'MAV_CMD_DO_JUMP_TAG', -1): + target = jump_tag_index(mission, int(point.param1)) + else: + return (point, index) + if loops == 0 or target is None: + return (None, index) + loops -= 1 + # an invalid target aborts the search, as in ArduPilot + if target >= total or target == 0: + return (None, index) + num_times = int(point.param2) + run = jump_counts.get(index, 0) + if num_times == JUMP_REPEAT_FOREVER or run < num_times: + jump_counts[index] = run + 1 + index = target + else: + # having finished a jump loop ArduPilot zeroes the counter, so + # coming back to it later runs the loop again + jump_counts[index] = 0 + index += 1 + return (None, index) + + +def jump_tag_index(mission, tag): + '''index of the JUMP_TAG item carrying this tag, as + AP_Mission::get_index_of_jump_tag() finds it''' + tag_cmd = getattr(mavlink, 'MAV_CMD_JUMP_TAG', None) + if tag_cmd is None: + return None + for point in mission.points: + if point.command == tag_cmd and int(point.param1) == tag: + return point.seq + return None + + +def build_return_paths(mission): + '''the return path from each DO_RETURN_PATH_START, walked the way + AP_Mission::distance_to_mission_leg() walks it: following DO_JUMP, and + stopping at a landing or a DO_LAND_START inclusive''' + paths = [] + budget = SEARCH_BUDGET + exhausted = False + for start in mission.return_path_starts(): + points = [] + jump_counts = {} + index = start + finished = False + while budget > 0: + budget -= 1 + (point, index) = next_command(mission, index, jump_counts) + if point is None: + # ran off the end of the mission, which ArduPilot still + # accepts as a path + finished = True + break + index = point.seq + 1 + if point.has_location(): + points.append(point) + if point.is_landing() or point.command == mavlink.MAV_CMD_DO_LAND_START: + finished = True + break + if not finished: + # the search budget ran out part way through, which ArduPilot + # treats as no path rather than as a truncated one + exhausted = True + break + if len(points) > 0: + paths.append(ReturnPath(start, points)) + if exhausted and len(paths) == 0: + return [] + return paths + + +def closest_leg(paths, x, y, amsl): + '''AP_Mission::jump_to_closest_mission_leg(): the item the vehicle would + make current, or None. + + Within one path the first located point is measured as a point and + ties keep the earliest leg; across paths a tie keeps the last + DO_RETURN_PATH_START, matching the <= in ArduPilot. + ''' + best_point = None + best_distance = None + for path in paths: + (point, distance) = closest_in_path(path, x, y, amsl) + if point is None: + continue + if best_distance is None or distance <= best_distance: + best_distance = distance + best_point = point + return (best_point, best_distance) + + +def closest_in_path(path, x, y, amsl): + '''closest point of one return path, as distance_to_mission_leg() does''' + points = path.points + if len(points) == 0: + return (None, None) + first = points[0] + # the first point of a return path is measured as a point, not a leg + dx = x - first.x + dy = y - first.y + dz = amsl - first.amsl + best_distance = (dx * dx + dy * dy + dz * dz) ** 0.5 + best_point = first + prev = first + for point in points[1:]: + if point.x == prev.x and point.y == prev.y and point.amsl == prev.amsl: + # a zero length leg is skipped and does not advance prev_loc + continue + distance = mission_model.segment_distance_3d( + x, y, amsl, + prev.x, prev.y, prev.amsl, + point.x, point.y, point.amsl) + # strict, so a tie keeps the earlier leg + if distance < best_distance: + best_distance = distance + best_point = point + prev = point + return (best_point, best_distance) + + +def rejoin_target(mission, point): + '''where the vehicle actually flies to. + + set_current_cmd() runs advance_current_nav_cmd(), which walks forward + executing do-commands until it reaches a nav command, so if the closest + item is not a nav command the aircraft heads for the next nav command + after it. A DO_RETURN_PATH_START carrying a location can be the closest + item, which is why this matters. + ''' + if point is None: + return None + if point.is_nav() and point.has_location(): + return point + jump_counts = {} + index = point.seq + 1 + for _ in range(mission.count() + 1): + (candidate, index) = next_command(mission, index, jump_counts) + if candidate is None: + return None + index = candidate.seq + 1 + if candidate.is_nav() and candidate.has_location(): + return candidate + return None + + +class FailSpan(object): + '''a run of consecutive failing samples along the mission''' + + def __init__(self): + self.points = [] + self.legs = set() + self.worst = 0.0 + + def add(self, sample, deviation): + self.points.append((sample.x, sample.y)) + self.legs.add((sample.leg_from, sample.leg_to)) + self.worst = max(self.worst, deviation) + + +class CheckResult(object): + '''outcome of a return path check''' + + def __init__(self): + self.checked = 0 + self.failed = 0 + self.worst_deviation = 0.0 + self.worst_radius = None + self.worst_leg = None + self.worst_rejoin = None + self.spans = [] + self.errors = [] + self.warnings = [] + self.return_path_starts = [] + self.terrain_missing = 0 + # set when a fixed width was used instead of the turn radius + self.fixed_width = None + + def ok(self): + return len(self.errors) == 0 and self.failed == 0 + + +def check_return_path(mission, cruise_eas, roll_limit_deg, granularity=50.0, + terrain_fn=None, width=0.0): + '''check that an RTL from any point on the mission returns within the + allowed distance of the mission path. + + That distance is the turn radius the vehicle can achieve at the + altitude of each point, unless width is greater than zero, in which + case it is used instead and the airspeed and bank limit are not + needed. + ''' + result = CheckResult() + if width > 0: + result.fixed_width = width + result.terrain_missing = mission.terrain_missing + result.return_path_starts = mission.return_path_starts() + + path = mission.flown_path() + if len(path) < 2: + result.errors.append("mission has no flyable path") + return result + if len(result.return_path_starts) == 0: + result.errors.append("mission has no DO_RETURN_PATH_START") + return result + + paths = build_return_paths(mission) + if len(paths) == 0: + result.errors.append( + "no return path found after DO_RETURN_PATH_START") + return result + + granularity = max(float(granularity), mission_model.MIN_SPACING) + index = mission_model.PathIndex(path) + stats = mission_model.SampleStats() + samples = mission_model.sample_path(path, granularity, + projector=mission.projector, + terrain_fn=terrain_fn, stats=stats) + result.terrain_missing += stats.terrain_missing + + unresolved = 0 + span = None + for sample in samples: + if width > 0: + radius = width + else: + radius = mission_model.turn_radius(cruise_eas, roll_limit_deg, + sample.amsl) + (closest, _) = closest_leg(paths, sample.x, sample.y, sample.amsl) + target = rejoin_target(mission, closest) + if target is None: + # ArduPilot's set_current_cmd() would fail here, so an RTL from + # this point does not get a return path at all + unresolved += 1 + continue + deviation = cut_across_deviation(index, sample, target, granularity) + result.checked += 1 + if deviation > result.worst_deviation: + result.worst_deviation = deviation + result.worst_radius = radius + result.worst_leg = (sample.leg_from, sample.leg_to) + result.worst_rejoin = target.seq + if deviation > radius: + result.failed += 1 + if span is None: + span = FailSpan() + result.spans.append(span) + span.add(sample, deviation) + else: + span = None + + if unresolved > 0: + result.errors.append( + "%u of %u points along the mission have a return path that does " + "not reach a navigation command, so an RTL there would not follow " + "it" % (unresolved, unresolved + result.checked)) + if result.checked == 0 and len(result.errors) == 0: + result.errors.append("no point along the mission could be checked") + + return result + + +def cut_across_deviation(index, sample, target, granularity): + '''how far the direct line from a point to the rejoin waypoint gets from + the mission path, in metres''' + dx = target.x - sample.x + dy = target.y - sample.y + length = (dx * dx + dy * dy) ** 0.5 + if length <= 0: + return index.distance(sample.x, sample.y) + steps = max(1, int(math.ceil(length / granularity))) + worst = 0.0 + for step in range(steps + 1): + frac = float(step) / steps + d = index.distance(sample.x + dx * frac, sample.y + dy * frac) + if d > worst: + worst = d + return worst diff --git a/setup.py b/setup.py index 168351a904..0d6c82c6b4 100755 --- a/setup.py +++ b/setup.py @@ -77,6 +77,7 @@ def package_files(directory): packages=['MAVProxy', 'MAVProxy.modules', 'MAVProxy.modules.mavproxy_anufireproject', + 'MAVProxy.modules.mavproxy_bvlos_plan', 'MAVProxy.modules.mavproxy_fieldcheck', 'MAVProxy.modules.mavproxy_map', 'MAVProxy.modules.mavproxy_map3d', From 83db4579eb95958d163e0884228d7690fda72293 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 30 Jul 2026 16:18:07 +1000 Subject: [PATCH 2/3] bvlos_plan: add an Add Return Paths menu item Knowing which stretches of a mission have no safe return is most of the work, but fixing them by hand means drawing a reversed offset track waypoint by waypoint. This does it: for each failing stretch, a DO_RETURN_PATH_START and a run of waypoints back along the mission the way the aircraft came, offset to one side so the reversal is a turn it can fly rather than a course reversal on the spot, ending in a DO_JUMP onto the nearest point of the existing return path. The offset is the turn radius over that stretch, taking the largest so it is enough everywhere along it. Deliberately not the return_path_width, which is a check tolerance and may be much wider than anything the aircraft can fly. return_path_sep overrides it, which is worth using: offsetting by exactly the turn radius leaves the result only a metre or so inside the limit, where a slightly larger separation gives real margin. The new items go after the landing, so they change nothing about the mission as flown, and ArduPilot considers every DO_RETURN_PATH_START and follows DO_JUMP while walking a return path, so the appended block is found and leads home. Because this writes to the mission, it checks the result before committing to it: the candidate is built, the mission is checked with it, and it is kept only if fewer points fail and no new errors appear, so a fix cannot leave a mission worse than it found it. It refuses a mission whose terrain is unavailable, where the new legs would be placed from guessed altitudes, and one that does not end at a landing, where appended navigation items would be flown. The mission is changed in MAVProxy only, and says so, leaving "wp save" or "wp list" to the operator. --- .../modules/mavproxy_bvlos_plan/__init__.py | 110 ++++++++++- .../mavproxy_bvlos_plan/add_return_paths.py | 184 ++++++++++++++++++ 2 files changed, 292 insertions(+), 2 deletions(-) create mode 100644 MAVProxy/modules/mavproxy_bvlos_plan/add_return_paths.py diff --git a/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py b/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py index 9acf705b75..cbf6e702a9 100644 --- a/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py +++ b/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py @@ -18,6 +18,7 @@ from MAVProxy.modules.lib import mp_settings from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.mavproxy_bvlos_plan import add_return_paths from MAVProxy.modules.mavproxy_bvlos_plan import mission_model from MAVProxy.modules.mavproxy_bvlos_plan import return_path @@ -49,6 +50,8 @@ def __init__(self, mpstate): items=[ MPMenuItem('Return Path Check', 'Return Path Check', '# bvlos_plan returncheck'), + MPMenuItem('Add Return Paths', 'Add Return Paths', + '# bvlos_plan addreturnpaths'), MPMenuItem('Clear Highlight', 'Clear Highlight', '# bvlos_plan clear'), ]) @@ -60,15 +63,19 @@ def __init__(self, mpstate): # if set, the distance either side of the mission path that the # return may use, in metres, instead of the turn radius ('return_path_width', float, 0.0), + # if set, how far to one side an added return path is put, in + # metres, instead of the turn radius + ('return_path_sep', float, 0.0), ]) self.add_command('bvlos_plan', self.cmd_bvlos_plan, - "BVLOS planning", ['returncheck', 'clear', + "BVLOS planning", ['returncheck', 'addreturnpaths', + 'clear', 'set (BVLOSPLANSETTING)']) self.add_completion_function('(BVLOSPLANSETTING)', self.bvlos_settings.completion) def usage(self): - return "Usage: bvlos_plan " + return "Usage: bvlos_plan " def cmd_bvlos_plan(self, args): if len(args) == 0: @@ -76,6 +83,8 @@ def cmd_bvlos_plan(self, args): return if args[0] == "returncheck": self.cmd_returncheck() + elif args[0] == "addreturnpaths": + self.cmd_addreturnpaths() elif args[0] == "clear": self.clear_highlight() elif args[0] == "set": @@ -179,6 +188,103 @@ def cmd_returncheck(self): self.report(result, mission, cruise, roll) self.highlight(result, mission) + def cmd_addreturnpaths(self): + '''add return paths covering the parts of the mission that fail''' + items = self.mission_items() + if items is None: + return + # the new legs are offset by the turn radius, never by the return + # path width, which is only a check tolerance. return_path_sep + # overrides that offset + separation = self.bvlos_settings.return_path_sep + cruise = self.cruise_airspeed() + roll = self.roll_limit() + if separation <= 0 and (cruise is None or roll is None): + print("bvlos_plan: adding return paths needs the cruise airspeed " + "and bank limit, as the new legs are offset by the turn " + "radius. Connect to a vehicle, set them with 'bvlos_plan " + "set cruise_airspeed' and 'bvlos_plan set roll_limit', or " + "give the offset directly with 'bvlos_plan set " + "return_path_sep'") + return + run = self.run_check(items) + if run is None: + return + (mission, result, _, _) = run + if len(result.errors) > 0: + for err in result.errors: + print("bvlos_plan: %s" % err) + return + if result.failed == 0: + print("bvlos_plan: the mission already passes, nothing to add") + return + if mission.terrain_missing: + print("bvlos_plan: not changing the mission while %u items need " + "terrain that is not available, as the new legs would be " + "placed from guessed altitudes. Try 'terrain set source " + "SRTM1'" % mission.terrain_missing) + return + if not mission.ends_in_landing(): + # new navigation items after a mission that does not end at a + # landing would be flown as part of the mission + print("bvlos_plan: the mission does not end at a landing, so " + "adding items to the end would change the mission as " + "flown. Not changing it") + return + + added = add_return_paths.build(mission, items, result, cruise, roll, + separation=separation) + if len(added) == 0: + print("bvlos_plan: could not work out a return path to add") + return + + # check what we are about to do before doing it, so a fix that does + # not help cannot be left behind in the mission + new_items = [] + for new_path in added: + new_items.extend(new_path.items) + trial = self.run_check(items + new_items) + if trial is None: + return + (trial_mission, trial_result, _, _) = trial + if len(trial_result.errors) > 0 or trial_result.failed >= result.failed: + print("bvlos_plan: the return paths this would add do not improve " + "the mission (%u failing points before, %u after), so it has " + "not been changed" % (result.failed, trial_result.failed)) + for err in trial_result.errors: + print(" would give: %s" % err) + if trial_result.failed >= result.failed and separation > 0: + print(" a smaller 'bvlos_plan set return_path_sep' may help") + return + + loader = self.module('wp').wploader + count = 0 + for new_path in added: + for item in new_path.items: + loader.add(item) + count += 1 + # keep the loader self consistent for anything watching it + loader.expected_count = loader.count() + print("Added %u return path(s), %u mission items:" % (len(added), count)) + for new_path in added: + source = ("the return_path_sep setting" if new_path.from_setting + else "the turn radius") + print(" mission %u..%u now has a return path offset %.0fm to one " + "side, %s, rejoining the existing return path at waypoint %u" + % (new_path.from_seq, new_path.to_seq, new_path.separation, + source, new_path.rejoin_seq)) + print(" the mission is changed here only, use 'wp save' to keep it " + "or 'wp list' to go back to the vehicle's copy") + + # show what the mission looks like now + items = [loader.wp(i) for i in range(loader.count())] + run = self.run_check(items) + if run is None: + return + (mission, result, cruise, roll) = run + self.report(result, mission, cruise, roll) + self.highlight(result, mission) + def report(self, result, mission, cruise, roll): '''print the outcome of a check''' if result.fixed_width is not None: diff --git a/MAVProxy/modules/mavproxy_bvlos_plan/add_return_paths.py b/MAVProxy/modules/mavproxy_bvlos_plan/add_return_paths.py new file mode 100644 index 0000000000..5544e8c479 --- /dev/null +++ b/MAVProxy/modules/mavproxy_bvlos_plan/add_return_paths.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 +''' +Build alternative return paths to fix the parts of a mission that fail the +return path check. + +Where a stretch of the mission has no return path leg near it, an RTL from +there cuts across ground the mission never covers. The fix is to give that +stretch a return path of its own: a DO_RETURN_PATH_START and a run of +waypoints back along the mission the way the aircraft came, offset to one +side by the turn radius so the reversal is a turn the aircraft can fly rather +than a course reversal on the spot. A DO_JUMP at the end brings it back onto +the existing return path. + +The new items go at the end of the mission, after the landing, so they change +nothing about the mission as flown. ArduPilot considers every +DO_RETURN_PATH_START in the mission and follows DO_JUMP while it walks a +return path, so the appended block is found and leads home. +''' + +# AP_FLAKE8_CLEAN + +import math + +from pymavlink import mavutil + +from MAVProxy.modules.mavproxy_bvlos_plan import mission_model +from MAVProxy.modules.mavproxy_bvlos_plan import return_path + +mavlink = mavutil.mavlink + +# DO_JUMP repeat count meaning "always", AP_MISSION_JUMP_REPEAT_FOREVER +JUMP_FOREVER = -1 + + +class NewPath(object): + '''one alternative return path that was added''' + + def __init__(self, from_seq, to_seq, separation, from_setting, + rejoin_seq, items): + # the stretch of mission it covers, in mission order + self.from_seq = from_seq + self.to_seq = to_seq + # how far to one side the new legs were put, in metres + self.separation = separation + # True if that came from the setting rather than the turn radius + self.from_setting = from_setting + self.rejoin_seq = rejoin_seq + self.items = items + + +def span_vertices(path, span): + '''the mission points a failing span covers, in mission order''' + legs = sorted(span.legs) + first = legs[0][0] + last = legs[-1][1] + return [p for p in path if first <= p.seq <= last] + + +def offset_side(points, distance): + '''offset each point to the right of the direction of travel. + + points are in the order they will be flown, so this puts the new path + to one side of the mission leg it shadows and the aircraft turns onto + it rather than reversing on the spot. + ''' + out = [] + for i in range(len(points)): + if i + 1 < len(points): + (ax, ay) = (points[i].x, points[i].y) + (bx, by) = (points[i + 1].x, points[i + 1].y) + else: + (ax, ay) = (points[i - 1].x, points[i - 1].y) + (bx, by) = (points[i].x, points[i].y) + dx = bx - ax + dy = by - ay + length = math.hypot(dx, dy) + if length <= 0: + out.append((points[i].x, points[i].y)) + continue + # right of travel in an east/north frame + out.append((points[i].x + distance * dy / length, + points[i].y - distance * dx / length)) + return out + + +def nearest_return_item(paths, point): + '''the item of an existing return path closest to a mission point, which + is where the new path rejoins''' + best = None + best_distance = None + for path in paths: + for candidate in path.points: + d = math.hypot(candidate.x - point.x, candidate.y - point.y) + if best_distance is None or d < best_distance: + best_distance = d + best = candidate + return best + + +def make_item(target_system, target_component, seq, command, frame, + lat, lon, alt, param1=0.0, param2=0.0): + '''a new mission item addressed to the same vehicle as the mission''' + return mavlink.MAVLink_mission_item_message( + target_system, target_component, seq, frame, command, 0, 1, + param1, param2, 0.0, 0.0, lat, lon, alt) + + +def build_for_span(mission, span, existing_paths, cruise_eas, roll_limit_deg, + target_system, target_component, next_seq, separation=0.0): + '''the items for one alternative return path, or None if there is nothing + useful to add''' + path = mission.flown_path() + vertices = span_vertices(path, span) + if len(vertices) < 2: + return None + + # fly it back the way we came + vertices = list(reversed(vertices)) + + # how far to one side to put the new legs. By default the turn radius + # over this stretch, taking the largest so it is enough everywhere along + # it, as that is what the aircraft can actually fly. Never the return + # path width, which is only a check tolerance + from_setting = separation > 0 + if from_setting: + offset = separation + else: + offset = max(mission_model.turn_radius(cruise_eas, roll_limit_deg, + v.amsl) + for v in vertices) + + rejoin = nearest_return_item(existing_paths, vertices[-1]) + if rejoin is None: + return None + + offsets = offset_side(vertices, offset) + items = [] + seq = next_seq + for (i, vertex) in enumerate(vertices): + (lat, lon) = mission.projector.unproject(offsets[i][0], offsets[i][1]) + command = (mavlink.MAV_CMD_DO_RETURN_PATH_START if i == 0 + else mavlink.MAV_CMD_NAV_WAYPOINT) + items.append(make_item(target_system, target_component, seq, command, + vertex.frame, lat, lon, vertex.alt)) + seq += 1 + # back onto the existing return path + items.append(make_item(target_system, target_component, seq, + mavlink.MAV_CMD_DO_JUMP, 0, 0.0, 0.0, 0.0, + param1=float(rejoin.seq), + param2=float(JUMP_FOREVER))) + + return NewPath(vertices[-1].seq, vertices[0].seq, offset, + from_setting, rejoin.seq, items) + + +def build(mission, items, result, cruise_eas, roll_limit_deg, + separation=0.0): + '''alternative return paths covering every failing span of a check. + + Returns a list of NewPath, whose items are to be appended to the + mission in order. items is the mission as mavlink items, used to + address the new ones to the same vehicle. separation, if greater than + zero, is used instead of the turn radius to offset the new legs. + ''' + if len(result.spans) == 0 or len(items) == 0: + return [] + target_system = getattr(items[0], 'target_system', 0) + target_component = getattr(items[0], 'target_component', 0) + + existing_paths = return_path.build_return_paths(mission) + if len(existing_paths) == 0: + return [] + + added = [] + next_seq = mission.count() + for span in result.spans: + new_path = build_for_span(mission, span, existing_paths, cruise_eas, + roll_limit_deg, target_system, + target_component, next_seq, separation) + if new_path is None: + continue + added.append(new_path) + next_seq += len(new_path.items) + return added From ff46c51d8bf8ac5080eb548a4ffb132ba8b8386b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 30 Jul 2026 16:18:18 +1000 Subject: [PATCH 3/3] misseditor: notice a mission changed inside MAVProxy The editor only refreshed on wp module activity, so a mission changed by another module was shown stale until the next download. Watch the loader's change stamp as well. That stamp moves for every item received during a download, and idle_task runs often enough that this would rebuild the whole table once per waypoint, so ignore the loader while it holds fewer items than are expected and refresh only on a finished mission. --- .../mavproxy_misseditor/mission_editor.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/MAVProxy/modules/mavproxy_misseditor/mission_editor.py b/MAVProxy/modules/mavproxy_misseditor/mission_editor.py index 4bdb8e748c..5e598e0f3e 100644 --- a/MAVProxy/modules/mavproxy_misseditor/mission_editor.py +++ b/MAVProxy/modules/mavproxy_misseditor/mission_editor.py @@ -245,7 +245,21 @@ def idle_task(self): if not self.child.is_alive(): self.close() return - last_wp_change = self.mpstate.module('wp').loading_waypoint_lasttime + wp_module = self.mpstate.module('wp') + if wp_module is None: + return + # loading_waypoint_lasttime only moves when items go to or from the + # vehicle, so also watch the loader itself, which is what the map + # watches. Otherwise a mission changed in MAVProxy alone never + # reaches the editor. Ignore the loader while a download is part way + # through, as it changes on every item received and rebuilding the + # table for each one is a lot of work for nothing + loader = wp_module.wploader + loader_change = loader.last_change + expected = getattr(loader, 'expected_count', 0) + if expected and loader.count() < expected: + loader_change = 0 + last_wp_change = max(wp_module.loading_waypoint_lasttime, loader_change) if last_wp_change > self.last_wp_change: self.last_wp_change = last_wp_change self.get_wps_from_module()