diff --git a/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py b/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py new file mode 100644 index 0000000000..cbf6e702a9 --- /dev/null +++ b/MAVProxy/modules/mavproxy_bvlos_plan/__init__.py @@ -0,0 +1,403 @@ +#!/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 add_return_paths +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('Add Return Paths', 'Add Return Paths', + '# bvlos_plan addreturnpaths'), + 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), + # 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', 'addreturnpaths', + '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] == "addreturnpaths": + self.cmd_addreturnpaths() + 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 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: + 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/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 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/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() 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',