diff --git a/MAVProxy/modules/lib/log_mission.py b/MAVProxy/modules/lib/log_mission.py new file mode 100644 index 0000000000..8fce9ab617 --- /dev/null +++ b/MAVProxy/modules/lib/log_mission.py @@ -0,0 +1,156 @@ +'''the mission a log holds, as it was downloaded, uploaded or written out + +AP_FLAKE8_CLEAN +''' + +from pymavlink import mavutil + + +# the messages a log carries its mission in +MISSION_TYPES = ['MISSION_COUNT', 'MISSION_CLEAR_ALL', + 'MISSION_ITEM', 'MISSION_ITEM_INT', 'CMD', 'MSG'] +# a MAVLink1 message has no mission_type, and is only ever about the mission +DEFAULT_MISSION_TYPE = mavutil.mavlink.MAV_MISSION_TYPE_MISSION + + +class LogMission(object): + '''the last whole mission a log holds. A telemetry log carries the + missions downloaded from or uploaded to the vehicle, each a MISSION_COUNT + and then its items, in whatever order they were asked for, and perhaps + never all of them. A dataflash log carries a CMD for each item of the + mission whenever the vehicle writes the whole mission out: at the start + of the log, and after it changes. + + mission_type is which of the tables the mission protocol carries to + keep: the mission itself, or the rally points or fence, which travel in + the same messages. Only the mission is written out as CMD, so a table + of another kind is whatever a telemetry log carries of it''' + + def __init__(self, mission_type=mavutil.mavlink.MAV_MISSION_TYPE_MISSION): + self.mission_type = mission_type + # seq -> MISSION_ITEM, for the last mission to arrive whole + self.items = {} + # (count, {seq: MISSION_ITEM}) for one arriving, while it does. The + # count is None where the log does not say how many items to expect + self.transfer = None + # how many times a table has arrived whole, or been cleared away: + # the vehicle has said what it holds, even where that is the table + # held already + self.arrivals = 0 + + def start(self, count): + '''a mission of count items starts to arrive''' + if self.transfer is not None and self.transfer[0] is None: + # with no count to finish it, the last one ended where this starts + self.items = self.transfer[1] + self.arrivals += 1 + self.transfer = (count, {}) + if count == 0: + self.clear() + + def clear(self): + self.items = {} + self.transfer = None + self.arrivals += 1 + + def add(self, item): + if self.transfer is None: + # an item which is not part of a mission seen to start arriving: + # a log which starts part way through, or a single item changed + self.items[item.seq] = item + self.arrivals += 1 + return + (count, items) = self.transfer + if count is not None and item.seq >= count: + return + items[item.seq] = item + if count is not None and len(items) == count: + self.items = items + self.transfer = None + self.arrivals += 1 + + def read(self, m, type=None): + '''take in one of the MISSION_TYPES messages. type is what to take + it as, for a caller which has made one message stand in for another + -- a telemetry log's STATUSTEXT for a dataflash MSG, say''' + if type is None: + type = m.get_type() + if self.mission_type != mavutil.mavlink.MAV_MISSION_TYPE_MISSION \ + and type in ('MSG', 'CMD'): + # a dataflash log writes out the mission this way, and nothing + # else the mission protocol carries + return + if type == 'MSG': + if getattr(m, 'Message', None) == 'New mission': + # the logger says so before it writes the mission out, and + # a mission cleared away is written out as nothing more + self.start(0) + return + if type == 'CMD': + if m.CNum == 0: + # the vehicle writes out the whole mission from home onwards + self.start(getattr(m, 'CTot', None)) + # a log from before 2014 carries neither the frame nor the + # parameters of an item + params = [getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)] + self.add(mavutil.mavlink.MAVLink_mission_item_message( + 0, 0, m.CNum, + getattr(m, 'Frame', + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT), + m.CId, 0, 1, params[0], params[1], params[2], params[3], + m.Lat, m.Lng, m.Alt)) + return + # fence and rally points travel in the same messages + mission_type = getattr(m, 'mission_type', DEFAULT_MISSION_TYPE) + if type == 'MISSION_CLEAR_ALL': + if mission_type in (self.mission_type, + mavutil.mavlink.MAV_MISSION_TYPE_ALL): + self.clear() + return + if mission_type != self.mission_type: + return + if type == 'MISSION_COUNT': + self.start(m.count) + elif type == 'MISSION_ITEM_INT': + self.add(mavutil.mavlink.MAVLink_mission_item_message( + 0, 0, m.seq, m.frame, m.command, m.current, m.autocontinue, + m.param1, m.param2, m.param3, m.param4, + m.x / 1.0e7, m.y / 1.0e7, m.z)) + else: + self.add(m) + + def whole(self): + '''the mission by sequence, from its first item as far as it runs + without a gap. Nothing is made up to stand in for an item never + seen, and a mission which never arrived whole is not one: a log + which starts part way through one carries no mission at all, since + there is no flying, numbering or jumping through a mission whose + first items are missing''' + items = self.items + if self.transfer is not None: + (count, arriving) = self.transfer + if count is None or not items: + # the last one ran to the end of the log, or it never arrived + # whole and nothing before it did + items = arriving + out = {} + seq = 0 + while seq in items: + out[seq] = items[seq] + seq += 1 + return out + + def partial(self): + '''whether what whole() gives is less than what arrived whole: the + log ends part way through the only transfer it carries. What did + arrive is where the vehicle was told to go, and is worth drawing, + but not worth flying: what would have followed it is not known''' + if self.transfer is None: + return False + (count, arriving) = self.transfer + return count is not None and not self.items and len(arriving) < count + + def fill(self, wp): + '''add the mission to the MAVWPLoader wp''' + for (_, item) in sorted(self.whole().items()): + wp.add(item) diff --git a/MAVProxy/modules/lib/mission_item_protocol.py b/MAVProxy/modules/lib/mission_item_protocol.py index 9aa2f22dde..4bd3c97f8f 100644 --- a/MAVProxy/modules/lib/mission_item_protocol.py +++ b/MAVProxy/modules/lib/mission_item_protocol.py @@ -884,6 +884,7 @@ def cmd_show(self, args): print("usage: wp show ") return self.wploader.load(args[0]) + self.wploader.expected_count = self.wploader.count() def cmd_update(self, args): if not self.check_have_list(): @@ -1099,6 +1100,9 @@ def ftp_callback(self, fh, callback=None): w = mavmsg(*t) w = self.wp_from_mission_item_int(w) self.wploader.add(w) + # the whole of it arrived, however many a MAVLink download before it + # was expecting + self.wploader.expected_count = self.wploader.count() self.show_and_save(self.target_system) if callback is not None: callback(self.wploader) @@ -1144,6 +1148,7 @@ def wp_ftp_upload(self, args, callback=None): callback(None) return print("Loaded %u %s from %s" % (self.wploader.count(), self.itemstype(), filename)) + self.wploader.expected_count = self.wploader.count() self.ftp_upload(self.wploader, callback=callback) def ftp_upload(self, loader, callback=None): diff --git a/MAVProxy/modules/lib/mp_util.py b/MAVProxy/modules/lib/mp_util.py index 4240d288e8..706f5cbbe8 100644 --- a/MAVProxy/modules/lib/mp_util.py +++ b/MAVProxy/modules/lib/mp_util.py @@ -13,6 +13,11 @@ import re from math import cos, sin, tan, atan2, sqrt, radians, degrees, pi, log, fmod +# MAVLink commands newer than some pymavlink releases know about -- 2.4.49 +# has neither -- so they are named here rather than looked up in the dialect +MAV_CMD_DO_ORBIT = 34 +MAV_CMD_NAV_ARC_WAYPOINT = 36 + # Some platforms (CYGWIN and others) many not have the wx library # use imp to see if wx is on the path has_wxpython = False @@ -188,6 +193,38 @@ def arc_points(latlon1, latlon2, arc_angle, steps=None): return points +# takeoff items carry an altitude and often no position of their own: the +# vehicle climbs from where it already is, which for a mission is home. +# Numeric because mp_util does not import pymavlink at module scope +TAKEOFF_COMMANDS = (22, 84) # NAV_TAKEOFF, NAV_VTOL_TAKEOFF + + +# what each kind of mission item is called where a map labels one, as the 2D +# map has always labelled its waypoints. Numeric for the same reason as +# TAKEOFF_COMMANDS above +MISSION_LABEL_SUFFIXES = { + 22: "TOff", # NAV_TAKEOFF + 189: "DLS", # DO_LAND_START + 82: "SW", # NAV_SPLINE_WAYPOINT + MAV_CMD_NAV_ARC_WAYPOINT: "AW", + 17: "LU", # NAV_LOITER_UNLIM + 18: "LT", # NAV_LOITER_TURNS + 19: "LTime", # NAV_LOITER_TIME + 31: "LAlt", # NAV_LOITER_TO_ALT + MAV_CMD_DO_ORBIT: "Orbit", + 85: "VL", # NAV_VTOL_LAND +} + + +def mission_item_label(seq, command): + """the label a map puts on a mission item: its number, and what it is + where that is worth saying -- "3(DLS)" for a land start""" + suffix = MISSION_LABEL_SUFFIXES.get(command) + if suffix is None: + return str(seq) + return "%s(%s)" % (seq, suffix) + + # vehicles which hold position at a loiter point rather than flying a circle # around it. MAVProxy's own vehicle type names are accepted as well as the # MAV_TYPEs, since that is what a live module has to hand @@ -272,7 +309,7 @@ def mission_circle_radius(command, params, default_radius=None, vehicle=None): mavlink.MAV_CMD_NAV_LOITER_TURNS: 2, mavlink.MAV_CMD_NAV_LOITER_TIME: 2, mavlink.MAV_CMD_NAV_LOITER_TO_ALT: 1, - getattr(mavlink, 'MAV_CMD_DO_ORBIT', 34): 0, + MAV_CMD_DO_ORBIT: 0, }.get(command) if index is None: return None @@ -285,10 +322,32 @@ def mission_circle_radius(command, params, default_radius=None, vehicle=None): # circles, so those are left alone return None radius = params[index] - if radius is None or math.isnan(radius) or radius == 0: - radius = default_radius - if radius is None or math.isnan(radius) or radius == 0: - return None + if command == MAV_CMD_DO_ORBIT: + if radius is None or math.isnan(radius) or radius == 0: + radius = default_radius + if radius is None or math.isnan(radius) or radius == 0: + return None + return radius + if radius is None or math.isnan(radius): + radius = 0.0 + if (command == mavlink.MAV_CMD_NAV_LOITER_TIME or abs(radius) <= 1): + # the vehicle's own radius. ArduPilot has nowhere to keep a radius + # for LOITER_TIME -- the loiter time takes all of the storage -- so + # it keeps only a bit saying the item asked for counter-clockwise, + # and hands that back as -1 (or +1 when it did not); whatever radius + # the item was uploaded with, ArduPlane's update_loiter(0) flies + # WP_LOITER_RAD. It takes any radius of a metre or less the same + # way, and flies counter-clockwise if the item asked for it, and + # otherwise the way WP_LOITER_RAD's sign says + if default_radius is None or math.isnan(default_radius): + return None + size = abs(default_radius) + if size <= 1: + # ArduPlane's LOITER_RADIUS_DEFAULT + size = 60.0 + if radius < 0 or default_radius < 0: + return -size + return size return radius @@ -299,7 +358,7 @@ def mission_circle_turns(command, params): mavlink = mavutil.mavlink if command == mavlink.MAV_CMD_NAV_LOITER_TURNS: turns = params[0] - elif command == getattr(mavlink, 'MAV_CMD_DO_ORBIT', 34): + elif command == MAV_CMD_DO_ORBIT: # DO_ORBIT counts in radians rather than turns turns = params[3] / (2 * pi) else: @@ -309,6 +368,63 @@ def mission_circle_turns(command, params): return turns +def mission_crosstracks_from_centre(command, params): + """whether the leg out of a circling mission item is flown against a + track starting at the item's own location rather than at the point the + vehicle leaves the circle. + + ArduPlane crosstracks from the centre unless param4 asks for the exit + location instead, and only the loiter items which are flown until + something lets the vehicle go carry that choice there. LOITER_UNLIM + never leaves, and DO_ORBIT counts its turns in param4, so neither is + asked + """ + from pymavlink import mavutil + mavlink = mavutil.mavlink + if command not in (mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavlink.MAV_CMD_NAV_LOITER_TIME, + mavlink.MAV_CMD_NAV_LOITER_TO_ALT): + return False + xtrack = params[3] + if xtrack is None or math.isnan(xtrack): + return True + return not xtrack > 0 + + +def log_params(mlog): + """the parameters a log carries. A telemetry log's params are only the + PARAM_VALUEs the autopilot sent. Where MAVProxy fetched them over FTP + it logged them itself, as PARAM_VALUEs from MAV_COMP_ID_MISSIONPLANNER so + they can be told apart, and those are left out: a log from a vehicle + whose parameters were fetched that way offers only the few the autopilot + sent as they changed. Take both, the autopilot's own over the fetched + ones""" + params = getattr(mlog, 'params', None) + param_state = getattr(mlog, 'param_state', None) + if not param_state: + return params + from pymavlink import mavutil + sysid = getattr(mlog, 'sysid', 0) + if not sysid: + # the log picks the vehicle whose params it offers from its first + # heartbeat, and a log with none in it never picks one. Take the + # vehicle the parameters came from, so long as only one did + systems = set(k[0] for (k, state) in param_state.items() + if k[0] != 0 and state.params) + if len(systems) != 1: + return params + sysid = systems.pop() + own = param_state.get((sysid, 1)) + params = own.params if own is not None else {} + fetched = param_state.get((sysid, + mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER)) + if fetched is None or not fetched.params: + return params + ret = dict(fetched.params) + ret.update(params or {}) + return ret + + def param_value(params, name): """look one parameter up in params, which may be a mapping (a live vehicle's mav_param, or a log's params) or a callable taking a name. diff --git a/MAVProxy/modules/lib/plane_track.py b/MAVProxy/modules/lib/plane_track.py new file mode 100644 index 0000000000..662f929c3b --- /dev/null +++ b/MAVProxy/modules/lib/plane_track.py @@ -0,0 +1,1418 @@ +''' +the path an ArduPlane flies along a mission + +ArduPlane does not fly the lines drawn between its mission items. Its L1 +controller tracks each leg from wherever set_next_WP() says the leg starts, +which is the item before, the centre of a loiter just left, the point the +loiter was left at, or wherever the vehicle happened to be; it flies at a +loiter's centre until it is close enough to capture the circle, and it +leaves only once it is lined up on whatever comes next. How the aircraft +then turns onto each of those tracks is the controller's own dynamics, so +rather than approximate them with geometry this ports the controller and +the mission logic around it, and flies a simple aircraft through them: a +fixed airspeed, turning with a coordinated bank that lags the controller's +demand, and climbing and descending within the rates TECS allows. + +The ports follow AP_L1_Control and ArduPlane's commands_logic.cpp, +navigation.cpp, altitude.cpp and mode_loiter.cpp. + +AP_FLAKE8_CLEAN +''' + +import math + +from pymavlink import mavutil + +from MAVProxy.modules.lib import mp_util + +GRAVITY = 9.80665 +# ArduPilot's Location maths: metres per 1e-7 degree of latitude +LOCATION_SCALING_FACTOR = 0.011131884502145034 + +# ArduPlane's own defaults, for parameters a vehicle or log does not carry. +# Older firmware names some of them differently, in other units +PARAMETERS = { + 'NAVL1_PERIOD': ([('NAVL1_PERIOD', 1.0)], 17.0), + 'NAVL1_DAMPING': ([('NAVL1_DAMPING', 1.0)], 0.75), + 'NAVL1_XTRACK_I': ([('NAVL1_XTRACK_I', 1.0)], 0.02), + 'NAVL1_LIM_BANK': ([('NAVL1_LIM_BANK', 1.0)], 0.0), + 'ROLL_LIMIT': ([('ROLL_LIMIT_DEG', 1.0), ('LIM_ROLL_CD', 0.01)], 45.0), + 'RLL2SRV_TCONST': ([('RLL2SRV_TCONST', 1.0)], 0.5), + 'WP_RADIUS': ([('WP_RADIUS', 1.0)], 90.0), + 'WP_MAX_RADIUS': ([('WP_MAX_RADIUS', 1.0)], 0.0), + 'WP_LOITER_RAD': ([('WP_LOITER_RAD', 1.0)], 60.0), + 'AIRSPEED_CRUISE': ([('AIRSPEED_CRUISE', 1.0), ('TRIM_ARSPD_CM', 0.01)], + 12.0), + 'AIRSPEED_MIN': ([('AIRSPEED_MIN', 1.0), ('ARSPD_FBW_MIN', 1.0)], 9.0), + 'AIRSPEED_MAX': ([('AIRSPEED_MAX', 1.0), ('ARSPD_FBW_MAX', 1.0)], 22.0), + 'TECS_CLMB_MAX': ([('TECS_CLMB_MAX', 1.0)], 5.0), + 'TECS_SINK_MAX': ([('TECS_SINK_MAX', 1.0)], 5.0), + 'TECS_TIME_CONST': ([('TECS_TIME_CONST', 1.0)], 5.0), + 'ALT_SLOPE_MIN': ([('ALT_SLOPE_MIN', 1.0)], 15.0), + 'CLIMB_SLOPE_HGT': ([('CLIMB_SLOPE_HGT', 1.0)], 25.0), + 'RTL_ALTITUDE': ([('RTL_ALTITUDE', 1.0), ('ALT_HOLD_RTL', 0.01)], 100.0), + 'RTL_RADIUS': ([('RTL_RADIUS', 1.0)], 0.0), + 'RALLY_LIMIT_KM': ([('RALLY_LIMIT_KM', 1.0)], 5.0), + 'RALLY_INCL_HOME': ([('RALLY_INCL_HOME', 1.0)], 0.0), + 'Q_ENABLE': ([('Q_ENABLE', 1.0)], 0.0), + 'Q_OPTIONS': ([('Q_OPTIONS', 1.0)], 0.0), + 'Q_RTL_MODE': ([('Q_RTL_MODE', 1.0)], 0.0), + 'Q_RTL_ALT': ([('Q_RTL_ALT', 1.0)], 15.0), + 'Q_FW_LND_APR_RAD': ([('Q_FW_LND_APR_RAD', 1.0)], 0.0), + 'Q_TRANS_DECEL': ([('Q_TRANS_DECEL', 1.0)], 2.0), + 'FLIGHT_OPTIONS': ([('FLIGHT_OPTIONS', 1.0)], 0.0), + 'ARSPD_TYPE': ([('ARSPD_TYPE', 1.0)], 0.0), + 'ARSPD_USE': ([('ARSPD_USE', 1.0)], 0.0), +} +# FLIGHT_OPTIONS bit for climbing to a higher waypoint at once in AUTO, +# rather than on a slope along the leg +FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO = 1 << 14 +# the pitch in degrees a fixed-wing takeoff climbs at where its item gives +# none (Plane::do_takeoff) +DEFAULT_TAKEOFF_PITCH = 4.0 +# QuadPlane's Q_OPTIONS bit for flying NAV_TAKEOFF as a fixed-wing takeoff +Q_OPTION_ALLOW_FW_TAKEOFF = 1 << 1 +# QuadPlane's Q_OPTIONS bit for landing a mission's NAV_VTOL_LAND after a +# fixed-wing approach, which the item's param1 can ask for too +Q_OPTION_MISSION_LAND_FW_APPROACH = 1 << 4 +NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH = 1 +# the course of a VTOL landing approach where no wind is known: ArduPlane +# takes it as into the wind, atan2(-wind.y, -wind.x), which a wind of zero +# makes due south +STILL_AIR_APPROACH = -180.0 +# QuadPlane::RTL_MODE, what Q_RTL_MODE makes of a return to launch +Q_RTL_DISABLED = 0 +Q_RTL_SWITCH_QRTL = 1 +Q_RTL_VTOL_APPROACH = 2 +Q_RTL_QRTL_ALWAYS = 3 + +# how often ArduPlane navigates, and how finely the aircraft is flown +NAV_PERIOD = 0.1 +# the drawn path gets a point at least this often, and whenever the aircraft +# has turned this far since the last one +POINT_SPACING = 20.0 +POINT_TURN = math.radians(4.0) +# a mission is not flown for longer than this, and one item not for longer +# than it takes to fly its leg this many times over, plus the time its +# loiter asks for -- the turns or the time, or the climb or descent -- and +# MAX_LAPS laps more: past that the aircraft is taken to be stuck, circling +# a waypoint it cannot turn tightly enough to reach. Flying a mission takes +# a second or so of work for every hour of it, so one which plainly takes +# longer than this is not attempted +MAX_FLIGHT_TIME = 4 * 3600.0 +LEG_ALLOWANCE = 4.0 +MAX_LAPS = 20 +LOITER_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, +) +WAYPOINT_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + mavutil.mavlink.MAV_CMD_NAV_LAND, + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, + mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, +) +TAKEOFF_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, +) +# items a flight ends at, rather than running out of mission and returning +# to launch (MissionFlight.run) +ENDS_THE_FLIGHT = ( + mavutil.mavlink.MAV_CMD_NAV_LAND, + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, +) +# navigation commands numbered past MAV_CMD_NAV_LAST +LATE_NAV_COMMANDS = (42702, 42703) # NAV_SCRIPT_TIME, NAV_ATTITUDE_TIME +# AP_MISSION_JUMP_REPEAT_FOREVER: the only repeat count which repeats for +# ever, and the fewest AP_Mission follows at all +JUMP_FOREVER = -1 +MAV_CMD_JUMP_TAG = 600 +MAV_CMD_DO_JUMP_TAG = 601 +# ArduPlane rejects these, and moves straight on to the item after +SKIPPED_COMMANDS = ( + mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT, + mp_util.MAV_CMD_NAV_ARC_WAYPOINT, +) + + +def parameter(params, name): + '''a parameter the flight depends on, from params or ArduPlane's + default''' + (names, default) = PARAMETERS[name] + for (param, scale) in names: + value = mp_util.param_value(params, param) + if value is not None and not math.isnan(value): + return float(value) * scale + return default + + +def eas2tas(amsl): + '''the true airspeed of each metre per second of equivalent airspeed at + an altitude, in the standard atmosphere''' + temperature = max(288.15 - 0.0065 * amsl, 150.0) + return 1.0 / math.sqrt((temperature / 288.15) ** 4.2559) + + +def wrap_pi(angle): + return (angle + math.pi) % (2 * math.pi) - math.pi + + +def wrap_longitude(dlon): + '''a difference in longitude, the short way round: Location's + diff_longitude()''' + return (dlon + 180.0) % 360.0 - 180.0 + + +def cross(a, b): + return a[0] * b[1] - a[1] * b[0] + + +def dot(a, b): + return a[0] * b[0] + a[1] * b[1] + + +def minus(a, b): + return (a[0] - b[0], a[1] - b[1]) + + +def norm(a): + return math.hypot(a[0], a[1]) + + +def unit(a): + length = norm(a) + return (a[0] / length, a[1] / length) + + +def path_proportion(point, start, end): + '''Location::line_path_proportion: how far along start->end point is, + more than 1 once past end''' + leg = minus(end, start) + squared = dot(leg, leg) + if squared < 0.001: + return 1.0 + return dot(leg, minus(point, start)) / squared + + +class L1Control(object): + '''AP_L1_Control, working in metres north and east''' + + def __init__(self, period, damping, xtrack_i_gain, bank_limit): + self.period = period + self.damping = damping + self.xtrack_i_gain = xtrack_i_gain + self.bank_limit = bank_limit + self.xtrack_i = 0.0 + self.last_nu = 0.0 + self.lateral_acceleration = 0.0 + self.circling = False + self.distance = 0.0 + self.target_bearing = 0.0 + + def loiter_radius(self, radius, amsl, cruise): + '''the radius actually flown for a loiter radius at an altitude''' + scale = eas2tas(amsl) ** 2 + bank = min(max(self.bank_limit, 0.0), 89.0) + if bank <= 0 or cruise <= 0: + return radius * scale + sea_level = cruise ** 2 / (math.tan(math.radians(bank)) * GRAVITY) + if sea_level > radius: + return radius * scale + return max(sea_level * scale, radius) + + def turn_distance(self, wp_radius, amsl, turn_angle): + distance = min(wp_radius * eas2tas(amsl) ** 2, self.distance) + turn_angle = abs(turn_angle) + if turn_angle >= 90: + return distance + return distance * turn_angle / 90.0 + + def _prevent_indecision(self, nu, yaw): + limit = 0.9 * math.pi + if (abs(nu) > limit and abs(self.last_nu) > limit and + abs(wrap_pi(self.target_bearing - yaw)) > math.radians(120) and + nu * self.last_nu < 0): + return self.last_nu + return nu + + def update_waypoint(self, position, velocity, yaw, start, end, dt): + k_l1 = 4.0 * self.damping ** 2 + speed = max(norm(velocity), 0.1) + self.target_bearing = math.atan2(end[1] - position[1], + end[0] - position[0]) + self.distance = 0.3183099 * self.damping * self.period * speed + track = minus(end, start) + track_length = norm(track) + if track_length < 1.0e-6: + track = minus(end, position) + if norm(track) < 1.0e-6: + track = (math.cos(yaw), math.sin(yaw)) + track = unit(track) + from_start = minus(position, start) + crosstrack = cross(from_start, track) + start_distance = norm(from_start) + along = dot(from_start, track) + if (start_distance > self.distance and + along / max(start_distance, 1.0) < -0.7071): + # behind the start of the leg: fly to its start + towards = unit(from_start) + towards = (-towards[0], -towards[1]) + nu = math.atan2(cross(velocity, towards), dot(velocity, towards)) + elif along > track_length + speed * 3: + # three seconds past the end of it: fly back to the end + towards = unit(minus(end, position)) + nu = math.atan2(cross(velocity, towards), dot(velocity, towards)) + else: + nu2 = math.atan2(cross(velocity, track), dot(velocity, track)) + sine_nu1 = crosstrack / max(self.distance, 0.1) + nu1 = math.asin(min(max(sine_nu1, -0.7071), 0.7071)) + if self.xtrack_i_gain <= 0: + self.xtrack_i = 0.0 + elif abs(nu1) < math.radians(5): + self.xtrack_i += nu1 * self.xtrack_i_gain * dt + self.xtrack_i = min(max(self.xtrack_i, -0.1), 0.1) + nu = nu1 + self.xtrack_i + nu2 + nu = self._prevent_indecision(nu, yaw) + self.last_nu = nu + nu = min(max(nu, -1.5708), 1.5708) + self.lateral_acceleration = k_l1 * speed * speed / self.distance * math.sin(nu) + self.circling = False + + def update_loiter(self, position, velocity, yaw, centre, radius, direction): + omega = 2 * math.pi / self.period + kx = omega * omega + kv = 2.0 * self.damping * omega + k_l1 = 4.0 * self.damping ** 2 + speed = max(norm(velocity), 1.0) + self.target_bearing = math.atan2(centre[1] - position[1], + centre[0] - position[0]) + self.distance = 0.3183099 * self.damping * self.period * speed + from_centre = minus(position, centre) + if norm(from_centre) > 0.1: + outwards = unit(from_centre) + else: + outwards = unit(velocity) + # capturing the centre, as a waypoint + xtrack_velocity = cross(outwards, velocity) + ltrack_velocity = -dot(velocity, outwards) + nu = self._prevent_indecision( + math.atan2(xtrack_velocity, ltrack_velocity), yaw) + self.last_nu = nu + nu = min(max(nu, -math.pi / 2), math.pi / 2) + capture = k_l1 * speed * speed / self.distance * math.sin(nu) + # tracking the circle + radial_velocity = -ltrack_velocity + radial_error = norm(from_centre) - radius + pd = radial_error * kx + radial_velocity * kv + tangential = xtrack_velocity * direction + centripetal = tangential ** 2 / max(0.5 * radius, radius + radial_error) + if ltrack_velocity < 0: + if tangential < 0: + pd = max(pd, 0.0) + elif radial_error < 0: + pd = max(pd, -centripetal) + circle = direction * (pd + centripetal) + # the switch from capture to circling happens where the two demands + # cross, and only outside the circle + if radial_error > 0 and direction * capture < direction * circle: + self.lateral_acceleration = capture + self.circling = False + else: + self.lateral_acceleration = circle + self.circling = True + + +class MissionFlight(object): + '''ArduPlane flying a mission in AUTO, from home''' + + def __init__(self, origin, home, items, params, heading=None, start=None, + rally=None, approach=None, takeoff_course=None): + self.origin = origin + self.lon_scale = math.cos(math.radians(origin[0])) + self.params = params + self.l1 = L1Control(parameter(params, 'NAVL1_PERIOD'), + parameter(params, 'NAVL1_DAMPING'), + parameter(params, 'NAVL1_XTRACK_I'), + parameter(params, 'NAVL1_LIM_BANK')) + self.cruise = parameter(params, 'AIRSPEED_CRUISE') + self.airspeed = self.cruise + self.airspeed_min = parameter(params, 'AIRSPEED_MIN') + self.airspeed_max = parameter(params, 'AIRSPEED_MAX') + self.roll_limit = math.radians(parameter(params, 'ROLL_LIMIT')) + self.roll_tconst = max(parameter(params, 'RLL2SRV_TCONST'), 0.05) + self.wp_radius = parameter(params, 'WP_RADIUS') + self.wp_max_radius = parameter(params, 'WP_MAX_RADIUS') + self.loiter_radius = parameter(params, 'WP_LOITER_RAD') + self.climb = parameter(params, 'TECS_CLMB_MAX') + self.sink = parameter(params, 'TECS_SINK_MAX') + self.alt_tconst = max(parameter(params, 'TECS_TIME_CONST'), 0.5) + self.alt_slope_min = parameter(params, 'ALT_SLOPE_MIN') + self.climb_slope_height = parameter(params, 'CLIMB_SLOPE_HGT') + self.rtl_altitude = parameter(params, 'RTL_ALTITUDE') + self.rtl_radius = parameter(params, 'RTL_RADIUS') + self.rally_limit = parameter(params, 'RALLY_LIMIT_KM') * 1000.0 + self.rally_incl_home = parameter(params, 'RALLY_INCL_HOME') > 0 + self.quadplane = parameter(params, 'Q_ENABLE') > 0 + self.q_options = int(parameter(params, 'Q_OPTIONS')) + self.immediate_climb = bool(int(parameter(params, 'FLIGHT_OPTIONS')) & + FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO) + # AP_AHRS::using_airspeed_sensor, so far as the parameters say. + # ARSPD_USE 2, for a glider with its sensor behind the propeller, is + # not used while the throttle is running (AP_Airspeed::use): a + # takeoff is taken to run it, as all but a glider's tow or winch + # launch does, so such a sensor is not one to fly the climb out by + self.airspeed_sensor = (parameter(params, 'ARSPD_TYPE') != 0 and + parameter(params, 'ARSPD_USE') == 1) + # whether the aircraft is returning to launch, rather than in AUTO + self.rtl = False + self.q_rtl_mode = int(parameter(params, 'Q_RTL_MODE')) + self.q_rtl_alt = parameter(params, 'Q_RTL_ALT') + self.q_approach_radius = parameter(params, 'Q_FW_LND_APR_RAD') + self.q_trans_decel = parameter(params, 'Q_TRANS_DECEL') + # the course a VTOL landing approach is flown on + if approach is None: + approach = STILL_AIR_APPROACH + self.approach = math.radians(approach) + # the jumps followed so far, by index + self.jumps_taken = set() + # whether the mission has come round to a jump repeated for ever, + # which it never gets past + self.looping = False + self.home = self.local(home[0], home[1]) + self.home_amsl = home[2] + self.rally = [(self.local(lat, lon), amsl) + for (lat, lon, amsl) in (rally or [])] + self.items = items + # the course a VTOL takeoff transitions on, where there is + # something to go on: the way the aircraft points + self.takeoff_heading = heading + # the course a fixed-wing takeoff was flown on, where a flight of it + # has been measured; nothing says it beforehand + self.takeoff_course = takeoff_course + # where the aircraft is when the mission starts: home, unless it + # started somewhere else + self.position = self.home + self.amsl = home[2] + if start is not None: + self.position = self.local(start[0], start[1]) + if start[2] is not None: + self.amsl = start[2] + self.yaw = 0.0 + self.roll = 0.0 + self.time = 0.0 + self.points = [] + self.last_point = None + # what set_next_WP() keeps + self.prev_wp = (self.position, self.amsl) + self.next_wp = (self.position, self.amsl) + self.crosstrack = False + self.next_wp_crosstrack = False + self.offset_altitude = 0.0 + self.target_amsl = self.amsl + # QRTL's approach: how far above where it returns to the aircraft + # was, and how far from it, when the approach began, once it has + self.qrtl = False + self.qrtl_start = None + + def local(self, lat, lon): + return ((lat - self.origin[0]) * 1.0e7 * LOCATION_SCALING_FACTOR, + wrap_longitude(lon - self.origin[1]) * 1.0e7 * + LOCATION_SCALING_FACTOR * self.lon_scale) + + def latlon(self, point): + '''back from local(). The longitude is left unwrapped, so a path + across the antimeridian stays continuous, as the 3D map's rings do''' + return (self.origin[0] + point[0] / (1.0e7 * LOCATION_SCALING_FACTOR), + self.origin[1] + point[1] / (1.0e7 * LOCATION_SCALING_FACTOR * + self.lon_scale)) + + def location(self, item): + '''an item's location and altitude. An item with no position of its + own is flown where the aircraft is, and one with no altitude at the + one it has''' + (command, lat, lon, amsl, params) = item + if lat == 0 and lon == 0: + point = self.position + else: + point = self.local(lat, lon) + if amsl is None: + amsl = self.amsl + return (point, amsl) + + def jump_target(self, index): + '''the index a DO_JUMP or DO_JUMP_TAG at index goes to, or None if it + goes nowhere there is''' + (command, lat, lon, amsl, params) = self.items[index] + target = int(params[0] or 0) + if command == MAV_CMD_DO_JUMP_TAG: + for (i, item) in enumerate(self.items): + if item[0] == MAV_CMD_JUMP_TAG and int(item[4][0] or 0) == target: + return i + return None + # items are numbered from home, which is not among them + if target < 1 or target > len(self.items): + return None + return target - 1 + + def jumps(self, index): + '''whether the jump at index is followed. AP_Mission follows one its + repeat count times, or for ever with a count of -1, and never one + asking for fewer than that; drawn, each is followed once, so the path + goes round a loop the once''' + repeats = int(self.items[index][4][1] or 0) + return repeats >= JUMP_FOREVER and repeats != 0 and \ + index not in self.jumps_taken + + def loops_forever(self, index): + '''whether the jump at index has already been followed and is + followed for ever, so that the mission never gets past it''' + repeats = int(self.items[index][4][1] or 0) + return repeats == JUMP_FOREVER and index in self.jumps_taken + + def next_nav_index(self, index, take=False, passed=None): + '''AP_Mission::get_next_nav_cmd: the index of the next item after + index which ArduPlane navigates to, following jumps, or None. take + counts the jumps as followed, and passed collects the indexes of the + other commands on the way''' + i = index + 1 + followed = set() + while i < len(self.items): + command = self.items[i][0] + if command in (mavutil.mavlink.MAV_CMD_DO_JUMP, MAV_CMD_DO_JUMP_TAG): + if i in followed: + # round to this jump again without navigating anywhere. + # AP_Mission gives up only while it is looking ahead + # (AP_Mission::get_next_cmd); advancing, it runs the + # jump's repeats out and carries on past it + if not take: + return None + i += 1 + continue + if self.loops_forever(i): + # the aircraft goes round this loop for ever, and has + # been drawn going round it once + self.looping = True + return None + if self.jumps(i): + target = self.jump_target(i) + if target is None: + return None + followed.add(i) + if take: + self.jumps_taken.add(i) + i = target + continue + i += 1 + continue + if command in SKIPPED_COMMANDS: + i += 1 + continue + if (command in LOITER_COMMANDS or command in WAYPOINT_COMMANDS or + command in TAKEOFF_COMMANDS or + command < mavutil.mavlink.MAV_CMD_NAV_LAST or + command in LATE_NAV_COMMANDS): + # including a navigation command this does not know how to fly + return i + if passed is not None: + passed.append(i) + i += 1 + return None + + def add_point(self, force=False): + if self.last_point is not None and not force: + (point, yaw) = self.last_point + if (norm(minus(self.position, point)) < POINT_SPACING and + abs(wrap_pi(self.yaw - yaw)) < POINT_TURN): + return + self.last_point = (self.position, self.yaw) + (lat, lon) = self.latlon(self.position) + self.points.append((lat, lon, self.amsl)) + + def set_next_wp(self, location): + '''Plane::set_next_WP''' + if self.next_wp_crosstrack: + self.prev_wp = self.next_wp + self.crosstrack = True + else: + self.prev_wp = (self.position, self.amsl) + self.next_wp_crosstrack = True + self.crosstrack = False + self.next_wp = location + if path_proportion(self.position, self.prev_wp[0], self.next_wp[0]) >= 1: + self.prev_wp = (self.position, self.amsl) + # Plane::setup_alt_slope: RTL only slopes down to where it is going, + # climbing to it at once, as AUTO does with IMMEDIATE_CLIMB_IN_AUTO + above = self.amsl > self.next_wp[1] + if not above and (self.rtl or self.immediate_climb): + self.offset_altitude = 0.0 + return + # Plane::set_offset_altitude_location + self.offset_altitude = self.next_wp[1] - self.prev_wp[1] + if (self.alt_slope_min <= 0 or + abs(self.offset_altitude) < self.alt_slope_min): + self.offset_altitude = 0.0 + + def update_target_altitude(self): + '''Mode::update_target_altitude, for AUTO away from landing''' + if self.qrtl: + self.update_qrtl_target_altitude() + return + (end, end_amsl) = self.next_wp + (start, start_amsl) = self.prev_wp + if self.l1.circling: + self.target_amsl = end_amsl + elif (self.offset_altitude != 0 and + path_proportion(self.position, start, end) < 1): + if (self.offset_altitude > 0 and + self.amsl - self.home_amsl < self.climb_slope_height): + # a full rate climb up to CLIMB_SLOPE_HGT + self.target_amsl = end_amsl + return + proportion = path_proportion(self.position, start, end) + proportion = min(max(1.0 - proportion, 0.0), 1.0) + target = end_amsl - self.offset_altitude * proportion + self.target_amsl = min(max(target, min(start_amsl, end_amsl)), + max(start_amsl, end_amsl)) + else: + self.target_amsl = end_amsl + + def update_qrtl_target_altitude(self): + '''ModeQRTL::update_target_altitude, for an aircraft which entered + QRTL flying as a plane, whose position controller is then on its + approach: it comes in at RTL_ALTITUDE, or down from where it started + towards that where the difference is worth a slope, and drops to + Q_RTL_ALT over the last stretch, which is as long as descending at + 0.6 of TECS_SINK_MAX at AIRSPEED_CRUISE takes. + + This is ArduPilot master's, since bd9c0285fc. Plane 4.7 and + before have no slope: they go to RTL_ALTITUDE at once, so a return + entered well above it is flown lower than drawn here until the + last stretch''' + (end, end_amsl) = self.next_wp + radius = max(abs(self.loiter_radius), abs(self.rtl_radius)) + rtl_alt_delta = max(0.0, self.rtl_altitude - self.q_rtl_alt) + sink_time = rtl_alt_delta / max(0.6 * self.sink, 1.0) + sink_dist = self.cruise * sink_time + dist = norm(minus(end, self.position)) + rad_min = 2 * radius + rad_max = 20 * radius + reached = max(rad_min, min(rad_max, rad_min + sink_dist)) + if self.qrtl_start is None: + self.qrtl_start = (self.amsl - end_amsl, dist) + (start_delta, start_dist) = self.qrtl_start + if dist > reached: + if (self.alt_slope_min > 0 and + start_delta - rtl_alt_delta >= self.alt_slope_min and + start_dist > reached): + alt = linear_interpolate(rtl_alt_delta, start_delta, dist, + reached, start_dist) + # never back up to the slope, once below it + alt = min(alt, max(rtl_alt_delta, self.amsl - end_amsl)) + else: + alt = rtl_alt_delta + else: + alt = linear_interpolate(0.0, rtl_alt_delta, dist, rad_min, + reached) + self.target_amsl = end_amsl + alt + + def fly(self, lateral_acceleration, dt, climb=None): + '''move the aircraft on by dt, banking towards the controller's + demand, and climbing or descending towards the target altitude, or + at the climb rate given''' + demand = math.atan(lateral_acceleration / GRAVITY) + demand = min(max(demand, -self.roll_limit), self.roll_limit) + self.roll += (demand - self.roll) * (1.0 - math.exp(-dt / self.roll_tconst)) + speed = self.speed() + self.yaw = wrap_pi(self.yaw + GRAVITY * math.tan(self.roll) / speed * dt) + self.position = (self.position[0] + speed * math.cos(self.yaw) * dt, + self.position[1] + speed * math.sin(self.yaw) * dt) + if climb is None: + rate = (self.target_amsl - self.amsl) / self.alt_tconst + climb = min(max(rate, -self.sink), self.climb) + self.amsl += climb * dt + self.time += dt + self.add_point() + + def speed(self): + return self.airspeed * eas2tas(self.amsl) + + def velocity(self): + speed = self.speed() + return (speed * math.cos(self.yaw), speed * math.sin(self.yaw)) + + def turn_angle(self, index): + '''Plane::setup_turn_angle: the turn at the end of this leg''' + following = self.next_nav_index(index) + if following is None: + return 90.0 + (end, _) = self.next_wp + (after, _) = self.location(self.items[following]) + if norm(minus(after, end)) < 1.0e-6: + return 90.0 + leg = math.atan2(end[1] - self.prev_wp[0][1], end[0] - self.prev_wp[0][0]) + course = math.atan2(after[1] - end[1], after[0] - end[0]) + return math.degrees(wrap_pi(course - leg)) + + def time_limit(self, extra=0.0): + (end, _) = self.next_wp + leg = norm(minus(end, self.position)) + limit = self.time + 120.0 + LEG_ALLOWANCE * leg / self.speed() + extra + return min(limit, MAX_FLIGHT_TIME) + + def rtl_location(self, home_amsl=None): + '''AP_Rally::calc_best_rally_or_home_location: home, at home_amsl + where that is given, and otherwise RTL_ALTITUDE above it, or at the + altitude the aircraft is at if that is negative; or the rally point + nearest the aircraft, at its own altitude, where it is within + RALLY_LIMIT_KM and, with RALLY_INCL_HOME set, nearer than home. A + rally point whose altitude is not known raises, since nobody can say + how high the aircraft flies to it''' + if home_amsl is not None: + best = (self.home, home_amsl) + elif self.rtl_altitude < 0: + best = (self.home, self.amsl) + else: + best = (self.home, self.home_amsl + self.rtl_altitude) + nearest = None + for (point, amsl) in self.rally: + distance = norm(minus(point, self.position)) + if nearest is None or distance < nearest[0]: + nearest = (distance, point, amsl) + if nearest is None: + return best + (distance, point, amsl) = nearest + if self.rally_limit > 0 and distance > self.rally_limit: + return best + if (self.rally_incl_home and + distance >= norm(minus(self.home, self.position))): + return best + if amsl is None: + # the caller could not work out how high the point is -- terrain + # it does not have, most likely -- so nobody can say how high + # the aircraft flies its return, and it is not drawn flown + raise ValueError('rally point altitude unknown') + return (point, amsl) + + def fly_waypoint(self, index): + '''Plane::verify_nav_wp. False if the aircraft never gets there''' + (command, lat, lon, amsl, params) = self.items[index] + acceptance = 0.0 + passby = 0.0 + if command == mavutil.mavlink.MAV_CMD_NAV_WAYPOINT: + acceptance = min(max(params[1] or 0.0, 0.0), 255.0) + passby = min(max(params[2] or 0.0, 0.0), 255.0) + final = command != mavutil.mavlink.MAV_CMD_NAV_WAYPOINT + turn_angle = self.turn_angle(index) + limit = self.time_limit() + while self.time < limit: + (start, _) = self.prev_wp + (end, _) = self.next_wp + if passby > 0 and norm(minus(end, start)) > 0: + along = unit(minus(end, start)) + end = (end[0] + along[0] * passby, end[1] + along[1] * passby) + track_start = start if self.crosstrack else self.position + self.l1.update_waypoint(self.position, self.velocity(), self.yaw, + track_start, end, NAV_PERIOD) + self.update_target_altitude() + if (not final and self.wp_max_radius > 0 and + norm(minus(self.next_wp[0], self.position)) > + self.wp_max_radius): + # WP_MAX_RADIUS: not reached until this close, however far + # past it the aircraft has flown. Having flown past, it + # comes back round for it along a track from where it is + if (passby == 0 and + path_proportion(self.position, start, end) >= 1): + self.prev_wp = (self.position, self.amsl) + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + continue + if final: + accept = 0.0 + elif acceptance > 0: + accept = acceptance + elif passby == 0: + accept = self.l1.turn_distance(self.wp_radius, self.amsl, + turn_angle) + else: + accept = 0.0 + if (norm(minus(end, self.position)) <= max(accept, 1.0) or + path_proportion(self.position, start, end) >= 1): + return True + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + return False + + def fly_loiter(self, index): + '''Plane::verify_loiter_*, flying update_loiter() until the item is + done. False if it never is''' + mavlink = mavutil.mavlink + (command, lat, lon, amsl, params) = self.items[index] + if command == mavlink.MAV_CMD_NAV_LOITER_TIME: + requested = 0.0 + ccw = (params[2] or 0.0) < 0 + elif command == mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + requested = params[1] or 0.0 + ccw = requested < 0 + else: + requested = params[2] or 0.0 + ccw = requested < 0 + radius = abs(requested) + direction = -1 if ccw else 1 + if radius <= 1: + # Plane::update_loiter: WP_LOITER_RAD, turned the way its sign says + # unless the item asks for counter-clockwise + radius = default_loiter_radius(self.loiter_radius) + if not ccw: + direction = -1 if self.loiter_radius < 0 else 1 + if command == mavlink.MAV_CMD_NAV_LOITER_TURNS and radius > 255: + radius = min(255.0, radius * 0.1) * 10.0 + exit_from_here = not mp_util.mission_crosstracks_from_centre(command, + params) + # AP_Mission keeps whole turns in a byte, and a turn less than one + # in 256ths of one + turns = min(max(params[0] or 0.0, 0.0), 255.0) + if turns >= 1: + turns = float(int(turns)) + else: + turns = int(turns * 256) / 256.0 + loiter_time = max(params[0] or 0.0, 0.0) + started = False + start_time = None + swept = 0.0 + old_bearing = None + reached_alt = False + unable = False + lap_check = 3 * 360.0 + lap_alt = self.amsl + lining_up = False + following = self.next_nav_index(index) + seconds_per_lap = 2 * math.pi * radius * 1.5 / max(self.speed(), 1.0) + extra = MAX_LAPS * seconds_per_lap + if command == mavlink.MAV_CMD_NAV_LOITER_TIME: + extra += loiter_time + elif command == mavlink.MAV_CMD_NAV_LOITER_TURNS: + extra += turns * seconds_per_lap + elif command == mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + (_, centre_amsl) = self.next_wp + rate = self.climb if centre_amsl > self.amsl else self.sink + extra += 1.5 * abs(centre_amsl - self.amsl) / max(rate, 0.1) + limit = self.time_limit(extra) + while self.time < limit: + (centre, centre_amsl) = self.next_wp + (start, _) = self.prev_wp + flown = self.l1.loiter_radius(radius, self.amsl, self.airspeed) + if (not started and self.crosstrack and + norm(minus(centre, self.position)) > 3 * flown): + self.l1.update_waypoint(self.position, self.velocity(), self.yaw, + start, centre, NAV_PERIOD) + else: + self.l1.update_loiter(self.position, self.velocity(), self.yaw, + centre, flown, direction) + if not started and (self.l1.circling or + path_proportion(self.position, start, centre) > 1): + started = True + start_time = self.time + # Plane::loiter_angle_update + reached = self.l1.circling + if swept == 0 and not reached: + delta = 0.0 + elif swept == 0: + delta = 0.01 + lap_alt = self.amsl + lap_check = 3 * 360.0 + else: + delta = math.degrees(wrap_pi(self.l1.target_bearing - old_bearing)) + old_bearing = self.l1.target_bearing + swept += delta * direction + if reached and abs(self.amsl - centre_amsl) < 5.0: + reached_alt = True + elif not reached_alt and abs(swept) >= lap_check: + unable = abs(self.amsl - lap_alt) < 5.0 + lap_alt = self.amsl + lap_check += 3 * 360.0 + self.update_target_altitude() + if not lining_up: + if command == mavlink.MAV_CMD_NAV_LOITER_UNLIM: + if reached and swept >= 360.0: + return False + elif command == mavlink.MAV_CMD_NAV_LOITER_TURNS: + lining_up = reached and swept > turns * 360.0 and swept > 0.01 + elif command == mavlink.MAV_CMD_NAV_LOITER_TIME: + lining_up = (start_time is not None and swept > 0.01 and + self.time - start_time > loiter_time) + else: + lining_up = abs(swept) > 0.01 and (reached_alt or unable) + if lining_up: + swept = 0.0 + old_bearing = self.l1.target_bearing + if lining_up and (reached or + command != mavlink.MAV_CMD_NAV_LOITER_TURNS): + if following is None: + return True + if self.lined_up(centre, flown, direction, swept, + self.lineup_target(self.items[following], + centre)): + if exit_from_here: + self.next_wp = (self.position, centre_amsl) + return True + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + return False + + def lineup_target(self, item, centre): + '''where Plane::verify_loiter_heading lines a loiter's exit up with: + the next item's location as the mission holds it, which for one with + no position of its own is 0, 0 -- only starting it puts it where the + aircraft is -- as Location::get_distance_NE measures it from the + loiter's centre, the longitude scaled at the latitude between them''' + (command, lat, lon, amsl, params) = item + (clat, clon) = self.latlon(centre) + scale = max(math.cos(math.radians((lat + clat) / 2.0)), 0.01) + return (centre[0] + (lat - clat) * 1.0e7 * LOCATION_SCALING_FACTOR, + centre[1] + wrap_longitude(lon - clon) * 1.0e7 * + LOCATION_SCALING_FACTOR * scale) + + def lined_up(self, centre, radius, direction, swept, target): + '''ModeLoiter::isHeadingLinedUp''' + if radius <= 0: + return True + projected = minus(self.position, centre) + if norm(projected) < 1.0e-6: + projected = (math.cos(self.yaw), math.sin(self.yaw)) + projected = unit(projected) + projected = (projected[0] * radius, projected[1] * radius) + to_target = minus(target, centre) + target_distance = norm(to_target) + if target_distance <= 0: + return True + if target_distance >= radius: + bearing = math.atan2(to_target[1] - projected[1], + to_target[0] - projected[0]) + else: + chord = radius - target_distance + segment = 2.0 * math.asin(chord / (2.0 * radius)) + bearing = wrap_pi(math.atan2(to_target[1], to_target[0]) + + (math.pi / 2 - segment) * direction) + heading = wrap_pi(math.atan2(projected[1], projected[0]) + + math.pi / 2 * direction) + acceptance = 10.0 + 10.0 * int(abs(swept) / 360.0) + return abs(math.degrees(wrap_pi(bearing - heading))) <= acceptance + + def vtol_takeoff(self, command): + '''QuadPlane::is_vtol_takeoff''' + return vtol_takeoff(command, self.quadplane, self.q_options) + + def face(self, index): + '''turn to face the item at index, if it is somewhere else''' + if index is None: + return + (point, _) = self.location(self.items[index]) + if norm(minus(point, self.position)) > 1.0: + self.yaw = math.atan2(point[1] - self.position[1], + point[0] - self.position[0]) + + def fly_takeoff(self, index): + '''climb out to the takeoff altitude. A VTOL takeoff climbs straight + up where the aircraft is, and sets off the way it points: the heading + the caller has for it, and otherwise, as a guess, towards where the + mission goes next. A fixed-wing one climbs away at the item's pitch, + or faster where TECS has an airspeed sensor to fly by, until it is + past the takeoff altitude. It holds the ground course it had once + it got moving (Plane::verify_takeoff), which is wherever it was + launched or pointed down the runway: the course the caller measured + a flight of it on, where it has one, and otherwise towards where the + mission goes next, which is at least where it heads once it is up''' + (command, lat, lon, amsl, params) = self.items[index] + (point, target) = self.location(self.items[index]) + if self.vtol_takeoff(command): + # it transitions the way it is pointing, which is the way it + # was pointing on the ground, where that is known + self.amsl = max(self.amsl, target) + if self.takeoff_heading is not None: + self.yaw = wrap_pi(math.radians(self.takeoff_heading)) + else: + self.face(self.next_nav_index(index)) + self.add_point(force=True) + else: + if self.takeoff_course is not None: + self.yaw = wrap_pi(math.radians(self.takeoff_course)) + else: + self.face(self.next_nav_index(index)) + # Plane::takeoff_calc_pitch: no lower than the item's pitch, + # which is all it climbs at without an airspeed sensor for TECS + # to fly by + pitch = params[0] if params[0] > 0 else DEFAULT_TAKEOFF_PITCH + climb = self.speed() * math.tan(math.radians(pitch)) + if self.airspeed_sensor: + climb = max(climb, self.climb) + limit = min(self.time + 600.0, MAX_FLIGHT_TIME) + while self.amsl < target and self.time < limit: + self.target_amsl = target + self.fly(0.0, NAV_PERIOD, climb=climb) + # Plane::verify_takeoff: no crosstracking the leg after a takeoff + self.next_wp = self.prev_wp = (self.position, self.amsl) + self.next_wp_crosstrack = False + self.target_amsl = self.amsl + return True + + def change_speed(self, params): + '''Plane::do_change_speed, for the airspeed it flies at. Only an + airspeed change, of -2 for the cruise airspeed or one within + AIRSPEED_MIN and AIRSPEED_MAX, changes it: a groundspeed change sets + a minimum ground speed, which there is no wind here to call on''' + (speed_type, speed) = (params[0] or 0.0, params[1] or 0.0) + if int(speed_type) != 0: + return + if speed == -2: + self.airspeed = self.cruise + elif self.airspeed_min <= speed <= self.airspeed_max: + self.airspeed = speed + + def run(self): + '''fly the mission. Returns the path flown, or None where it cannot + be flown through to its end''' + mavlink = mavutil.mavlink + passed = [] + index = self.next_nav_index(-1, take=True, passed=passed) + if index is None: + return None + self.face(index) + self.add_point(force=True) + while index is not None: + if self.time > MAX_FLIGHT_TIME: + return None + for i in passed: + if self.items[i][0] == mavlink.MAV_CMD_DO_CHANGE_SPEED: + self.change_speed(self.items[i][4]) + passed = [] + command = self.items[index][0] + if command in TAKEOFF_COMMANDS: + self.fly_takeoff(index) + index = self.next_nav_index(index, take=True, passed=passed) + continue + if command == mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH: + return self.fly_rtl(index) + if command in WAYPOINT_COMMANDS or command in LOITER_COMMANDS: + location = self.location(self.items[index]) + else: + return None + self.set_next_wp(location) + if spiral_approach(command, self.items[index][4], self.quadplane, + self.q_options): + if not self.fly_vtol_approach(location): + return None + break + if command == mavlink.MAV_CMD_NAV_VTOL_LAND and self.quadplane: + # QuadPlane::do_vtol_land: flown in at the altitude the + # aircraft is at, which its position controller takes it + # down from once there. How far down is the ground's, so + # the path ends above the landing at that altitude + self.next_wp = (self.next_wp[0], self.amsl) + if command in LOITER_COMMANDS: + finished = self.fly_loiter(index) + if not finished and command == mavlink.MAV_CMD_NAV_LOITER_UNLIM: + break + else: + finished = self.fly_waypoint(index) + if not finished: + return None + if command in (mavlink.MAV_CMD_NAV_LAND, + mavlink.MAV_CMD_NAV_VTOL_LAND): + break + index = self.next_nav_index(index, take=True, passed=passed) + else: + if not self.looping: + # AP_Mission has no next command, so the mission is complete + # and Plane::exit_mission_callback changes mode to RTL + for i in passed: + if self.items[i][0] == mavlink.MAV_CMD_DO_CHANGE_SPEED: + self.change_speed(self.items[i][4]) + self.items.append((mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, 0, None, (0.0, 0.0, 0.0, 0.0))) + return self.fly_rtl(len(self.items) - 1) + self.add_point(force=True) + return self.points + + def loiter_step(self, centre, radius, direction): + '''fly on by a step, circling centre''' + flown = self.l1.loiter_radius(radius, self.amsl, self.airspeed) + self.l1.update_loiter(self.position, self.velocity(), self.yaw, + centre, flown, direction) + self.update_target_altitude() + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + + def stopping_distance(self): + '''QuadPlane::stopping_distance_m''' + return self.speed() ** 2 / (2 * max(self.q_trans_decel, 0.1)) + + def fly_vtol_approach(self, location, rtl=False): + '''Plane::verify_landing_vtol_approach: circle to the altitude of + location, at Q_FW_LND_APR_RAD -- WP_LOITER_RAD where that is zero -- + the way its sign says; break out of the circle square to the + approach course; fly the approach line through location, and land + there once near enough and lined up. A return to launch, with rtl, + first circles where it returns to until it is on the circle, and + then goes down to Q_RTL_ALT. The path ends above the landing, at + the altitude it was approached at. False if the aircraft never + gets there''' + mavlink = mavutil.mavlink + radius = self.q_approach_radius or self.loiter_radius + direction = -1 if radius < 0 else 1 + radius = abs(radius) + (centre, _) = location + lap = 2 * math.pi * max(radius, 1.0) * 1.5 / max(self.speed(), 1.0) + # there is no circling for ever before the approach + self.crosstrack = False + if rtl: + limit = self.time_limit(MAX_LAPS * lap) + while not self.l1.circling: + if self.time >= limit: + return False + self.loiter_step(centre, radius, direction) + # do_RTL(): on to where a return goes, at Q_RTL_ALT above home + self.next_wp_crosstrack = False + location = self.rtl_location(self.home_amsl + self.q_rtl_alt) + (centre, _) = location + self.set_next_wp(location) + self.crosstrack = False + # LOITER_TO_ALT, flown as a loiter to altitude which nothing follows + (lat, lon) = self.latlon(centre) + self.items.append((mavlink.MAV_CMD_NAV_LOITER_TO_ALT, + lat, lon, location[1], + (0.0, direction * radius, 0.0, 0.0))) + if not self.fly_loiter(len(self.items) - 1): + return False + limit = self.time_limit(MAX_LAPS * lap) + # ENSURE_RADIUS: out onto the circle, if inside it + while True: + distance = norm(minus(centre, self.position)) + if not (distance < radius and abs(distance - radius) > 5.0): + break + if self.time >= limit: + return False + self.loiter_step(centre, radius, direction) + # WAIT_FOR_BREAKOUT: round until heading square to the approach + breakout = wrap_pi(self.approach + + math.radians(270.0 if direction > 0 else 90.0)) + while abs(wrap_pi(self.yaw - breakout)) >= math.radians(5.0): + if self.time >= limit: + return False + self.loiter_step(centre, radius, direction) + # APPROACH_LINE: the line 1km either side of the landing + self.set_next_wp(location) + along = (math.cos(self.approach), math.sin(self.approach)) + + def back(distance): + return (centre[0] - along[0] * distance, + centre[1] - along[1] * distance) + (start, end) = (back(1000.0), back(-1000.0)) + breakout_point = back(radius) + limit = self.time_limit() + while True: + self.l1.update_waypoint(self.position, self.velocity(), self.yaw, + start, end, NAV_PERIOD) + self.update_target_altitude() + past = path_proportion(self.position, start, + back(self.stopping_distance())) >= 1 + half_radius = path_proportion(self.position, breakout_point, + centre) > 0.5 + to_centre = minus(centre, self.position) + velocity = self.velocity() + lined_up = True + if norm(to_centre) > 0: + cosine = (dot(velocity, to_centre) / + (norm(velocity) * norm(to_centre))) + lined_up = math.degrees( + math.acos(min(max(cosine, -1.0), 1.0))) < 30.0 + if past and (lined_up or half_radius): + break + if self.time >= limit: + return False + self.fly(self.l1.lateral_acceleration, NAV_PERIOD) + # VTOL_LANDING: QuadPlane::do_vtol_land, whose position controller + # takes the aircraft from here to where it lands, drawn straight + # there. How far down it goes is the ground's, which is not known + # here, so the path ends above the landing at the approach altitude + self.position = centre + self.amsl = location[1] + self.add_point(force=True) + return True + + def fly_rtl(self, index): + '''a return to launch, which ArduPlane flies in RTL mode, or QRTL + where Q_RTL_MODE has a QuadPlane do so. It ends the flight: the + path returned is the one flown''' + mavlink = mavutil.mavlink + # Plane::do_RTL: flown from wherever the aircraft is + self.rtl = True + self.next_wp_crosstrack = False + qrtl = self.quadplane and self.q_rtl_mode in (Q_RTL_SWITCH_QRTL, + Q_RTL_QRTL_ALWAYS) + if self.quadplane and self.q_rtl_mode == Q_RTL_QRTL_ALWAYS: + # ModeRTL::_enter goes straight to QRTL, which returns at + # Q_RTL_ALT + location = self.rtl_location(self.home_amsl + self.q_rtl_alt) + else: + location = self.rtl_location() + self.set_next_wp(location) + if self.quadplane and self.q_rtl_mode == Q_RTL_VTOL_APPROACH: + if not self.fly_vtol_approach(location, rtl=True): + return None + elif qrtl: + # with Q_RTL_MODE 1 the aircraft flies at the point in RTL, along + # a track from where it turned for it + # (Plane::update_loiter_update_nav), and QRTL takes it the rest + # of the way; QRTL, which 3 goes to at once, steers for it from + # wherever the aircraft is, do_RTL() having left no track + # (QuadPlane::vtol_position_controller) + self.crosstrack = self.q_rtl_mode == Q_RTL_SWITCH_QRTL + # QRTL comes in on its own altitude profile, which Q_RTL_MODE 1 + # only switches to near home, where the path ends + self.qrtl = self.q_rtl_mode == Q_RTL_QRTL_ALWAYS + if not self.fly_waypoint(index): + return None + else: + # ModeRTL::update: the aircraft circles where it returns to, at + # RTL_RADIUS -- WP_LOITER_RAD where that is zero -- the way its + # sign says. It is flown by update_loiter() from the start, + # with no track to follow there since do_RTL() leaves none, so + # it joins the circle rather than flying over its middle + self.items.append((mavlink.MAV_CMD_NAV_LOITER_UNLIM, + location[0][0], location[0][1], location[1], + (0.0, 0.0, self.rtl_radius, 0.0))) + self.fly_loiter(len(self.items) - 1) + self.add_point(force=True) + return self.points + + +def vtol_takeoff(command, quadplane, q_options): + '''QuadPlane::is_vtol_takeoff: whether command is flown as a VTOL + takeoff, straight up, rather than as a fixed-wing one''' + if command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF: + return True + return (command == mavutil.mavlink.MAV_CMD_NAV_TAKEOFF and quadplane and + not q_options & Q_OPTION_ALLOW_FW_TAKEOFF) + + +def takeoff_is_vtol(command, params): + '''whether command is flown as a VTOL takeoff by a vehicle with these + parameters''' + return vtol_takeoff(command, parameter(params, 'Q_ENABLE') > 0, + int(parameter(params, 'Q_OPTIONS'))) + + +def spiral_approach(command, params, quadplane, q_options): + '''QuadPlane::landing_with_fixed_wing_spiral_approach: whether command, + with params, is a VTOL landing flown in on a fixed-wing approach''' + return (quadplane and command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND and + (bool(q_options & Q_OPTION_MISSION_LAND_FW_APPROACH) or + params[0] == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH)) + + +def uses_vtol_approach(items, params): + '''whether flying items takes the course of a VTOL landing approach, + which is into the wind: a landing flown in on a fixed-wing approach, or + a return to launch which Q_RTL_MODE has approach its landing so -- the + one an item asks for, or the one a mission which runs out flies''' + quadplane = parameter(params, 'Q_ENABLE') > 0 + q_options = int(parameter(params, 'Q_OPTIONS')) + q_rtl_mode = int(parameter(params, 'Q_RTL_MODE')) + returning = quadplane and q_rtl_mode == Q_RTL_VTOL_APPROACH + runs_out = True + walk = MissionFlight((0.0, 0.0), (0.0, 0.0, 0.0), list(items), {}) + try: + index = walk.next_nav_index(-1, take=True) + while index is not None: + (command, _, _, _, item_params) = walk.items[index] + if spiral_approach(command, item_params, quadplane, q_options): + return True + if (returning and + command == mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH): + return True + if command in ENDS_THE_FLIGHT: + # the flight ends there rather than running out of mission + runs_out = False + break + index = walk.next_nav_index(index, take=True) + except (ValueError, ZeroDivisionError, OverflowError): + # an item nothing can make sense of, as mission_track() takes it: + # there is no path to draw for this mission, wind or no wind + return False + if walk.looping: + # round a loop for ever, so the mission never runs out either + runs_out = False + return returning and runs_out + + +def is_navigation_command(command): + '''whether ArduPlane flies command, rather than just doing it''' + return (command < mavutil.mavlink.MAV_CMD_NAV_LAST or + command in LATE_NAV_COMMANDS) + + +def positionless_amsl(alt, frame, home_amsl): + '''the AMSL altitude an item with no position of its own is flown at, + or None for the altitude the aircraft is at. Location::sanitize() puts + such an item where the aircraft is, and Plane::set_next_WP() keeps the + altitude it has there unless that is zero, in whatever frame, which is + the altitude the aircraft is at as well; a terrain-relative one needs + the terrain under wherever that is, which is not known here''' + if alt is None or alt == 0: + return None + if frame in (0, 5): + return alt + if frame not in (3, 6) or home_amsl is None: + return None + return home_amsl + alt + + +# Location::AltFrame, as a rally point's flags carry it +RALLY_ALT_ABSOLUTE = 0 +RALLY_ALT_ABOVE_HOME = 1 +RALLY_ALT_ABOVE_ORIGIN = 2 +RALLY_ALT_ABOVE_TERRAIN = 3 + + +def rally_alt_frame(flags): + '''the frame a rally point's altitude is in. + AP_Rally::rally_location_to_location(): above home, unless the flags say + the frame they carry is a valid one''' + if int(flags or 0) & (1 << 2): + return (int(flags) >> 3) & 3 + return RALLY_ALT_ABOVE_HOME + + +def rally_alt_is_fixed(flags): + '''whether a rally point is at an altitude of its own rather than one + which moves with home''' + return rally_alt_frame(flags) != RALLY_ALT_ABOVE_HOME + + +def rally_amsl(alt, flags, home_amsl, origin_amsl=None, terrain_amsl=None): + '''the AMSL altitude of a rally point, or None where it cannot be told. + + Location::get_alt_cm() measures the frame the flags carry from its own + datum: home, the EKF origin, or the terrain below the point, which the + caller looks up where it can''' + if alt is None: + return None + frame = rally_alt_frame(flags) + if frame == RALLY_ALT_ABSOLUTE: + return float(alt) + datum = {RALLY_ALT_ABOVE_HOME: home_amsl, + RALLY_ALT_ABOVE_ORIGIN: origin_amsl, + RALLY_ALT_ABOVE_TERRAIN: terrain_amsl}[frame] + if datum is None: + return None + return datum + alt + + +def default_loiter_radius(wp_loiter_rad): + '''the radius of a loiter which gives none of its own: WP_LOITER_RAD, + or LOITER_RADIUS_DEFAULT where that is no radius at all + (Plane::update_loiter)''' + if abs(wp_loiter_rad) <= 1: + return 60.0 + return abs(wp_loiter_rad) + + +def linear_interpolate(low_output, high_output, value, low, high): + '''AP_Math's linear_interpolate: output from low_output at low to + high_output at high, held at either end beyond them''' + if low > high: + (low, high) = (high, low) + (low_output, high_output) = (high_output, low_output) + if value <= low: + return low_output + if value >= high: + return high_output + p = (value - low) / (high - low) + return low_output + p * (high_output - low_output) + + +def least_flight_time(home, items, params, start=None): + '''the least time flying the mission could take: straight from each + item it navigates to on to the next, in the order the flight takes them + -- following jumps as it does -- at the fastest airspeed it may fly, and + loitering for the times asked. start is where the aircraft is when the + mission starts, where that is not home''' + mavlink = mavutil.mavlink + speed = max(parameter(params, 'AIRSPEED_CRUISE'), + parameter(params, 'AIRSPEED_MAX'), 1.0) + walk = MissionFlight((home[0], home[1]), home, list(items), {}) + seconds = 0.0 + (lat, lon) = (start[0], start[1]) if start else (home[0], home[1]) + index = walk.next_nav_index(-1, take=True) + while index is not None: + (command, item_lat, item_lon, amsl, item_params) = walk.items[index] + if command == mavlink.MAV_CMD_NAV_LOITER_TIME: + seconds += max(item_params[0] or 0.0, 0.0) + if not (item_lat == 0 and item_lon == 0): + seconds += mp_util.gps_distance(lat, lon, item_lat, item_lon) / speed + (lat, lon) = (item_lat, item_lon) + if command in (mavlink.MAV_CMD_NAV_LAND, mavlink.MAV_CMD_NAV_VTOL_LAND, + mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + mavlink.MAV_CMD_NAV_LOITER_UNLIM): + break + index = walk.next_nav_index(index, take=True) + return seconds + + +def first_navigation_item(items): + '''the number of the first item in items -- numbered from 1, as + mission_track() takes them -- which ArduPlane navigates to when the + mission starts, or None, which an item nothing can make sense of leaves + as well, as it leaves mission_track() with no path''' + flight = MissionFlight((0.0, 0.0), (0.0, 0.0, 0.0), list(items), {}) + try: + index = flight.next_nav_index(-1) + except (ValueError, ZeroDivisionError, OverflowError): + return None + return None if index is None else index + 1 + + +def mission_track(home, items, params, heading=None, start=None, rally=None, + approach=None, takeoff_course=None): + '''the path an ArduPlane flies a mission along, as (lat, lon, amsl) + points, or None where it cannot be worked out. + + home is (lat, lon, amsl). items are the mission items after home, in + order and numbered from 1, as (command, lat, lon, amsl, (param1, param2, + param3, param4)): every item, since the ones with no position of their + own change how the others are flown. amsl is None where it is not + known. params are the vehicle's parameters; ArduPlane's defaults stand + in for any not there. heading is the course in degrees a VTOL takeoff + transitions on, where there is something to go on: the way the aircraft + points. takeoff_course is the course in degrees a fixed-wing takeoff + was flown on, for a caller which has measured a flight of it: nothing + says it beforehand, since ArduPlane holds whatever course the launch or + the runway gave it, so without one such a takeoff is drawn towards the + next item. start is (lat, lon, amsl) + where the aircraft is when the mission starts, where that is not home. + rally is the vehicle's rally points, as (lat, lon, amsl), where it has + any, which a return to launch may go to instead of home. approach is + the course in degrees of a VTOL landing approach, which ArduPlane flies + into the wind it estimates, as MAVLink's WIND gives where it comes from; + STILL_AIR_APPROACH where none is given. + + The aircraft sets off from home, or start, towards the first item. + Each DO_JUMP is followed the once, so a loop is drawn once however many + times it is flown, and one repeated for ever ends the flight the second + time it is reached. A mission which runs out is complete, and the + aircraft returns to launch. A navigation command this does not know how to fly + -- NAV_DELAY, CONTINUE_AND_CHANGE_ALT and the like -- ends the attempt, + as does an item the aircraft never finishes, and a flight longer than + MAX_FLIGHT_TIME + ''' + if home is None or (home[0] == 0 and home[1] == 0) or home[2] is None: + return None + try: + if least_flight_time(home, items, params, start) > MAX_FLIGHT_TIME: + return None + flight = MissionFlight((home[0], home[1]), home, list(items), params, + heading, start, rally, approach, + takeoff_course) + return flight.run() + except (ValueError, ZeroDivisionError, OverflowError): + return None diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 95e387a9de..7017b5aa45 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -10,6 +10,7 @@ import functools import time import datetime +import types from MAVProxy.modules.lib import mp_util from MAVProxy.modules.lib import mp_settings from MAVProxy.modules.lib import mp_module @@ -18,10 +19,6 @@ from pymavlink import mavutil from PIL import ImageColor -# Older pymavlink releases lack this common.xml command. -MAV_CMD_NAV_ARC_WAYPOINT = getattr(mavutil.mavlink, "MAV_CMD_NAV_ARC_WAYPOINT", 36) -MAV_CMD_DO_ORBIT = getattr(mavutil.mavlink, "MAV_CMD_DO_ORBIT", 34) - # pymavlink may not yet carry the enumeration entry for the # home-centred inclusion circle. Fall back to its known value (from # development.xml) so we don't raise AttributeError on older pymavlink: @@ -73,11 +70,11 @@ def __init__(self, mpstate): ('showahrs3pos', int, 0), ('brightness', float, 1), ('rallycircle', bool, False), - ('loitercircle', bool, False), + ('loitercircle', bool, True), ('showclicktime', int, 2), ('showwpnum', bool, True), ('circle_linewidth', int, 1), - ('showdirection', bool, False), + ('showdirection', bool, True), ('setpos_accuracy', float, 50), ('mission_color', str, "white"), ('font_size', float, 0.5), @@ -157,30 +154,22 @@ def __init__(self, mpstate): # waypoint commands mavutil.mavlink.MAV_CMD_NAV_WAYPOINT: (0, 255, 255), mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT: (64, 255, 64), - MAV_CMD_NAV_ARC_WAYPOINT: (64, 255, 255), + mp_util.MAV_CMD_NAV_ARC_WAYPOINT: (64, 255, 255), # circling commands mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM: (255, 64, 255), mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS: (255, 64, 255), mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME: (255, 64, 255), mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: (255, 64, 255), - MAV_CMD_DO_ORBIT: (255, 64, 255), + mp_util.MAV_CMD_DO_ORBIT: (255, 64, 255), # other commands mavutil.mavlink.MAV_CMD_DO_LAND_START: (255, 127, 0), } - self._label_suffix_for_wp_command = { - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: "TOff", - mavutil.mavlink.MAV_CMD_DO_LAND_START: "DLS", - mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT: "SW", - MAV_CMD_NAV_ARC_WAYPOINT: "AW", - mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM: "LU", - mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS: "LT", - mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME: "LTime", - mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: "LAlt", - MAV_CMD_DO_ORBIT: "Orbit", - mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: "VL", - } + # what each kind of item is called, which the 3D map labels with too, + # so it is not to be changed from here + self._label_suffix_for_wp_command = types.MappingProxyType( + mp_util.MISSION_LABEL_SUFFIXES) self.add_menu(MPMenuSubMenu('Terrain', items=[ MPMenuItem('Show Contours', returnkey='showTerrainContours'), @@ -470,10 +459,7 @@ def label_for_waypoint(self, wp_num, wploader=None): if wploader is None: wploader = get_mission_for_map(self.mpstate) wp = wploader.wp(wp_num) - command = wp.command - if command not in self._label_suffix_for_wp_command: - return str(wp_num) - return str(wp_num) + "(" + self._label_suffix_for_wp_command[command] + ")" + return mp_util.mission_item_label(wp_num, wp.command) def display_waypoints(self): '''display the waypoints''' diff --git a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py index 1b9a383471..a2ac0a69d6 100644 --- a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py +++ b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py @@ -14,14 +14,9 @@ import numpy as np import warnings -from pymavlink import mavutil - from MAVProxy.modules.mavproxy_map import mp_tile from MAVProxy.modules.lib import mp_util -# Older pymavlink releases lack this common.xml command. -MAV_CMD_NAV_ARC_WAYPOINT = getattr(mavutil.mavlink, "MAV_CMD_NAV_ARC_WAYPOINT", 36) - font = cv2.FONT_HERSHEY_DUPLEX @@ -45,7 +40,7 @@ def mission_arcs(wploader, wp_indexes): wp = wploader.wp(wp_indexes[i]) if wp is None: continue - if wp.command != MAV_CMD_NAV_ARC_WAYPOINT: + if wp.command != mp_util.MAV_CMD_NAV_ARC_WAYPOINT: continue # the arc runs from the previous point in the mission view arcs[i-1] = wp.param1 diff --git a/MAVProxy/modules/mavproxy_map3d/__init__.py b/MAVProxy/modules/mavproxy_map3d/__init__.py index 4a06165d6d..9a7c054cdc 100644 --- a/MAVProxy/modules/mavproxy_map3d/__init__.py +++ b/MAVProxy/modules/mavproxy_map3d/__init__.py @@ -4,6 +4,8 @@ mission, fence, rally, vehicle) for both live telemetry and log review. Andrew Tridgell / CanberraUAV + +AP_FLAKE8_CLEAN ''' import math @@ -18,7 +20,8 @@ from MAVProxy.modules.lib import mp_util from MAVProxy.modules.mavproxy_misseditor import get_mission_for_map from MAVProxy.modules.mavproxy_map3d.map3d import ( - Map3D, MissionItem, missing_packages, missing_packages_message) + Map3D, MissionItem, MISSION_LABEL_SIZE, MISSION_LABEL_SIZES, + MISSION_STYLES, missing_packages, missing_packages_message) # fence colours as the 2D map's PolyFence layer uses them (OpenCV BGR) FENCE_INCLUSION_BGR = (0, 255, 0) @@ -26,9 +29,17 @@ FENCE_EXCLUSION_BGR = (255, 0, 0) FENCE_RETURN_BGR = (255, 127, 127) FENCE_RETURN_RADIUS = 10.0 - -# takeoff items normally carry an altitude and no position -TAKEOFF_COMMANDS = (22, 84) # NAV_TAKEOFF, NAV_VTOL_TAKEOFF +# a plane's mission is flown again when the way it points on the ground turns +# this far, but not more often than this +TAKEOFF_HEADING_CHANGE = 5.0 +TAKEOFF_HEADING_REDRAW = 2.0 +# nor for home moving less than this, in metres, which it does by the GPS +# wandering while ArduPlane keeps setting it before arming +HOME_MOVE_CHANGE = 10.0 +# a VTOL landing approach is flown into the wind, whose direction the +# vehicle's estimate wanders about: a mission landing so is flown again for +# a change of this many degrees +APPROACH_CHANGE = 10.0 def bgr_to_rgb(bgr, default=(1.0, 0.0, 1.0)): @@ -39,24 +50,40 @@ def bgr_to_rgb(bgr, default=(1.0, 0.0, 1.0)): return default +def make_settings(): + '''the module's settings. The view's own controls set some of them: + showlabels, missionpath, follow and the render settings''' + return mp_settings.MPSettings([ + ('service', str, 'MicrosoftSat'), + ('zexag', float, 1.0), + ('debug', bool, False), + mp_settings.MPSetting('fpvfov', float, 90.0, range=(20.0, 150.0)), + mp_settings.MPSetting('terrainbrightness', float, 1.25, + range=(0.25, 2.0)), + ('terrainshading', bool, True), + ('terrainwireframe', bool, False), + ('showdirection', bool, True), + ('showlabels', bool, False), + mp_settings.MPSetting('labelsize', int, MISSION_LABEL_SIZE, + range=MISSION_LABEL_SIZES), + # flown, geometry or plain: see Map3D.set_mission_style() + mp_settings.MPSetting('missionpath', str, MISSION_STYLES[0], + choice=MISSION_STYLES), + ]) + + class Map3DModule(mp_module.MPModule): + # the EKF origin's altitude, which a rally point may be measured from, + # until the vehicle says where it is + origin_amsl = None + def __init__(self, mpstate): # Do not register as a public "map*" module. ADS-B, AIS, KML and other # modules use that wildcard for the 2D SlipMap API (add_object, # remove_object, set_position), which Map3D intentionally does not # implement. super().__init__(mpstate, "map3d", "3D map display") - self.map3d_settings = mp_settings.MPSettings([ - ('service', str, 'MicrosoftSat'), - ('zexag', float, 1.0), - ('debug', bool, False), - mp_settings.MPSetting('fpvfov', float, 90.0, range=(20.0, 150.0)), - mp_settings.MPSetting('terrainbrightness', float, 1.25, - range=(0.25, 2.0)), - ('terrainshading', bool, True), - ('terrainwireframe', bool, False), - ('showdirection', bool, False), - ]) + self.map3d_settings = make_settings() self.add_command('map3d', self.cmd_map3d, "3D map control", ['', 'set (MAP3DSETTING)']) @@ -71,6 +98,17 @@ def __init__(self, mpstate): self.last_attitude = (0.0, 0.0, 0.0) self.home_amsl = None self.home_position = None + # whether the vehicle is armed, and the way it last pointed while it + # was not. A fixed-wing takeoff holds the ground course the vehicle + # has once it gets moving, which is not known until it does: this + # is the best there is before then, and wrong wherever a crosswind, + # a turn on the ground or a compass error has it moving some other + # way + self.armed = False + self.ground_heading = None + self.ground_heading_changed = False + self.ground_heading_redrawn = 0 + self.reset_flown_track() self.icon_type = None self.follow = True self.kml_change_state = None @@ -104,13 +142,17 @@ def cmd_map3d(self, args): self.map.center_on_vehicle() elif cmd == "set": self.map3d_settings.command(args[1:]) + if (len(args) > 1 and args[1] == 'missionpath' and + self.map3d_settings.missionpath == 'flown'): + self.flown_asked = True if self.map is not None and self.map.is_alive(): - self.map.set_fpv_fov(self.map3d_settings.fpvfov) self.map.set_mission_arrows(self.map3d_settings.showdirection) - self.map.set_render_settings( - self.map3d_settings.terrainbrightness, - self.map3d_settings.terrainshading, - self.map3d_settings.terrainwireframe) + self.map.set_mission_labels(self.map3d_settings.showlabels) + self.map.set_mission_label_size(self.map3d_settings.labelsize) + self.map.set_mission_style(self.map3d_settings.missionpath) + # the path flown is only worked out while it is drawn + self.send_mission() + self.send_render_settings() else: print("unknown map3d command: %s" % cmd) @@ -133,12 +175,22 @@ def start_map(self): follow=self.follow) # push whatever we already know self.map.set_mission_arrows(self.map3d_settings.showdirection) + self.map.set_mission_labels(self.map3d_settings.showlabels) + self.map.set_mission_label_size(self.map3d_settings.labelsize) + self.map.set_mission_style(self.map3d_settings.missionpath) self.send_mission() self.send_fence() self.send_rally() self.send_cached_state() self.send_kml() + def send_render_settings(self): + self.map.set_fpv_fov(self.map3d_settings.fpvfov) + self.map.set_render_settings( + self.map3d_settings.terrainbrightness, + self.map3d_settings.terrainshading, + self.map3d_settings.terrainwireframe) + def stop_map(self): if self.map is not None: self.map.close() @@ -172,6 +224,10 @@ def send_cached_state(self): (attitude.Roll, attitude.Pitch, attitude.Yaw)) + origin = messages.get('GPS_GLOBAL_ORIGIN') or messages.get('ORGN') + if origin is not None: + self.mavlink_packet(origin, force=True) + home = messages.get('HOME_POSITION') if home is not None: self.home_amsl = home.altitude * 1.0e-3 @@ -277,6 +333,27 @@ def mission_home(self, wploader): return None return (home.x, home.y) + def item_amsl(self, alt, frame): + '''the AMSL altitude of a mission item, or None if we cannot tell. + + The viewer resolves the frames itself; this is only so that the climb + between two items can be measured with both of them in the one frame + ''' + if frame in (0, 5): # already AMSL + return alt + if frame in (10, 11): + # above terrain which has not been fetched yet: send_mission() + # turns these into AMSL once it has the terrain height, and + # until then home's height stands in for the ground's, which is + # nearer the mark than the height the aircraft happens to be at + if self.home_amsl is None: + return None + return self.home_amsl + alt + if self.home_amsl is None: + # relative to a home we have not been told about yet + return None + return self.home_amsl + alt + def send_mission(self): if self.map is None: return @@ -287,12 +364,32 @@ def send_mission(self): if wploader is None: return items = [] + # every item after home, for flying the mission through, and which + # of their altitudes are absolute rather than moving with home: True + # for one of its own, False for one above home, and None for an item + # whose altitude neither moves nor stays, having none to fly at + flown = [] + fixed_alt = [] default_radius = self.default_circle_radius() previous = None for w in wploader.wpoints: frame = getattr(w, 'frame', 0) (lat, lon) = (w.x, w.y) - if lat == 0 and lon == 0 and w.command in TAKEOFF_COMMANDS: + if w.seq != 0 and (lat == 0 and lon == 0 and + w.command not in mp_util.TAKEOFF_COMMANDS): + from MAVProxy.modules.lib import plane_track + amsl = plane_track.positionless_amsl(w.z, frame, + self.home_amsl) + flown.append((w.command, 0.0, 0.0, amsl, + (w.param1, w.param2, w.param3, w.param4))) + if (amsl is None or + not plane_track.is_navigation_command(w.command)): + # flown at the altitude the aircraft is at, or not + # flown at all + fixed_alt.append(None) + else: + fixed_alt.append(frame in (0, 5)) + if lat == 0 and lon == 0 and w.command in mp_util.TAKEOFF_COMMANDS: # draw the climb from home, otherwise the takeoff altitude is # dropped and the mission appears to start at the first waypoint home = self.mission_home(wploader) @@ -307,33 +404,294 @@ def send_mission(self): if terr is not None: z = terr + w.z frame = 0 + # the items are drawn in whatever frames they carry, so resolve + # the altitude before measuring anything against the item before + amsl = self.item_amsl(z, frame) + params = (w.param1, w.param2, w.param3, w.param4) circle_radius = mp_util.mission_circle_radius( - w.command, - (w.param1, w.param2, w.param3, w.param4), - default_radius, - self.vehicle_type) - circle_turns = None + w.command, params, default_radius, self.vehicle_type) + # items which say how many turns they fly say so themselves; + # None is an item which circles until something else stops it + circle_turns = mp_util.mission_circle_turns(w.command, params) if w.command == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: # this one circles until it reaches its altitude, so what is # left to climb on arrival decides how many turns to draw approach = None + alt_change = None if previous is not None: approach = mp_util.gps_distance(previous[0], previous[1], lat, lon) + if amsl is not None and previous[2] is not None: + alt_change = amsl - previous[2] circle_turns = mp_util.loiter_to_alt_turns( - circle_radius, - z - previous[2] if previous is not None else None, - self.mav_param, approach) + circle_radius, alt_change, self.mav_param, approach) exit_converge = None - if circle_radius is not None and not (w.param4 > 0): - # param4 == 0 asks for the next leg to be crosstracked from - # the loiter centre rather than from where it was left + if (circle_radius is not None and + mp_util.mission_crosstracks_from_centre(w.command, params)): + # the next leg is flown against a track from the loiter + # centre, so the vehicle pulls back onto it after leaving exit_converge = mp_util.vehicle_track_convergence(self.mav_param) items.append(MissionItem(lat, lon, z, frame, w.command, w.seq, w.param1, circle_radius, circle_turns, exit_converge)) - previous = (lat, lon, z) - self.map.set_mission(items) + if w.seq != 0: + flown.append((w.command, lat, lon, amsl, params)) + # a terrain item resolved above has frame 0 by now: its + # altitude is where it is, whatever home does + fixed_alt.append(frame in (0, 5)) + previous = (lat, lon, amsl) + self.mission_sent = items + self.map.set_mission( + items, self.flown_track(wploader, flown, fixed_alt)) + + def flight_parameters(self): + '''the vehicle's parameters which the path a mission is flown along + is worked out from, as far as they are known''' + from MAVProxy.modules.lib import plane_track + return dict((name, mp_util.param_value(self.mav_param, name)) + for (names, _) in plane_track.PARAMETERS.values() + for (name, _) in names) + + def reset_flown_track(self): + '''forget any plane mission flown. Flying one can take a second or + more, so it is done on a thread of its own rather than the main one: + the mission is drawn at once, and again with the path flown when the + thread has worked it out''' + # the parameters the mission was last drawn flown with, so a change + # to one of them -- or the first of them to arrive -- draws it again + self.flown_params = None + self.track_lock = threading.Lock() + # the last path worked out, as (key, track, home), so an unchanged + # mission is not flown again + self.plane_track = (None, None, None) + # for the thread: the latest mission to fly, as (key, home, items, + # params, heading, rally), and the thread while it is running + self.track_request = None + self.track_thread = None + # from the thread, for the idle task to draw: (key, track, home) + self.track_result = None + # (key, flown_from, home, items, alt_moves) of the mission on screen + # while it waits for its path: the home it is being flown from, the + # home and items it has now, and whether any of its altitudes move + # with home + self.track_wanted = None + self.mission_sent = None + # the mission last found to have no path flown, so it is said once + self.unflown_key = None + # whether the path flown has been asked for by name since it was + # last said it could not be drawn for this vehicle + self.flown_asked = False + # the course of a VTOL landing approach, into the wind the vehicle + # last said it estimates, to APPROACH_CHANGE; and whether the path + # on screen was flown with it + self.approach = None + self.approach_used = False + + @staticmethod + def same_home(home, other): + '''whether two homes are near enough to fly a mission from alike''' + return (other is not None and + mp_util.gps_distance(home[0], home[1], + other[0], other[1]) < HOME_MOVE_CHANGE and + abs(home[2] - other[2]) < HOME_MOVE_CHANGE) + + def flown_track(self, wploader, items, fixed_alt): + '''the path a plane flies the mission along, where it has already + been worked out; otherwise None, and the thread is asked to work it + out. None too for any other vehicle, where the mission cannot be + flown through, or where the map is not drawing the path flown''' + if self.map3d_settings.missionpath != 'flown': + self.track_wanted = None + return None + if self.vehicle_type != 'plane': + if self.flown_asked and self.vehicle_type is not None: + # asked for by name, rather than left as it starts + print("map3d: only a plane's mission can be drawn as the " + "path flown; drawing its geometry") + self.flown_asked = False + self.track_wanted = None + return None + expected = getattr(wploader, 'expected_count', None) + if expected and wploader.count() < expected: + # a download still coming in: the items so far are drawn, but + # flying them would end the mission where it has got to, with a + # return to launch or a landing the rest of it may say nothing of + self.track_wanted = None + return None + home = self.mission_home(wploader) + if home is None or self.home_amsl is None: + self.track_wanted = None + return None + from MAVProxy.modules.lib import plane_track + home = (home[0], home[1], self.home_amsl) + # copied here, since the thread must not read them as they change + params = self.flight_parameters() + + # the mission as it stands relative to home, so the same mission from + # a home which has only wandered a little is still the same mission. + # An altitude of its own stands as it is, since home does not move it + def keyed_alt(amsl, fixed): + if amsl is None or fixed is None: + return None + # which of the two it is matters as much as the number: 100m + # above home and 100m AMSL are not the same item + return (fixed, round(amsl if fixed else amsl - home[2], 2)) + relative = tuple( + (command, + 'home' if (lat, lon) == home[:2] else (lat, lon), + keyed_alt(amsl, fixed), + tuple(params_of_item)) + for ((command, lat, lon, amsl, params_of_item), fixed) + in zip(items, fixed_alt)) + points = self.rally_points(home) + rally = [point[:3] for point in points] + moves = [not fixed for fixed in fixed_alt if fixed is not None] + moves += [not fixed for (_, _, _, fixed) in points] + # where some of it moves with home and the rest does not, the path + # cannot be moved to fit a home which has: it is moved up or down + # with home all the same while home is near enough to be the same + # one, which puts what does not move out by no more than that, and + # flown again beyond + key = (relative, tuple(sorted(params.items())), self.ground_heading, + tuple((lat, lon, keyed_alt(amsl, fixed)) + for (lat, lon, amsl, fixed) in points)) + # only a mission which lands on a VTOL approach is flown again as + # the wind changes + approach = None + self.approach_used = plane_track.uses_vtol_approach(items, params) + if self.approach_used: + approach = self.approach + key += (approach,) + (cached_key, cached_track, cached_home) = self.plane_track + if cached_key == key and self.same_home(home, cached_home): + self.track_wanted = None + return self.moved_home(cached_track, cached_home, home, items, + any(moves)) + wanted = self.track_wanted + if wanted is not None and wanted[0] == key and self.same_home(home, wanted[1]): + # already being flown, from a home near enough: drawn, when it + # has been, from where home is now + self.track_wanted = (key, wanted[1], home, items, any(moves)) + return None + self.track_wanted = (key, home, home, items, any(moves)) + with self.track_lock: + self.track_request = (key, home, list(items), params, + self.ground_heading, rally, approach) + if self.track_thread is None: + self.track_thread = threading.Thread(target=self.fly_tracks, + daemon=True) + self.track_thread.start() + return None + + def rally_points(self, home): + '''the vehicle's rally points, as (lat, lon, amsl, fixed_alt), for a + return to launch to go to. fixed_alt is a point whose altitude is + its own rather than one which moves with home''' + from MAVProxy.modules.lib import plane_track + try: + rally = self.module('rally') + points = [rally.rally_point(i) + for i in range(rally.rally_count())] + except Exception: + return [] + out = [] + for r in points: + flags = getattr(r, 'flags', 0) + frame = plane_track.rally_alt_frame(flags) + (lat, lon) = (r.lat * 1.0e-7, r.lng * 1.0e-7) + terrain = None + if frame == plane_track.RALLY_ALT_ABOVE_TERRAIN: + terrain = self.terrain_alt(lat, lon) + # the EKF origin is where the vehicle first had a position, which + # is usually home and near enough to it until it says otherwise + origin = self.origin_amsl + fixed = plane_track.rally_alt_is_fixed(flags) + if frame == plane_track.RALLY_ALT_ABOVE_ORIGIN and origin is None: + (origin, fixed) = (home[2], False) + out.append((lat, lon, + plane_track.rally_amsl(r.alt, flags, home[2], + origin, terrain), + fixed)) + return out + + @staticmethod + def moved_home(track, flown_home, home, items, alt_moves=True): + '''a path flown from flown_home, moved to start from home, which is + near enough not to fly it again. Its altitudes move with home where + the mission's do, and its start moves with home too, less and less + along the way to the first item with a position of its own, which + does not move''' + if track is None or flown_home == home: + return track + dlat = home[0] - flown_home[0] + dlon = (home[1] - flown_home[1] + 180.0) % 360.0 - 180.0 + dalt = home[2] - flown_home[2] if alt_moves else 0.0 + reach = 0.0 + for (command, lat, lon, amsl, params) in items: + if (lat, lon) != (0.0, 0.0) and (lat, lon) != home[:2]: + reach = mp_util.gps_distance(flown_home[0], flown_home[1], + lat, lon) + break + moved = [] + travelled = 0.0 + previous = None + for (lat, lon, amsl) in track: + if previous is not None: + travelled += mp_util.gps_distance(previous[0], previous[1], + lat, lon) + previous = (lat, lon) + weight = max(0.0, 1.0 - travelled / reach) if reach > 0 else 0.0 + moved.append((lat + dlat * weight, lon + dlon * weight, + amsl + dalt)) + return moved + + def fly_tracks(self): + '''the thread flying plane missions: the latest asked for, until there + is none left to fly''' + from MAVProxy.modules.lib import plane_track + while True: + with self.track_lock: + request = self.track_request + self.track_request = None + if request is None: + self.track_thread = None + return + (key, home, items, params, heading, rally, approach) = request + try: + track = plane_track.mission_track(home, items, params, + heading, rally=rally, + approach=approach) + except Exception as ex: + # drawn from its items instead; the thread carries on + print("map3d: could not fly the mission: %s" % ex) + track = None + with self.track_lock: + self.track_result = (key, track, home) + + def draw_flown_track(self): + '''draw the path the thread has flown, if it is for the mission on + screen. Called from the idle task, on the main thread''' + with self.track_lock: + result = self.track_result + self.track_result = None + if result is None: + return + self.plane_track = result + (key, track, home) = result + wanted = self.track_wanted + if wanted is None or wanted[0] != key or home != wanted[1]: + # another mission since, which the thread has yet to fly + return + self.track_wanted = None + if track is None and key != self.unflown_key: + self.unflown_key = key + print("map3d: could not work out the path this mission is flown " + "along -- it may be too long, never finish an item, or use " + "a command which cannot be flown here; drawing its geometry") + if self.map is not None and self.mission_sent is not None: + self.map.set_mission(self.mission_sent, + self.moved_home(track, home, wanted[2], + wanted[3], wanted[4])) def set_icon_type(self, name): '''vehicle type changed: the viewer picks its icon from it. Not named @@ -348,10 +706,11 @@ def set_home_position(self, lat, lon): '''home moved: around-home fence circles are centred on it, and a positionless takeoff is drawn there''' if (lat, lon) == self.home_position: - return + return False self.home_position = (lat, lon) self.send_fence() self.send_mission() + return True @staticmethod def _fence_latlon(item): @@ -406,15 +765,18 @@ def send_fence(self): self.map.set_fence(shapes) def send_rally(self): + '''draw the rally points at the altitude a return to launch goes to + each at, where that can be worked out, and above home otherwise''' + if self.map is None: + return try: - rally_mod = self.module('rally') - pts = [] - for i in range(rally_mod.rallyloader.rally_count()): - r = rally_mod.rallyloader.rally_point(i) - pts.append((r.lat * 1.0e-7, r.lng * 1.0e-7, r.alt)) + rally = self.module('rally') + raw = [rally.rally_point(i) for i in range(rally.rally_count())] except Exception: return - self.map.set_rally(pts) + resolved = self.rally_points((0.0, 0.0, self.home_amsl)) + self.map.set_rally([(lat, lon, amsl, r.alt) for + ((lat, lon, amsl, _), r) in zip(resolved, raw)]) @staticmethod def _kml_state(kml_mod): @@ -459,14 +821,30 @@ def idle_task(self): for event in self.map.check_events(): if event[0] == 'startup_error': print("map3d: the 3D view failed to start:\n%s" % event[1]) + # what the view's own controls were set to. Each is sent back + # as the setting now stands: a command given here while the + # event was on its way has already been sent to the view, and + # would otherwise be overwritten there by this older event + # while the setting kept it elif event[0] == 'render_settings': (_, brightness, shading, wireframe, fpvfov) = event self.map3d_settings.terrainbrightness = brightness self.map3d_settings.terrainshading = shading self.map3d_settings.terrainwireframe = wireframe self.map3d_settings.fpvfov = fpvfov + self.send_render_settings() elif event[0] == 'follow': self.follow = bool(event[1]) + self.map.set_follow(self.follow) + elif event[0] == 'mission_labels': + self.map3d_settings.showlabels = bool(event[1]) + self.map.set_mission_labels(self.map3d_settings.showlabels) + elif event[0] == 'mission_style': + self.map3d_settings.missionpath = event[1] + self.flown_asked = event[1] == 'flown' + self.map.set_mission_style(self.map3d_settings.missionpath) + # the path flown is only worked out while it is drawn + self.send_mission() if not alive: self.map = None return @@ -489,16 +867,56 @@ def idle_task(self): except Exception: pass try: - rally_change = self.module('rally').rallyloader.last_change + rally_change = self.module('rally').last_change() if rally_change != self.rally_change_time: self.rally_change_time = rally_change self.send_rally() + # a return to launch may go to one + self.send_mission() except Exception: pass if self.terrain_resolved: # a deferred terrain lookup landed: redo the terrain-frame items self.terrain_resolved = False self.send_mission() + self.send_rally() + try: + params = self.flight_parameters() + except Exception: + params = self.flown_params + if params != self.flown_params: + # a parameter the path is flown by has changed, or arrived for + # the first time: how the vehicle flies the mission has changed + # with it + self.flown_params = params + self.send_mission() + self.redraw_for_ground_heading() + self.draw_flown_track() + + def redraw_for_ground_heading(self, now=None): + '''fly the mission again from the way the vehicle points now, if that + has changed, and not too often''' + if not self.ground_heading_changed: + return + if now is None: + now = time.time() + if now - self.ground_heading_redrawn < TAKEOFF_HEADING_REDRAW: + return + self.ground_heading_changed = False + self.ground_heading_redrawn = now + if self.vehicle_type == 'plane': + self.send_mission() + + def note_ground_heading(self, yaw): + '''the vehicle points yaw radians while disarmed. A change of less + than TAKEOFF_HEADING_CHANGE is not worth flying the mission again''' + heading = round(math.degrees(yaw)) % 360 + if self.ground_heading is not None: + change = abs((heading - self.ground_heading + 180) % 360 - 180) + if change < TAKEOFF_HEADING_CHANGE: + return + self.ground_heading = heading + self.ground_heading_changed = True def mavlink_packet(self, m, force=False): if self.map is None or not self.map.is_alive(): @@ -506,12 +924,51 @@ def mavlink_packet(self, m, force=False): mtype = m.get_type() if mtype in ('HEARTBEAT', 'HIGH_LATENCY2'): self.set_icon_type(mp_util.vehicle_type_name(m.type)) + if (mtype == 'HEARTBEAT' and + m.autopilot != mavutil.mavlink.MAV_AUTOPILOT_INVALID): + self.armed = bool(m.base_mode & + mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + elif mtype == 'GPS_GLOBAL_ORIGIN': + # the EKF origin, which a rally point's altitude may be above + origin = m.altitude * 1.0e-3 # AMSL (mm -> m) + if origin != self.origin_amsl: + self.origin_amsl = origin + self.send_rally() + self.send_mission() + elif mtype == 'ORGN': + if m.Type == 0 and m.Alt != self.origin_amsl: + self.origin_amsl = m.Alt + self.send_rally() + self.send_mission() + elif mtype == 'WIND' and math.isfinite(m.direction): + # where the wind comes from, which is the course ArduPlane + # works out for a VTOL landing approach, by the same sum. An + # EKF with no estimate sends NaN, which says nothing + approach = (round(m.direction / APPROACH_CHANGE) * + APPROACH_CHANGE) + approach = (approach + 180.0) % 360.0 - 180.0 + if approach != self.approach: + self.approach = approach + if self.approach_used: + self.send_mission() elif mtype == 'HOME_POSITION': - self.home_amsl = m.altitude * 1.0e-3 # AMSL (mm -> m) + home_amsl = m.altitude * 1.0e-3 # AMSL (mm -> m) + moved = home_amsl != self.home_amsl + if moved: + self.home_amsl = home_amsl + # rally points above home, or the origin in its place + self.send_rally() self.map.set_home(self.home_amsl) - self.set_home_position(m.latitude * 1.0e-7, m.longitude * 1.0e-7) + # a mission is flown from home, and everything above home is as + # high as home is, so it is drawn again for a home which has + # moved up or down as much as for one which has moved along + if (not self.set_home_position(m.latitude * 1.0e-7, + m.longitude * 1.0e-7) and moved): + self.send_mission() elif mtype == 'ATTITUDE': self.last_attitude = (m.roll, m.pitch, m.yaw) + if not self.armed: + self.note_ground_heading(m.yaw) elif mtype == 'ATT': # ArduPilot DataFlash attitude is recorded in degrees. self.last_attitude = tuple(math.radians(v) for v in diff --git a/MAVProxy/modules/mavproxy_map3d/elements.py b/MAVProxy/modules/mavproxy_map3d/elements.py index a431feeaa7..e9cd0eb51a 100644 --- a/MAVProxy/modules/mavproxy_map3d/elements.py +++ b/MAVProxy/modules/mavproxy_map3d/elements.py @@ -9,14 +9,10 @@ import vtk -from pymavlink import mavutil - from MAVProxy.modules.lib import mp_util -from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem -from MAVProxy.modules.mavproxy_map3d.terrain import enu, R - -# Older pymavlink releases lack this common.xml command. -MAV_CMD_NAV_ARC_WAYPOINT = getattr(mavutil.mavlink, "MAV_CMD_NAV_ARC_WAYPOINT", 36) +from MAVProxy.modules.mavproxy_map3d.map3d import ( + MissionItem, MISSION_LABEL_SIZE, MISSION_LABEL_SIZES, MISSION_STYLES) +from MAVProxy.modules.mavproxy_map3d.terrain import enu, R, wrap_longitude # MAV_FRAME altitude conventions FRAME_GLOBAL = (0, 5) # AMSL @@ -381,6 +377,20 @@ def _arrows(points_enu, colour, renderer=None, count=MISSION_ARROW_COUNT): return actor +def _label(point_enu, text, colour, size=MISSION_LABEL_SIZE): + """text at a point in the world, facing the screen and the same size on + it however far away it is""" + actor = vtk.vtkBillboardTextActor3D() + actor.SetInput(str(text)) + actor.SetPosition(*point_enu) + prop = actor.GetTextProperty() + prop.SetColor(*colour) + prop.SetFontSize(size) + prop.SetJustificationToLeft() + prop.SetVerticalJustificationToBottom() + return actor + + def _points(points_enu, colour, size): vpts = vtk.vtkPoints() verts = vtk.vtkCellArray() @@ -417,13 +427,23 @@ def __init__(self, renderer, lat0, lon0, zexag): self.vehicle_type = DEFAULT_VEHICLE_TYPE self.terrain_height = None self.fence = [] + self.rally = [] self.fence_geometry = None self.kml_features = [] self.kml_geometry = None self.kml_height_cache = {} self.mission_line = [] + self.mission_rings = [] self.mission_markers = [] + # the label of each item drawn, as (marker, label) + self.mission_labels = [] self.mission_arrows = False + self.mission_labelled = False + self.mission_label_size = MISSION_LABEL_SIZE + # the mission last given, so it can be drawn again in another style + self.mission_items = [] + self.mission_track = None + self.mission_style = MISSION_STYLES[0] def _enu(self, lat, lon, amsl): e, n, u = enu(lat, lon, amsl, self.lat0, self.lon0) @@ -446,6 +466,8 @@ def set_home(self, amsl): self.home_amsl = amsl if self.fence: self.refresh_fence() + if self.rally: + self.refresh_rally() if self.kml_features: self.refresh_kml() @@ -515,36 +537,92 @@ def add_trail_point(self, lat, lon, amsl): if len(self.trail) >= 2: self._replace('trail', [_polyline(self.trail, (1.0, 1.0, 0.0), 2.0)]) - def set_mission(self, items): - '''items: list of MissionItem (plain tuples are accepted too). - - The line drawn is the path the vehicle is expected to fly, so it is - continuous throughout: an arc waypoint curves, and an item which - circles about its location is entered from the near side of its - circle and left again where it comes off, rather than the line - running to the middle of the circle where the vehicle never goes. - The markers stay on the mission item locations + def set_mission(self, items, track=None): + '''items: list of MissionItem (plain tuples are accepted too). track + is the path the vehicle is expected to fly them along, as (lat, lon, + amsl) points, where the caller has worked it out by flying the + mission through the vehicle's own navigation. How the mission is + drawn from those is the mission style: see set_mission_style() ''' + self.mission_items = [MissionItem(*item) for item in items] + self.mission_track = list(track) if track else None + self.draw_mission() + + def set_mission_style(self, style): + '''draw the mission as one of MISSION_STYLES. flown draws the track + the mission was given with, and geometry where it came without one: + the path worked out from the items, continuous throughout, where an + arc waypoint curves and an item which circles about its location is + joined and left along tangents rather than the line running to the + middle of a circle the vehicle never goes to. plain runs the line + straight from item to item, arcs apart, with a ring at the altitude + of each item which circles''' + if style not in MISSION_STYLES or style == self.mission_style: + return + self.mission_style = style + self.draw_mission() + + def draw_mission(self): + '''draw the mission last given in the mission style. The markers stay + on the mission item locations''' + flown = [i for i in self.mission_items + if not (i.lat == 0 and i.lon == 0)] + self.mission_markers = [ + self._enu(i.lat, i.lon, self._resolve_amsl(i.alt, i.frame)) + for i in flown] + self.mission_labels = [ + (marker, mp_util.mission_item_label(item.seq, item.command)) + for (marker, item) in zip(self.mission_markers, flown)] + self.mission_rings = [] + if self.mission_style == 'flown' and self.mission_track: + self.mission_line = [self._enu(lat, lon, amsl) + for (lat, lon, amsl) in self.mission_track] + elif self.mission_style == 'plain': + self.mission_line = self._plain_line(flown) + else: + self.mission_line = self._geometry_line(flown) + self.refresh_mission() + + def _arc_into(self, line, item, amsl, previous): + '''add the leg into an arc waypoint to the line: a circular arc rather + than a straight line, climbing linearly along it''' + if (item.command != mp_util.MAV_CMD_NAV_ARC_WAYPOINT or + previous is None): + return + ((prev_lat, prev_lon), prev_amsl) = previous + arc = mp_util.arc_points((prev_lat, prev_lon), + (item.lat, item.lon), item.param1) + for i in range(1, len(arc) - 1): + fraction = float(i) / (len(arc) - 1) + line.append(self._enu(arc[i][0], arc[i][1], + prev_amsl + (amsl - prev_amsl) * fraction)) + + def _plain_line(self, flown): + '''the line straight through the items, and a ring for each which + circles, at its own altitude rather than draped over the terrain''' + line = [] + previous = None + for item in flown: + amsl = self._resolve_amsl(item.alt, item.frame) + self._arc_into(line, item, amsl, previous) + line.append(self._enu(item.lat, item.lon, amsl)) + if item.circle_radius: + ring = circle_latlon((item.lat, item.lon), + abs(item.circle_radius), + MISSION_CIRCLE_SEGMENTS) + ring = [self._enu(la, lo, amsl) for (la, lo) in ring] + self.mission_rings.append(ring + ring[:1]) + previous = ((item.lat, item.lon), amsl) + return line + + def _geometry_line(self, flown): + '''the path worked out from the items alone''' line = [] - markers = [] previous = None rejoin = None - flown = [MissionItem(*item) for item in items] - flown = [i for i in flown if not (i.lat == 0 and i.lon == 0)] for (index, item) in enumerate(flown): amsl = self._resolve_amsl(item.alt, item.frame) - if (item.command == MAV_CMD_NAV_ARC_WAYPOINT and - previous is not None): - # the leg into an arc waypoint is a circular arc rather - # than a straight line; climb linearly along it - ((prev_lat, prev_lon), prev_amsl) = previous - arc = mp_util.arc_points((prev_lat, prev_lon), - (item.lat, item.lon), item.param1) - for i in range(1, len(arc) - 1): - fraction = float(i) / (len(arc) - 1) - line.append(self._enu(arc[i][0], arc[i][1], - prev_amsl + (amsl - prev_amsl) * fraction)) - markers.append(self._enu(item.lat, item.lon, amsl)) + self._arc_into(line, item, amsl, previous) if item.circle_radius: previous = self._append_circle( line, item, amsl, previous, @@ -562,9 +640,7 @@ def set_mission(self, items): rejoin = None line.append(self._enu(item.lat, item.lon, amsl)) previous = ((item.lat, item.lon), amsl) - self.mission_line = line - self.mission_markers = markers - self.refresh_mission() + return line def set_mission_arrows(self, enable): '''show or hide the direction of travel along the mission''' @@ -574,6 +650,24 @@ def set_mission_arrows(self, enable): self.mission_arrows = enable self.refresh_mission() + def set_mission_labels(self, enable): + '''show or hide the label of each mission item''' + enable = bool(enable) + if enable == self.mission_labelled: + return + self.mission_labelled = enable + self.refresh_mission() + + def set_mission_label_size(self, size): + '''how big those labels are drawn, in points on the screen''' + (smallest, largest) = MISSION_LABEL_SIZES + size = min(largest, max(smallest, int(size))) + if size == self.mission_label_size: + return + self.mission_label_size = size + if self.mission_labelled: + self.refresh_mission() + def refresh_mission(self): '''rebuild the mission actors from the last mission drawn''' line = self.mission_line @@ -584,8 +678,14 @@ def refresh_mission(self): arrows = _arrows(line, (1.0, 1.0, 1.0), self.ren) if arrows is not None: actors.append(arrows) + for ring in self.mission_rings: + actors.append(_polyline(ring, (1.0, 1.0, 1.0), 2.0, dashed=True)) if self.mission_markers: actors.append(_points(self.mission_markers, (1.0, 1.0, 1.0), 9)) + if self.mission_labelled: + actors += [_label(marker, text, (1.0, 1.0, 1.0), + self.mission_label_size) + for (marker, text) in self.mission_labels] self._replace('mission', actors) def _append_rejoin(self, line, rejoin, target, target_amsl): @@ -708,6 +808,8 @@ def _terrain_samples(self, points, closed): for i in range(max(0, segment_count)): lat1, lon1 = points[i] lat2, lon2 = points[(i + 1) % len(points)] + # along the edge the short way round, as it is drawn + dlon = wrap_longitude(lon2 - lon1) e1, n1, _ = enu(lat1, lon1, 0.0, self.lat0, self.lon0) e2, n2, _ = enu(lat2, lon2, 0.0, self.lat0, self.lon0) distance = math.hypot(e2 - e1, n2 - n1) @@ -716,7 +818,7 @@ def _terrain_samples(self, points, closed): for j in range(count): t = float(j) / count yield (lat1 + (lat2 - lat1) * t, - lon1 + (lon2 - lon1) * t) + lon1 + dlon * t) if points: yield points[0] if closed else points[-1] @@ -806,11 +908,20 @@ def set_kml(self, features, terrain_height=None, refresh=True): self.refresh_kml() def set_rally(self, pts): - '''pts: list of (lat,lon,alt_rel)''' - if not pts: + '''pts: list of (lat, lon, amsl, alt): the AMSL altitude a return to + launch goes to the point at, or None where that is not known, and + the altitude the point carries, which is taken to be above home + where there is nothing better''' + self.rally = list(pts) + self.refresh_rally() + + def refresh_rally(self): + if not self.rally: self._replace('rally', []) return - markers = [self._enu(lat, lon, self.home_amsl + alt) for (lat, lon, alt) in pts] + markers = [self._enu(lat, lon, + self.home_amsl + alt if amsl is None else amsl) + for (lat, lon, amsl, alt) in self.rally] self._replace('rally', [_points(markers, (0.4, 0.8, 1.0), 12)]) def refresh_vehicle(self): diff --git a/MAVProxy/modules/mavproxy_map3d/map3d.py b/MAVProxy/modules/mavproxy_map3d/map3d.py index f5b4c587b8..5f5b185527 100644 --- a/MAVProxy/modules/mavproxy_map3d/map3d.py +++ b/MAVProxy/modules/mavproxy_map3d/map3d.py @@ -24,6 +24,18 @@ 'exit_converge', defaults=(0.0, None, None, None)) +# how the mission line is drawn. flown is the path the vehicle is worked +# out to fly, where the caller could work one out, and geometry where it +# could not; geometry joins and leaves circles on tangents and walks spirals +# round them; plain runs straight from item to item, with a ring at each +# item which circles +MISSION_STYLES = ('flown', 'geometry', 'plain') + +# how big a mission item's label is drawn, in points on the screen, and the +# sizes worth asking for: too small to read through to filling the view +MISSION_LABEL_SIZE = 14 +MISSION_LABEL_SIZES = (6, 48) + PACKAGES = ('vtk', 'quantized_mesh_tile') @@ -128,8 +140,12 @@ def set_time_range(self, trange): '''Limit timestamped path points to a Matplotlib date-number range.''' self._put(('time_range', None if trange is None else tuple(trange))) - def set_mission(self, items): - self._put(('mission', list(items))) + def set_mission(self, items, track=None): + '''items: MissionItems. track is the path the vehicle is expected to + fly them along, as (lat, lon, amsl) points, where the caller can + work that out; otherwise the path is drawn from the items alone''' + self._put(('mission', list(items), + None if track is None else list(track))) def set_fence(self, shapes): '''shapes: ('polygon', [(lat,lon), ...], rgb) / ('circle', (lat,lon), radius_m, rgb)''' @@ -147,6 +163,18 @@ def set_follow(self, enable): def set_mission_arrows(self, enable): self._put(('mission_arrows', bool(enable))) + def set_mission_labels(self, enable): + '''label each mission item with its number and what it is''' + self._put(('mission_labels', bool(enable))) + + def set_mission_label_size(self, size): + '''how big those labels are drawn, in points on the screen''' + self._put(('mission_label_size', int(size))) + + def set_mission_style(self, style): + '''one of MISSION_STYLES''' + self._put(('mission_style', str(style))) + def set_fpv_fov(self, fov): self._put(('fpvfov', float(fov))) diff --git a/MAVProxy/modules/mavproxy_map3d/map3d_ui.py b/MAVProxy/modules/mavproxy_map3d/map3d_ui.py index bb145470cf..21ac24f261 100644 --- a/MAVProxy/modules/mavproxy_map3d/map3d_ui.py +++ b/MAVProxy/modules/mavproxy_map3d/map3d_ui.py @@ -14,8 +14,10 @@ from MAVProxy.modules.mavproxy_map import mp_tile from MAVProxy.modules.mavproxy_map3d.camera import TerrainCamera, TerrainStyle from MAVProxy.modules.mavproxy_map3d.terrain import ( - TerrainManager, R, TILE_DOWNLOAD_THREADS) + TerrainManager, R, TILE_DOWNLOAD_THREADS, wrap_longitude) from MAVProxy.modules.mavproxy_map3d.elements import ElementManager +from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_STYLES) import vtk @@ -97,6 +99,13 @@ def __init__(self, state): self.overlay_mesh_revision = -1 self.fpv_enabled = False self.fpv_fov = min(150.0, max(20.0, float(state.fpvfov))) + # how the mission is drawn, as the element manager's own defaults. + # The parent pushes what it wants, which may be before there is a + # scene to draw it in, so the frame holds it for init_scene + self.mission_arrows = False + self.mission_labelled = False + self.mission_label_size = MISSION_LABEL_SIZE + self.mission_style = MISSION_STYLES[0] self.terrain_brightness = min( 2.0, max(0.25, float(state.terrain_brightness))) self.terrain_shading = bool(state.terrain_shading) @@ -128,6 +137,22 @@ def __init__(self, state): self.fpv_button.SetToolTip("Toggle level-horizon first-person view") self.Bind(wx.EVT_TOGGLEBUTTON, self.on_fpv_toggle, self.fpv_button) controls.Add(self.fpv_button, 0, wx.ALL, 4) + self.labels_check = wx.CheckBox(self, -1, "Labels") + self.labels_check.SetValue(self.mission_labelled) + self.labels_check.SetToolTip( + "Number each mission item, as the 2D map does") + self.Bind(wx.EVT_CHECKBOX, self.on_labels_toggle, self.labels_check) + controls.Add(self.labels_check, 0, + wx.ALL | wx.ALIGN_CENTER_VERTICAL, 4) + controls.Add(wx.StaticText(self, -1, "Mission"), 0, + wx.ALL | wx.ALIGN_CENTER_VERTICAL, 4) + self.style_choice = wx.Choice(self, -1, choices=list(MISSION_STYLES)) + self.style_choice.SetStringSelection(self.mission_style) + self.style_choice.SetToolTip( + "Draw the mission as the path the vehicle flies, as the geometry " + "of its items, or as plain lines from item to item") + self.Bind(wx.EVT_CHOICE, self.on_style_choice, self.style_choice) + controls.Add(self.style_choice, 0, wx.ALL, 4) gear = wx.ArtProvider.GetBitmap(wx.ART_HELP_SETTINGS, wx.ART_BUTTON, (16, 16)) self.settings_button = wx.BitmapButton(self, -1, gear) @@ -158,6 +183,10 @@ def init_scene(self, lat0, lon0, amsl_ref): shading=self.terrain_shading, wireframe=self.terrain_wireframe) self.elements = ElementManager(self.ren, lat0, lon0, self.state.zexag) + self.elements.set_mission_arrows(self.mission_arrows) + self.elements.set_mission_label_size(self.mission_label_size) + self.elements.set_mission_labels(self.mission_labelled) + self.elements.set_mission_style(self.mission_style) if self.vehicle_type is not None: self.elements.set_vehicle_type(self.vehicle_type) self.elements.set_terrain_height(self.terrain.height_at) @@ -189,6 +218,47 @@ def on_fpv_toggle(self, event): def on_follow_toggle(self, event): self.set_follow_enabled(self.follow_button.GetValue(), notify=True) + def on_labels_toggle(self, event): + self.set_mission_labels(self.labels_check.GetValue(), notify=True) + + def on_style_choice(self, event): + self.set_mission_style(self.style_choice.GetStringSelection(), + notify=True) + + def set_mission_arrows(self, enable): + self.mission_arrows = bool(enable) + if self.elements is not None: + self.elements.set_mission_arrows(self.mission_arrows) + + def set_mission_labels(self, enable, notify=False): + self.mission_labelled = bool(enable) + self.labels_check.SetValue(self.mission_labelled) + if self.elements is not None: + self.elements.set_mission_labels(self.mission_labelled) + self.render() + if notify: + self.state.event_queue.put( + ('mission_labels', self.mission_labelled)) + + def set_mission_label_size(self, size): + self.mission_label_size = int(size) + if self.elements is not None: + self.elements.set_mission_label_size(self.mission_label_size) + self.render() + + def set_mission_style(self, style, notify=False): + if style not in MISSION_STYLES: + return + self.mission_style = style + self.style_choice.SetStringSelection(self.mission_style) + if self.elements is not None: + self.elements.set_mission_style(self.mission_style) + self.render() + if notify: + # the parent draws the path flown only while it is asked for, so + # it may send the mission again rather than just take this + self.state.event_queue.put(('mission_style', self.mission_style)) + def schedule_kml_refresh(self): if self.elements is None or not self.kml_features: self.kml_refresh_due = None @@ -319,7 +389,8 @@ def render(self): self.widget.GetRenderWindow().Render() def look_at_latlon(self, lat, lon, amsl, dist=None): - e = math.radians(lon - self.terrain.lon0) * R * math.cos(math.radians(self.terrain.lat0)) + e = (math.radians(wrap_longitude(lon - self.terrain.lon0)) * R * + math.cos(math.radians(self.terrain.lat0))) n = math.radians(lat - self.terrain.lat0) * R u = amsl * self.state.zexag self.tc.look_at((e, n, u), dist=dist) @@ -334,6 +405,11 @@ def handle(self, msg): if kind == 'follow': self.set_follow_enabled(msg[1]) return + if kind == 'render_settings': + (_, brightness, shading, wireframe) = msg + self.apply_render_settings((brightness, shading, wireframe, + self.fpv_fov)) + return if kind == 'vehicletype': # may arrive before the scene exists, so keep it for init_scene self.vehicle_type = msg[1] @@ -341,6 +417,20 @@ def handle(self, msg): self.elements.set_vehicle_type(self.vehicle_type) self.render() return + # how the mission is drawn may arrive before there is a scene to + # draw it in, so the frame keeps it for init_scene + if kind == 'mission_arrows': + self.set_mission_arrows(msg[1]) + return + if kind == 'mission_labels': + self.set_mission_labels(msg[1]) + return + if kind == 'mission_label_size': + self.set_mission_label_size(msg[1]) + return + if kind == 'mission_style': + self.set_mission_style(msg[1]) + return if kind == 'kml': self.kml_features = list(msg[1]) if self.elements is not None: @@ -370,9 +460,7 @@ def handle(self, msg): elif kind == 'time_range': self.elements.set_time_range(msg[1]) elif kind == 'mission': - self.elements.set_mission(msg[1]) - elif kind == 'mission_arrows': - self.elements.set_mission_arrows(msg[1]) + self.elements.set_mission(msg[1], msg[2] if len(msg) > 2 else None) elif kind == 'fence': self.elements.set_fence(msg[1], self.terrain.height_at) elif kind == 'rally': diff --git a/MAVProxy/modules/mavproxy_map3d/terrain.py b/MAVProxy/modules/mavproxy_map3d/terrain.py index ca973af7d2..5faea5813f 100644 --- a/MAVProxy/modules/mavproxy_map3d/terrain.py +++ b/MAVProxy/modules/mavproxy_map3d/terrain.py @@ -11,6 +11,7 @@ import os import queue import threading +import time import urllib.request import warnings @@ -43,8 +44,15 @@ module=r"quantized_mesh_tile\.terrain") +def wrap_longitude(lon): + '''a longitude, or a difference of two, as the one from -180 up to 180 + it is. Differences wrapped take the short way round, so that either side + of the antimeridian is next to the other rather than the world apart''' + return (lon + 180.0) % 360.0 - 180.0 + + def enu(lat, lon, h, lat0, lon0): - e = math.radians(lon - lon0) * R * math.cos(math.radians(lat0)) + e = math.radians(wrap_longitude(lon - lon0)) * R * math.cos(math.radians(lat0)) n = math.radians(lat - lat0) * R return e, n, h @@ -65,13 +73,30 @@ def np_rgb_to_texture(img): return tex -def fetch_terrain_tile(z, x, y): +def fetch_terrain_tile(z, x, y, timeout=30): path = os.path.join(CACHE_DIR, str(z), str(x), "%d.terrain" % y) if not os.path.exists(path): os.makedirs(os.path.dirname(path), exist_ok=True) url = "%s/%d/%d/%d.terrain" % (QUANTIZED_BASE, z, x, y) req = urllib.request.Request(url, headers={"Accept": "application/octet-stream"}) - data = urllib.request.urlopen(req, timeout=30).read() + # a socket timeout is per read, so a tile dripping in a few bytes at + # a time would hold the caller for as long as it liked: give the + # whole fetch the deadline the caller asked for + deadline = time.time() + timeout + response = urllib.request.urlopen(req, timeout=timeout) + # read() waits to fill its buffer, however long the bytes take to + # come; read1() hands back what has arrived + read = getattr(response, 'read1', response.read) + chunks = [] + while True: + chunk = read(64 * 1024) + if not chunk: + break + chunks.append(chunk) + if time.time() > deadline: + raise TimeoutError("terrain tile took longer than %gs" + % timeout) + data = b"".join(chunks) # publish atomically under a unique name: the viewer child process and # the module share this cache, so a reader must never see a partial tile tmppath = "%s.tmp.%u.%u" % (path, os.getpid(), threading.get_ident()) @@ -83,11 +108,11 @@ def fetch_terrain_tile(z, x, y): return path, gz -def decode_terrain(z, x, y): +def decode_terrain(z, x, y, timeout=30): '''worker-safe: return dict(bbox, verts, idx) using numpy only''' g = GlobalGeodetic(True) bbox = g.TileBounds(x, y, z) - path, gz = fetch_terrain_tile(z, x, y) + path, gz = fetch_terrain_tile(z, x, y, timeout) tile = qmt_decode(path, bbox, gzipped=gz) verts = np.array(tile.getVerticesCoordinates()) nv = len(verts) @@ -99,15 +124,17 @@ def decode_terrain(z, x, y): _sample_cache = {} -def sample_terrain(lat, lon, zoom=12, cache_only=False): +def sample_terrain(lat, lon, zoom=12, cache_only=False, timeout=30): '''return terrain elevation (m AMSL) at lat/lon from the quantized mesh (same source we render), or None. Decoded tiles are cached. Assumes the regular grid mesh ArduPilot publishes; falls back to the tile mean for a non-grid tile. cache_only returns None rather than fetching/decoding a missing tile, so - callers on a latency-sensitive thread can defer the work.''' + callers on a latency-sensitive thread can defer the work; timeout is how + long a caller which does fetch waits for the whole tile.''' g = GlobalGeodetic(True) + lon = wrap_longitude(lon) x, y = g.LonLatToTile(lon, lat, zoom) key = (zoom, x, y) dec = _sample_cache.get(key) @@ -115,7 +142,7 @@ def sample_terrain(lat, lon, zoom=12, cache_only=False): if cache_only: return None try: - dec = decode_terrain(zoom, x, y) + dec = decode_terrain(zoom, x, y, timeout) except Exception: return None _sample_cache[key] = dec @@ -358,7 +385,8 @@ def _worker(self): def focal_latlon(self, tc): foclat = self.lat0 + math.degrees(tc.focal[1] / R) - foclon = self.lon0 + math.degrees(tc.focal[0] / (R * math.cos(math.radians(self.lat0)))) + foclon = wrap_longitude(self.lon0 + math.degrees( + tc.focal[0] / (R * math.cos(math.radians(self.lat0))))) return foclat, foclon def desired_set(self, foclat, foclon): @@ -380,6 +408,10 @@ def desired_set(self, foclat, foclon): def height_at(self, lat, lon): '''Return rendered terrain world Z at lat/lon, preferring fine tiles.''' e, n, _ = enu(lat, lon, 0.0, self.lat0, self.lon0) + lon = wrap_longitude(lon) + if lon == -180.0: + # the antimeridian is either tile's edge; 180 is the one tiles have + lon = 180.0 for key, tile in sorted(self.tiles.items(), reverse=True): west, south, east, north = tile.bbox if not (west <= lon <= east and south <= lat <= north): @@ -401,7 +433,7 @@ def tile_priority(key): clat = 0.5 * (south + north) clon = 0.5 * (west + east) dlat = clat - foclat - dlon = (clon - foclon) * math.cos(math.radians(foclat)) + dlon = wrap_longitude(clon - foclon) * math.cos(math.radians(foclat)) return (-z, dlat * dlat + dlon * dlon) for (z, x, y) in sorted(want, key=tile_priority): diff --git a/MAVProxy/tools/MAVExplorer.py b/MAVProxy/tools/MAVExplorer.py index e89e814e7d..f1542b8bee 100755 --- a/MAVProxy/tools/MAVExplorer.py +++ b/MAVProxy/tools/MAVExplorer.py @@ -13,6 +13,7 @@ import threading import shlex import traceback +from types import SimpleNamespace from math import * os.environ['MAVLINK20'] = '1' @@ -23,6 +24,7 @@ from MAVProxy.modules.lib import wxconsole from MAVProxy.modules.lib import param_help from MAVProxy.modules.lib import param_ftp +from MAVProxy.modules.lib import log_mission from MAVProxy.modules.lib.graph_ui import Graph_UI from pymavlink.mavextra import * from MAVProxy.modules.lib.mp_menu import * @@ -31,6 +33,8 @@ from pymavlink import mavwp from pymavlink import DFReader from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting +from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_LABEL_SIZES, MISSION_STYLES) from MAVProxy.modules.lib import wxsettings from MAVProxy.modules.lib.graphdefinition import GraphDefinition from lxml import objectify @@ -120,8 +124,16 @@ def __init__(self): MPSetting('paramdocs', bool, True, 'show param docs'), MPSetting('max_rate', float, 0, 'maximum display rate of graphs in Hz'), MPSetting('vehicle_type', str, 'Auto', 'force vehicle type for mode handling'), - MPSetting('showdirection', bool, False, + MPSetting('showdirection', bool, True, 'show direction of travel on the 3D map mission'), + MPSetting('showlabels', bool, False, + 'label the mission items on the 3D map'), + MPSetting('labelsize', int, MISSION_LABEL_SIZE, + 'size of the 3D map mission labels, in points', + range=MISSION_LABEL_SIZES), + MPSetting('missionpath', str, MISSION_STYLES[0], + 'draw the 3D map mission as ' + ', '.join(MISSION_STYLES), + choice=list(MISSION_STYLES)), ] ) @@ -618,6 +630,8 @@ def cmd_graphs(args): grui[-1].set_xlim(xlimits.last_xlim) map_timelim_pipes = [] +# the settings the open 3D views were last given +map3d_pushed = None def cmd_map(args): '''map command''' @@ -661,49 +675,601 @@ def cmd_map(args): map3d_views = [] -def cmd_map3d(args): - '''show a 3D map view: draped satellite imagery over terrain''' - try: - from MAVProxy.modules.mavproxy_map3d.map3d import ( - Map3D, missing_packages, missing_packages_message) - except ImportError as ex: - print("map3d needs extra packages: pip install 'MAVProxy[map3d]' (%s)" % ex) - return - # map3d.py itself does not import VTK; the viewer child process does - missing = missing_packages() - if missing: - print(missing_packages_message(missing)) - return +# the mission and rally protocol's messages, which a telemetry log carries +# as the ground station and the vehicle sent them +MISSION_PROTOCOL = ('MISSION_COUNT', 'MISSION_ITEM', 'MISSION_ITEM_INT', + 'MISSION_CLEAR_ALL') + + +def rally_points(rally_log): + '''a LogMission's rally table as mission_from_log() gives it, by + sequence: where each point is, and the flags AP_Rally keeps the frame + of its altitude in''' + out = {} + for (seq, item) in rally_log.whole().items(): + if item.command == mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT: + out[seq] = (item.x, item.y, item.z, + rally_flags_for_frame(item.frame)) + return out - mlog = mestate.mlog - path = [] + +# how far apart two of a mission's items can be and still be the one item. +# AP_Mission keeps a position in 1e-7 degrees, but turns a float +# MISSION_ITEM's into that in float32, which is up to 6.4e-6 degrees out at +# longitudes past 128; and it gives a float MISSION_ITEM back by turning the +# integer into float32 and then multiplying it in float32, which is out by +# up to 1.6e-5 degrees +SAME_POSITION = 2.0e-5 # degrees, about 2m +SAME_ALTITUDE = 0.1 # metres + +# the commands AP_Mission keeps a position for (stored_in_location()); the +# rest go back with no position, and frame 0, whatever they came in with +LOCATION_COMMANDS = frozenset(( + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, + mavutil.mavlink.MAV_CMD_NAV_LAND, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, + mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT, + mavutil.mavlink.MAV_CMD_NAV_GUIDED_ENABLE, + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + mavutil.mavlink.MAV_CMD_DO_RETURN_PATH_START, + mavutil.mavlink.MAV_CMD_DO_LAND_START, + mavutil.mavlink.MAV_CMD_DO_GO_AROUND, + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + mavutil.mavlink.MAV_CMD_DO_SET_ROI, + mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, + mavutil.mavlink.MAV_CMD_NAV_PAYLOAD_PLACE, + mp_util.MAV_CMD_NAV_ARC_WAYPOINT, +)) + +# the frame AP_Mission gives an item with a position back in: it keeps only +# whether the altitude is above home or terrain, and never says _INT. An +# item uploaded in MISSION, the frame for one with no position, is kept as +# absolute (AP_Mission::mavlink_int_to_mission_cmd) +RETURNED_FRAME = { + mavutil.mavlink.MAV_FRAME_MISSION: mavutil.mavlink.MAV_FRAME_GLOBAL, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT: mavutil.mavlink.MAV_FRAME_GLOBAL, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, +} + + +def kept_whole(value): + '''a parameter as AP_Mission keeps it in an integer''' + return int(value) if isfinite(value) else 0 + + +def kept_signed(value): + '''a radius or an angle AP_Mission keeps as a whole number and a + direction''' + return copysign(kept_whole(abs(value)), value) + + +def kept_float(value): + '''a parameter AP_Mission keeps as it came, where NaN is NaN again''' + return value if isfinite(value) else None + + +def kept_turns(value): + '''the turns of NAV_LOITER_TURNS: fewer than one in 256ths, and at most + 255 of them''' + if 0 < value < 1: + return kept_whole(value * 256) / 256.0 + return min(255, max(0, kept_whole(value))) + + +def kept_loiter_radius(value): + '''the radius of NAV_LOITER_TURNS: past 255m it is kept in tens of + metres, and at most 2550m''' + radius = abs(value) + if radius > 255: + radius = min(255, kept_whole(radius * 0.1)) * 10 + return copysign(kept_whole(radius), value) + + +def kept_ccw(value): + return value < 0 + + +def kept_xtrack(value): + return value > 0 + + +# the parameters of an item AP_Mission sends back as they were sent to it, +# every vehicle alike, as it keeps each of them. Another parameter goes +# back as 0, or packed up, or only on some vehicles -- NAV_WAYPOINT's are +# its delay on a copter and its acceptance radii on a plane -- so is no sign +# of another mission +RETURNED_PARAMS = { + mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM: + (None, None, kept_signed, None), + mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS: + (kept_turns, None, kept_loiter_radius, kept_xtrack), + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME: + (kept_whole, None, kept_ccw, kept_xtrack), + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + (None, kept_signed, None, kept_xtrack), + mp_util.MAV_CMD_NAV_ARC_WAYPOINT: + (kept_signed, None, None, None), + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: + (kept_whole, None, None, None), + mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: + (kept_whole, None, None, None), + mavutil.mavlink.MAV_CMD_CONDITION_DELAY: + (kept_float, None, None, None), + mavutil.mavlink.MAV_CMD_DO_JUMP: + (kept_whole, kept_whole, None, None), + mavutil.mavlink.MAV_CMD_DO_JUMP_TAG: + (kept_whole, kept_whole, None, None), + mavutil.mavlink.MAV_CMD_JUMP_TAG: + (kept_whole, None, None, None), + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED: + (kept_whole, kept_float, kept_float, None), +} + + +def returned_item(item): + '''a mission item as AP_Mission sends it back: what of it would come + back from the vehicle whichever way it was sent there''' + (lat, lon, alt, frame, command, _, params) = item + kept = RETURNED_PARAMS.get(command, (None,) * 4) + params = tuple(k(p) for (k, p) in zip(kept, params) if k is not None) + if command not in LOCATION_COMMANDS: + return (command, None, params, None) + return (command, RETURNED_FRAME.get(frame, frame), params, (lat, lon, alt)) + + +def same_mission(before, after): + '''whether a mission a log has just seen is the one it held, rather than + another. Item 0 is left out of it: that is home, the vehicle's own and + not the mission's -- an uploaded one is never stored + (AP_Mission::replace_cmd) and a downloaded one is wherever home is now, + which the vehicle keeps setting while it is disarmed -- so a mission + downloaded to see what the vehicle holds differs there from the upload + of it every time. The rest are compared as AP_Mission keeps them, since + a download gives back only that''' + if sorted(before) != sorted(after): + return False + for seq in before: + if seq == 0: + continue + (command, frame, params, where) = returned_item(before[seq]) + (was_command, was_frame, was_params, was_where) = \ + returned_item(after[seq]) + if (command, frame, params) != (was_command, was_frame, was_params): + return False + if where is None: + continue + (lat, lon, alt) = where + (was_lat, was_lon, was_alt) = was_where + if (abs(lat - was_lat) > SAME_POSITION or + abs(lon - was_lon) > SAME_POSITION or + abs(alt - was_alt) > SAME_ALTITUDE): + return False + return True + + +class RallyPoints(list): + '''a log's rally points in order, and whether the table they are of + arrived whole: the points of one a log ends part way through are points + to draw, and not a table to work a return out from, since the one it + would go to may be among those which never arrived''' + whole = True + + +class MissionCmds(dict): + '''a log's CMD entries keyed by sequence, which is how this reads a + mission, and whether the mission they are arrived whole: the items of one + a log ends part way through are a mission to draw and not one to fly''' + whole = True + + +def mission_cmds(mission_log): + '''a LogMission's mission as a log's CMD entries keyed by sequence''' + out = MissionCmds() + out.whole = not mission_log.partial() + for (seq, item) in mission_log.whole().items(): + out[seq] = (item.x, item.y, item.z, item.frame, item.command, seq, + (item.param1, item.param2, item.param3, item.param4)) + return out + + +def mission_items_from_cmds(items, started_at=None, flown_from=None): + '''the mission items to draw, from a log's CMD entries keyed by sequence. + + An item with no position of its own is a takeoff, which climbs from + wherever the vehicle is when it runs. flown_from maps an item's + sequence to where the vehicle was when the log says the item began, and + that is used when there is one. Otherwise the best guess is home, the + mission's own first item, since ArduPilot sets home where it arms; and a + mission uploaded before the vehicle had a home leaves that item empty + too, so started_at -- where the flight itself began -- stands in for it. + Anything else without a position is not something the map can place, + and is left out + ''' + flown_from = flown_from or {} + home = items.get(0) + if home is not None and (home[0] != 0 or home[1] != 0): + started_at = (home[0], home[1]) mission = [] + for seq in sorted(items): + item = items[seq] + if item[0] == 0 and item[1] == 0: + if item[4] not in mp_util.TAKEOFF_COMMANDS: + continue + where = flown_from.get(seq, started_at) + if where is None: + continue + item = (where[0], where[1]) + item[2:] + mission.append(item) + return mission + + +# what ArduPlane says as it picks the course of a VTOL landing approach +VTOL_APPROACH_MESSAGE = 'Selected an approach path of ' + +# a rally point's altitude frame, as its flags carry it (Location::AltFrame), +# for each MAVLink frame a rally item in the mission protocol can be in +RALLY_FRAME_FLAGS = { + mavutil.mavlink.MAV_FRAME_GLOBAL: 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT: 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT: 1, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: 1, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT: 3, + mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: 3, +} + + +def rally_flags_for_frame(frame): + '''the flags of a rally point in frame, as AP_Rally keeps them: the + frame, and the bit which says the flags carry one''' + return (1 << 2) | (RALLY_FRAME_FLAGS.get(frame, 1) << 3) + + +def mission_from_log(mlog, condition=None): + '''the flight path in a log, the mission the log ends with, that + mission's CMD entries keyed by sequence -- every one of them, where the + mission drawn has only the ones with a position -- by sequence, the + index into the path at which the log says each of its items began, the + rally points the log ends with, as (lat, lon, alt, flags), the EKF + origin, which a rally point's altitude may be measured from, the + course of the last VTOL landing approach the vehicle announced, or None. + + A log carries the whole mission again every time one is uploaded, and + once more when it starts, so the items are kept by sequence number and + the logger's "New mission" -- written before every one of them, even a + mission with no items at all -- starts them again. Drawing every mission + the flight ever held would join them into one line. + + A telemetry log carries the same things as the MAVLink messages the + vehicle sent: its position, home and origin, what it said, and the + mission and rally points as they were downloaded or uploaded while the + log was kept. Only a mission which arrives whole replaces the one held, + so a transfer the log ends part way through leaves the mission before + it, and a log which starts part way through one holds no mission at all; + the rally points which arrive are the table, since what the old protocol + set before them is gone whether or not the new table ever arrives whole, + and they say so + ''' + path = [] + # the mission and the rally points, as whatever kind of log this is + # carries them: a table which never arrives whole says nothing about the + # one the vehicle holds, so the one before it stands + mission_log = log_mission.LogMission() + rally_log = log_mission.LogMission( + mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + items = {} + # where the vehicle was when each item began, from the "Mission: " + # the autopilot announces as it starts one, and how far along the path + flown_from = {} + started = {} + # home, and the EKF origin, as the vehicle last logged them + home = None + origin = None + # the rally points, by sequence. The logger writes the whole table out + # when one is uploaded, and a single point when just that one is set -- + # appending one is logged as that one alone, with the new total -- so + # they are kept by sequence, and only the ones a smaller total leaves + # out are dropped + rally = {} + # whether the points held are the whole of the table they are of + rally_whole = True + approach = None + # the vehicle, in a telemetry log: its first heartbeat says which it is + vehicle = None while True: - m = mlog.recv_match(type=['POS', 'CMD'], condition=mestate.settings.condition) + m = mlog.recv_match(type=['POS', 'CMD', 'MSG', 'ORGN', 'RALY', + 'HEARTBEAT', 'GLOBAL_POSITION_INT', + 'HOME_POSITION', 'GPS_GLOBAL_ORIGIN', + 'STATUSTEXT', 'MISSION_COUNT', + 'MISSION_ITEM', 'MISSION_ITEM_INT', + 'MISSION_CLEAR_ALL', 'RALLY_POINT'], + condition=condition) if m is None: break mtype = m.get_type() + if mtype == 'HEARTBEAT': + if (vehicle is None and + m.autopilot != mavutil.mavlink.MAV_AUTOPILOT_INVALID and + m.type != mavutil.mavlink.MAV_TYPE_GCS): + vehicle = m.get_srcSystem() + continue + if mtype in MISSION_PROTOCOL: + # downloaded from the vehicle, or uploaded to it + if vehicle not in (m.get_srcSystem(), m.target_system): + continue + elif (mtype in ('GLOBAL_POSITION_INT', 'HOME_POSITION', + 'GPS_GLOBAL_ORIGIN', 'STATUSTEXT', 'RALLY_POINT') and + m.get_srcSystem() != vehicle): + continue + if mtype == 'GLOBAL_POSITION_INT': + if m.lat == 0 and m.lon == 0: + continue + m = SimpleNamespace(Lat=m.lat * 1.0e-7, Lng=m.lon * 1.0e-7, + Alt=m.alt * 0.001, _timestamp=m._timestamp) + mtype = 'POS' + elif mtype == 'HOME_POSITION': + m = SimpleNamespace(Type=1, Lat=m.latitude * 1.0e-7, + Lng=m.longitude * 1.0e-7, + Alt=m.altitude * 0.001) + mtype = 'ORGN' + elif mtype == 'GPS_GLOBAL_ORIGIN': + m = SimpleNamespace(Type=0, Lat=m.latitude * 1.0e-7, + Lng=m.longitude * 1.0e-7, + Alt=m.altitude * 0.001) + mtype = 'ORGN' + elif mtype == 'STATUSTEXT': + m = SimpleNamespace(Message=m.text) + mtype = 'MSG' + elif mtype == 'RALLY_POINT': + # the old rally protocol's, which carry AP_Rally's own flags + m = SimpleNamespace(Seq=m.idx, Tot=m.count, Lat=m.lat * 1.0e-7, + Lng=m.lng * 1.0e-7, Alt=m.alt, Flags=m.flags) + mtype = 'RALY' + elif mtype in MISSION_PROTOCOL: + # a rally table downloaded or uploaded, whose points are carried + # as a mission's items are. One which arrives whole is what the + # vehicle holds, even where the table was that already: the + # points the old protocol set may not be there any more + before = rally_points(rally_log) + arrivals = rally_log.arrivals + rally_log.read(m, mtype) + after = rally_points(rally_log) + if after != before or rally_log.arrivals != arrivals: + rally = after + if mtype in ('MISSION_COUNT', 'MISSION_CLEAR_ALL') or \ + after != before: + # what arrived is what is known of the table the vehicle + # holds, and all that is known: the points the old protocol + # set before it are gone. A download which says how many to + # expect and ends before any of them arrive leaves none of + # it, which is not the table either + rally_whole = not rally_log.partial() + if mtype in log_mission.MISSION_TYPES: + # the mission itself, which LogMission keeps: only the last one + # to arrive whole, so a download which stops part way through + # leaves the one before it as it was + before = mission_cmds(mission_log) + mission_log.read(m, mtype) + if not same_mission(before, mission_cmds(mission_log)): + # a mission other than the one flown so far: where the + # flight had got to in that one says nothing about this + flown_from = {} + started = {} + if mtype != 'MSG': + continue if mtype == 'POS': path.append((m.Lat, m.Lng, m.Alt, grapher.timestamp_to_days(m._timestamp))) - elif mtype == 'CMD' and (m.Lat != 0 or m.Lng != 0): - params = tuple(getattr(m, 'Prm%u' % i, 0.0) for i in range(1, 5)) - mission.append((m.Lat, m.Lng, m.Alt, getattr(m, 'Frame', 3), - m.CId, m.CNum, params)) - mlog.rewind() + elif mtype == 'RALY': + if any(seq >= m.Tot for seq in rally): + rally = dict((seq, point) for (seq, point) in rally.items() + if seq < m.Tot) + if m.Seq < m.Tot: + # RALY only carries the altitude frame from 4.5; before that + # a rally point's altitude is above home, which is flags 0 + rally[m.Seq] = (m.Lat, m.Lng, m.Alt, getattr(m, 'Flags', 0)) + # the logger writes the whole table out, so the points held are + # the table once there are as many of them as it says + rally_whole = len(rally) == m.Tot + elif mtype == 'ORGN': + # AP_AHRS: type 0 is the EKF origin, type 1 home + if m.Type == 1 and (m.Lat != 0 or m.Lng != 0): + home = (m.Lat, m.Lng, m.Alt) + elif m.Type == 0 and (m.Lat != 0 or m.Lng != 0): + origin = (m.Lat, m.Lng, m.Alt) + elif mtype == 'MSG': + fields = m.Message.split() + if m.Message.startswith(VTOL_APPROACH_MESSAGE): + # the course the vehicle took into the wind it had then + try: + approach = float(m.Message[len(VTOL_APPROACH_MESSAGE):]) + except ValueError: + pass + elif m.Message == 'New rally': + # written before the whole table, even an empty one: a + # cleared table is this and no points at all, which is the + # whole of it + rally = {} + rally_whole = True + elif (len(fields) > 1 and fields[0] == 'Mission:' and + fields[1].isdigit() and path): + flown_from.setdefault(int(fields[1]), (path[-1][0], path[-1][1])) + started.setdefault(int(fields[1]), len(path) - 1) + items = mission_cmds(mission_log) + if (home is not None and 0 in items and + items[0][0] == 0 and items[0][1] == 0): + # a mission uploaded before the vehicle had a home carries an empty + # one, which everything relative to home would be measured from + items[0] = (home[0], home[1], home[2], 0) + items[0][4:] + mission = mission_items_from_cmds( + items, started_at=(path[0][0], path[0][1]) if path else None, + flown_from=flown_from) + points = RallyPoints(rally[seq] for seq in sorted(rally)) + points.whole = rally_whole + return (path, mission, items, started, points, origin, approach) + + +def takeoff_flown(path, items, started): + '''the takeoff of the mission the log shows flown, as its CMD entry, and + the course in degrees it was flown on; (None, None) where the log shows + none. ArduPlane holds the ground course it has once it gets moving, and + a QuadPlane transitions the way it was pointing, neither of which the + mission says, so the course is taken from the path: where the aircraft + was when the takeoff began, towards where it was once it had gone a + little way''' + for (seq, index) in sorted(started.items()): + item = items.get(seq) + if item is None or item[4] not in ( + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF): + continue + (lat, lon) = path[index][:2] + for point in path[index + 1:]: + if mp_util.gps_distance(lat, lon, point[0], point[1]) >= 30.0: + return (item, mp_util.gps_bearing(lat, lon, point[0], + point[1])) + return (None, None) - if len(path) == 0: - print("No POS messages found for 3D map") - return +def takeoff_course(path, items, started): + '''the course in degrees the mission's takeoff was flown on, or None''' + return takeoff_flown(path, items, started)[1] + + +# a rally point is one point of one view: wait only so long for the terrain +# under it, rather than the whole fetch the tiles drawn are worth waiting for +RALLY_TERRAIN_TIMEOUT = 5.0 + + +def rally_point_amsl(lat, lon, alt, flags, home_amsl, origin=None): + '''the AMSL altitude of a rally point from a log. A point above the + terrain is resolved through the same quantized mesh the 3D view draws; + one above the EKF origin falls back to home, which the origin is + usually near, where the log does not say where the origin was''' + from MAVProxy.modules.lib import plane_track + frame = plane_track.rally_alt_frame(flags) + terrain = None + if frame == plane_track.RALLY_ALT_ABOVE_TERRAIN: + try: + from MAVProxy.modules.mavproxy_map3d.terrain import sample_terrain + terrain = sample_terrain(lat, lon, timeout=RALLY_TERRAIN_TIMEOUT) + except Exception as ex: + print("map3d: terrain elevation unavailable (%s)" % ex) + origin_amsl = origin[2] if origin else None + if frame == plane_track.RALLY_ALT_ABOVE_ORIGIN and origin_amsl is None: + origin_amsl = home_amsl + return plane_track.rally_amsl(alt, flags, home_amsl, origin_amsl, terrain) + + +def plane_mission_track(cmds, mission, started_at, params=None, mav_type=None, + path=None, started=None, rally=None, origin=None, + approach=None): + '''the path a plane flies a log's mission along, as (lat, lon, amsl) + points, or None for a vehicle which is not a plane or a mission which + cannot be flown through. + + cmds are the mission's CMD entries keyed by sequence, and mission the + MissionItems resolve_mission_amsl() made of them, which carry the + positions and AMSL altitudes worked out for the ones drawn. The flight + starts from home, or from started_at where the log's mission has no home + of its own. path and started are the flight path and where along it + each item began, as mission_from_log() gives them: where the log shows + the mission's first item beginning, the flight starts there instead, + and a takeoff is flown as the course measured over the start of it says: + a fixed-wing one on that course, and a VTOL one transitioning onto it. + rally is the log's rally points, which a return to launch may go to, and + origin the EKF origin one of them may be measured from. approach is the + course of a VTOL landing approach, which the vehicle flies into the wind + it has then: the one the log says it took, where it says so, and + otherwise that of still air, since the wind at some other time says + little about it + ''' + if mp_util.vehicle_type_name(mav_type) != 'plane': + return None + if sorted(cmds) != list(range(len(cmds))): + # an item is missing from the log. The flight is numbered by + # sequence -- jumps go to one, and where each began is kept by one -- + # so there is no flying it with a gap, and no guessing what was there + return None + if not getattr(cmds, 'whole', True): + # the log ends part way through the only transfer of this mission: + # flying what arrived would end it with a return to launch, or a + # landing, which whatever was still to come may say nothing of + return None + if not getattr(rally, 'whole', True): + # nor is a mission flown whose return may go to a rally point among + # those the log ends part way through the table of + return None + from MAVProxy.modules.lib import plane_track + placed = {item.seq: item for item in mission} + home = placed.get(0) + if home is not None: + home = (home.lat, home.lon, home.alt) + elif started_at is not None: + home = tuple(started_at) + items = [] + for seq in sorted(cmds): + if seq == 0: + continue + (lat, lon, alt, frame, command, _, prm) = cmds[seq] + item = placed.get(seq) + if item is not None: + items.append((command, item.lat, item.lon, item.alt, prm)) + else: + items.append((command, 0.0, 0.0, + plane_track.positionless_amsl( + alt, frame, home[2] if home else None), + prm)) + heading = None + course = None + start = None + if path and started: + # the course measured over the start of the takeoff, which is the way + # a QuadPlane pointed as it transitioned, and for a fixed-wing + # takeoff the course it held: nothing but a flight of one says either + (takeoff, measured) = takeoff_flown(path, cmds, started) + if takeoff is not None: + if plane_track.takeoff_is_vtol(takeoff[4], params or {}): + heading = measured + else: + course = measured + first = plane_track.first_navigation_item(items) + if first in started: + (lat, lon, alt) = path[started[first]][:3] + start = (lat, lon, alt) + rally_points = [(lat, lon, rally_point_amsl(lat, lon, alt, flags, + home[2] if home else None, + origin)) + for (lat, lon, alt, flags) in (rally or [])] + return plane_track.mission_track(home, items, params, heading, start, + rally_points, approach, + takeoff_course=course) + + +def path_view(path): + '''where a 3D map of a flight path looks from: the middle of the path, + its lowest altitude and how far across it is, as (lat, lon, amsl, span). + Longitudes are taken from the first point's the short way round, so a + flight across the antimeridian is centred on it rather than on the far + side of the world''' # note: sum/min/max are shadowed in this namespace (mavextra import *), so # accumulate explicitly + first_lon = path[0][1] sumlat = sumlon = 0.0 minlat = maxlat = path[0][0] - minlon = maxlon = path[0][1] + minlon = maxlon = 0.0 ground0 = path[0][2] for p in path: la, lo, al = p[:3] + lo = mp_util.wrap_180(lo - first_lon) sumlat += la sumlon += lo if al < ground0: @@ -713,19 +1279,62 @@ def cmd_map3d(args): minlon = lo if lo < minlon else minlon maxlon = lo if lo > maxlon else maxlon lat0 = sumlat / len(path) - lon0 = sumlon / len(path) - span_ns = mp_util.gps_distance(minlat, minlon, maxlat, minlon) - span_ew = mp_util.gps_distance(minlat, minlon, minlat, maxlon) + lon0 = mp_util.wrap_180(first_lon + sumlon / len(path)) + span_ns = mp_util.gps_distance(minlat, 0.0, maxlat, 0.0) + span_ew = mp_util.gps_distance(minlat, 0.0, minlat, maxlon - minlon) span = span_ns if span_ns > span_ew else span_ew if span < 1000.0: span = 1000.0 + return (lat0, lon0, ground0, span) + + +def cmd_map3d(args): + '''show a 3D map view: draped satellite imagery over terrain''' + try: + from MAVProxy.modules.mavproxy_map3d.map3d import ( + Map3D, missing_packages, missing_packages_message) + except ImportError as ex: + print("map3d needs extra packages: pip install 'MAVProxy[map3d]' (%s)" % ex) + return + # map3d.py itself does not import VTK; the viewer child process does + missing = missing_packages() + if missing: + print(missing_packages_message(missing)) + return + + mlog = mestate.mlog + (path, mission, cmds, started, rally, origin, + approach) = mission_from_log(mlog, mestate.settings.condition) + mlog.rewind() + + if len(path) == 0: + print("No POS or GLOBAL_POSITION_INT messages found for 3D map") + return + + (lat0, lon0, ground0, span) = path_view(path) # resolve mission item altitudes to AMSL before sending. Terrain-frame # waypoints (MAV_FRAME_GLOBAL_TERRAIN_ALT = 10/11) are "z above terrain", so # they need the terrain elevation at the waypoint, not home + z. + track = None if mission: - mission = resolve_mission_amsl(mission, ground0, mlog.params, - getattr(mlog, 'mav_type', None)) + params = mp_util.log_params(mlog) + mav_type = getattr(mlog, 'mav_type', None) + mission = resolve_mission_amsl(mission, ground0, params, mav_type) + + # a plane does not fly the lines between its items; where the log + # says enough to fly the mission through its navigation, that path + # is drawn, and worked out only when it is + def fly(): + return plane_mission_track(cmds, mission, + (path[0][0], path[0][1], ground0), + params, mav_type, path, started, + rally, origin, approach) + if mestate.settings.missionpath == 'flown': + track = fly() + if (track is None and + mp_util.vehicle_type_name(mav_type) == 'plane'): + say_unflown() # drop views the user has already closed, so their child processes are reaped for old in [v for v in map3d_views if not v.is_alive()]: @@ -737,11 +1346,18 @@ def cmd_map3d(args): m3d.set_origin(lat0, lon0, ground0) m3d.set_home(ground0) m3d.set_mission_arrows(mestate.settings.showdirection) + m3d.set_mission_labels(mestate.settings.showlabels) + m3d.set_mission_label_size(mestate.settings.labelsize) + m3d.set_mission_style(mestate.settings.missionpath) m3d.set_path(path) if xlimits.last_xlim is not None and mestate.settings.sync_xmap: m3d.set_time_range(xlimits.last_xlim) if mission: - m3d.set_mission(mission) + m3d.set_mission(mission, track) + # kept, to draw the path flown if the style changes to it later + m3d.mission_to_fly = None + if track is None and mestate.settings.missionpath != 'flown': + m3d.mission_to_fly = (mission, fly) m3d.look_at(lat0, lon0, ground0, dist=1.6 * span) def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): @@ -778,7 +1394,9 @@ def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): amsl = home_amsl + z radius = mp_util.mission_circle_radius(cid, prm, default_radius, mav_type) - turns = None + # items which say how many turns they fly say so themselves; None is + # an item which circles until something else stops it + turns = mp_util.mission_circle_turns(cid, prm) if cid == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT: approach = None if previous is not None: @@ -788,9 +1406,10 @@ def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): amsl - previous[2] if previous is not None else None, params, approach) converge = None - if radius is not None and not (prm[3] > 0): - # param4 == 0 asks for the next leg to be crosstracked from the - # loiter centre rather than from where it was left + if (radius is not None and + mp_util.mission_crosstracks_from_centre(cid, prm)): + # the next leg is flown against a track from the loiter centre, + # so the vehicle pulls back onto it after leaving the circle converge = mp_util.vehicle_track_convergence(params) out.append(MissionItem(la, lo, amsl, 0, cid, seq, prm[0], radius, turns, converge)) @@ -800,10 +1419,60 @@ def resolve_mission_amsl(mission, ground0, params=None, mav_type=None): def cmd_set(args): '''control MAVExporer options''' mestate.settings.command(args) - # settings the open 3D views care about + update_map3d_views() + +def update_map3d_views(): + '''push the settings the open 3D views care about''' + global map3d_pushed + map3d_pushed = map3d_settings() for view in map3d_views: if view.is_alive(): view.set_mission_arrows(mestate.settings.showdirection) + view.set_mission_labels(mestate.settings.showlabels) + view.set_mission_label_size(mestate.settings.labelsize) + view.set_mission_style(mestate.settings.missionpath) + pending = getattr(view, 'mission_to_fly', None) + if mestate.settings.missionpath == 'flown' and pending is not None: + (mission, fly) = pending + view.mission_to_fly = None + track = fly() + if track is None: + say_unflown() + view.set_mission(mission, track) + +def say_unflown(): + print("map3d: could not work out the path this mission is flown along " + "-- only a plane's can be, and not one which is too long, never " + "finishes an item, uses a command which cannot be flown here, or " + "which the log does not hold all of; drawing its geometry") + +def map3d_settings(): + '''the settings the open 3D views are drawn by''' + return (mestate.settings.showdirection, mestate.settings.showlabels, + mestate.settings.labelsize, mestate.settings.missionpath) + + +def poll_map3d_views(): + '''take what the open 3D views' own controls have been set to, and give + them any setting changed since they were last told -- by the settings + dialog as much as by a set command. A view which has died is drained + too, so we do not lose the reason it failed to start''' + global map3d_pushed + changed = map3d_settings() != map3d_pushed + for view in map3d_views: + for event in view.check_events(): + if event[0] == 'startup_error': + print("map3d: the 3D view failed to start:\n%s" % event[1]) + elif event[0] == 'mission_labels': + mestate.settings.showlabels = bool(event[1]) + changed = True + elif event[0] == 'mission_style': + mestate.settings.missionpath = event[1] + changed = True + if changed: + # every view follows the setting, and the path flown is only worked + # out while it is drawn + update_map3d_views() def cmd_condition(args): '''control MAVExporer conditions''' @@ -2028,6 +2697,7 @@ def main_loop(): cmds = line.split(';') for c in cmds: process_stdin(c) + poll_map3d_views() remlist = [] for i in range(0, len(grui)): diff --git a/MAVProxy/tools/mavflightview.py b/MAVProxy/tools/mavflightview.py index f164e37461..c5f942aa78 100755 --- a/MAVProxy/tools/mavflightview.py +++ b/MAVProxy/tools/mavflightview.py @@ -24,6 +24,7 @@ from MAVProxy.modules.lib import multiproc from MAVProxy.modules.lib import grapher from MAVProxy.modules.lib import kmlread +from MAVProxy.modules.lib.log_mission import LogMission, MISSION_TYPES def create_map(title): @@ -202,8 +203,30 @@ def colourmap_for_mav_type(mav_type): return map +def drawable_mission(wploader): + '''wploader, or a copy of it pymavlink can draw. A DO_JUMP to an item + which is not there -- or to home -- ends the mission in ArduPilot, but + MAVWPLoader follows it and fails on the missing item. In the copy such + a jump points at itself, which ends the line drawn there instead''' + import copy + count = wploader.count() + bad = [i for i in range(count) + if wploader.wp(i).command == mavutil.mavlink.MAV_CMD_DO_JUMP and + not 0 < int(wploader.wp(i).param1) < count] + if not bad: + return wploader + ret = mavwp.MAVWPLoader() + for i in range(count): + w = copy.copy(wploader.wp(i)) + if i in bad: + w.param1 = i + ret.add(w) + return ret + + def display_waypoints(wploader, map, default_radius=None, mav_type=None): '''display the waypoints''' + wploader = drawable_mission(wploader) mission_list = wploader.view_list() polygons = wploader.polygon_list() map.add_object(mp_slipmap.SlipClearLayer('Mission')) @@ -389,10 +412,6 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): wp = mavwp.MAVWPLoader() if options.mission is not None: wp.load(options.mission) - # the radius a loiter item without one of its own will be flown at, from - # the parameters the log carries - options.default_circle_radius = mp_util.param_value( - getattr(mlog, 'params', None), 'WP_LOITER_RAD') fen = mavwp.MAVFenceLoader() if options.fence is not None: fen.load(options.fence) @@ -423,8 +442,9 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): if options.ahr2: expressions.extend(pos_expressions(['AHR2', 'AHRS2', 'GPS'])) - # find the union of message types we need from the log for all expressions - recv_match_types = set() + # find the union of message types we need from the log for all + # expressions, and the ones the mission is read from + recv_match_types = set(MISSION_TYPES) for e in expressions: recv_match_types.update(set(e.recv_match_types)) @@ -443,6 +463,7 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): mlog.rewind() + mission = LogMission() while True: try: m = mlog.recv_match(type=recv_match_types) @@ -453,60 +474,10 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): type = m.get_type() - if type in ['MISSION_ITEM', 'MISSION_ITEM_INT']: - try: - new_m = m - if type == 'MISSION_ITEM_INT': - # create a MISSION_ITEM from MISSION_ITEM_INT - new_m = mavutil.mavlink.MAVLink_mission_item_message( - 0, - 0, - m.seq, - m.frame, - m.command, - m.current, - m.autocontinue, - m.param1, - m.param2, - m.param3, - m.param4, - m.x / 1.0e7, - m.y / 1.0e7, - m.z - ) - while new_m.seq > wp.count(): - print("Adding dummy WP %u" % wp.count()) - wp.set(new_m, wp.count()) - wp.set(new_m, m.seq) - except Exception as e: - print("Exception: %s" % str(e)) - pass - continue - elif type == 'CMD': + if type in MISSION_TYPES: + # a mission given on the command line is drawn instead if options.mission is None: - m = mavutil.mavlink.MAVLink_mission_item_message( - 0, - 0, - m.CNum, - mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, - m.CId, - 0, # current - 1, # autocontinue - m.Prm1, - m.Prm2, - m.Prm3, - m.Prm4, - m.Lat, - m.Lng, - m.Alt - ) - try: - while m.seq > wp.count(): - print("Adding dummy WP %u" % wp.count()) - wp.set(m, wp.count()) - wp.set(m, m.seq) - except Exception: - pass + mission.read(m) continue if not mlog.check_condition(options.condition): @@ -573,6 +544,14 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): path = paths2 + mission.fill(wp) + + # the radius a loiter item without one of its own will be flown at, from + # the parameters the log carries. A telemetry log only has those once it + # has been read through + options.default_circle_radius = mp_util.param_value( + mp_util.log_params(mlog), 'WP_LOITER_RAD') + if len(path) == 0: print("No points to plot") return None @@ -580,6 +559,33 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]): return [path, wp, fen, used_flightmodes, getattr(mlog, 'mav_type', None), instances] +def mission_objects(wp, options, mav_type, title): + '''the map objects the mission in wp is drawn with: its legs and arcs, + and the circles of its loiters. None if there is nothing to draw''' + if not options.show_waypoints: + return None + wp = drawable_mission(wp) + plist = wp.polygon_list() + vlist = wp.view_list() + if len(plist) == 0: + return None + mission_obj = [] + for i in range(len(plist)): + mission_obj.append(mp_slipmap.SlipPolygon( + 'Mission-%s-%u' % (title, i), + plist[i], + layer='Mission', + linewidth=2, + colour=(255, 255, 255), + arcs=mp_slipmap.mission_arcs(wp, vlist[i]), + )) + mission_obj.extend(mp_slipmap.mission_circles( + 'Loiter-%s' % title, 'Mission', wp, vlist[i], plist[i], + default_radius=getattr(options, 'default_circle_radius', None), + vehicle=mav_type)) + return mission_obj + + def mavflightview_show(path, wp, fen, @@ -634,29 +640,7 @@ def mavflightview_show(path, linewidth=2, showlines=(not getattr(options, "no_show_lines", False)), colour=(255, 0, 180))) - plist = [] - vlist = [] - if options.show_waypoints: - plist = wp.polygon_list() - vlist = wp.view_list() - mission_obj = None - if len(plist) > 0: - mission_obj = [] - for i in range(len(plist)): - mission_obj.append(mp_slipmap.SlipPolygon( - 'Mission-%s-%u' % (title, i), - plist[i], - layer='Mission', - linewidth=2, - colour=(255, 255, 255), - arcs=mp_slipmap.mission_arcs(wp, vlist[i]), - )) - mission_obj.extend(mp_slipmap.mission_circles( - 'Loiter-%s' % title, 'Mission', wp, vlist[i], plist[i], - default_radius=getattr(options, 'default_circle_radius', None), - vehicle=mav_type)) - else: - mission_obj = None + mission_obj = mission_objects(wp, options, mav_type, title) if len(fence) > 1: fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence', diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000000..48d768232a --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,164 @@ +'''shared test setup''' + +import os + +import pytest + + +@pytest.fixture(autouse=True) +def restore_mavlink20(): + """put MAVLINK20 back as it was after each test. MAVExplorer.py sets it + when it is loaded, and pymavlink's mission loader reads it to choose the + messages it builds, but the dialect pymavlink has already loaded does not + change with it: tests which run after one loading MAVExplorer.py would + otherwise build messages the loaded dialect cannot take""" + before = os.environ.get('MAVLINK20') + yield + if before is None: + os.environ.pop('MAVLINK20', None) + else: + os.environ['MAVLINK20'] = before + + +class Widget(object): + '''as much of a wx control as the 3D viewer's frame uses''' + + def __init__(self, value): + self.value = value + + def SetValue(self, value): + self.value = value + + def GetValue(self): + return self.value + + def SetStringSelection(self, value): + self.value = value + + def GetStringSelection(self): + return self.value + + def Enable(self, enable=True): + pass + + +class Terrain(object): + '''as much of the 3D viewer's terrain manager as the frame uses, with + nothing fetched''' + mesh_revision = 0 + tiles = [] + + def __init__(self, *args, **kwargs): + self.render_settings = (kwargs.get('brightness'), + kwargs.get('shading'), + kwargs.get('wireframe')) + + def height_at(self, lat, lon): + return 0.0 + + def set_render_settings(self, brightness, shading, wireframe): + self.render_settings = (brightness, shading, wireframe) + + def update(self, camera): + pass + + +@pytest.fixture +def map3d_frame(monkeypatch): + """build the 3D viewer's frame without wx: its real message handling, + controls and scene building, with the widgets, the renderer's window and + the terrain stood in for. The camera dialog's tests build their wx + object the same way. + + elements is the ElementManager to draw into, or None for a frame which + has no scene yet. state is what the frame was started with -- a Map3D, + to connect it to one -- and otherwise events collects what the frame + tells its parent""" + def build(elements=None, events=None, state=None): + pytest.importorskip("wx") + pytest.importorskip("vtk") + import vtk + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d_ui + from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_STYLES) + # a scene is built with the real element manager and camera, over + # terrain and imagery which are never fetched + monkeypatch.setattr(map3d_ui, 'TerrainManager', Terrain) + monkeypatch.setattr(map3d_ui.mp_tile, 'MPTile', + lambda **kwargs: None) + if state is None: + state = SimpleNamespace(event_queue=SimpleNamespace( + put=(events if events is not None else []).append)) + for (name, value) in (('service', None), ('zexag', 1.0), + ('height', 800)): + if not hasattr(state, name): + setattr(state, name, value) + frame = map3d_ui.Map3DFrame.__new__(map3d_ui.Map3DFrame) + frame.state = state + frame.terrain = None if elements is None else Terrain() + frame.elements = elements + frame.ren = vtk.vtkRenderer() + frame.widget = SimpleNamespace(SetInteractorStyle=lambda style: None) + frame.status_actor = SimpleNamespace(SetInput=lambda text: None) + frame.GetClientSize = lambda: (1100, 800) + frame.render = lambda: None + frame.tc = None + frame.follow = False + frame.fpv_enabled = False + frame.fpv_fov = 90.0 + frame.terrain_brightness = 1.25 + frame.terrain_shading = True + frame.terrain_wireframe = False + frame.saved_map_view = None + frame.vehicle_type = None + frame.kml_features = [] + frame.kml_refresh_due = None + frame.overlay_mesh_revision = -1 + frame.mission_arrows = False + frame.mission_labelled = False + frame.mission_label_size = MISSION_LABEL_SIZE + frame.mission_style = MISSION_STYLES[0] + frame.labels_check = Widget(frame.mission_labelled) + frame.style_choice = Widget(frame.mission_style) + frame.follow_button = Widget(frame.follow) + return frame + return build + + +@pytest.fixture +def map3d_module(): + """build the live map3d module with the settings it makes, around the + map it is given: there is no other module for it to ask anything of, + and no mission to fly""" + def build(map=None): + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_map3d + module = mavproxy_map3d.Map3DModule.__new__( + mavproxy_map3d.Map3DModule) + module.map3d_settings = mavproxy_map3d.make_settings() + module.map = map + module.follow = True + module.send_mission = lambda: None + module.mpstate = SimpleNamespace(module=lambda name: None, + mav_param={}) + module.kml_change_state = None + module.terrain_resolved = False + module.ground_heading_changed = False + module.reset_flown_track() + return module + return build + + +@pytest.fixture +def mavexplorer(): + '''MAVExplorer, which MAVProxy/tools is not a package to import''' + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module diff --git a/tests/missions/README.md b/tests/missions/README.md new file mode 100644 index 0000000000..4b324dbdc1 --- /dev/null +++ b/tests/missions/README.md @@ -0,0 +1,59 @@ +# missions for checking the map drawing + +Missions built to put every kind of item the maps draw specially in front +of the drawing code at once, so a SITL flight can be compared against what +was drawn: the live 2D and 3D maps while it flies, and mavflightview or +MAVExplorer's `map3d` over the dataflash log afterwards. + +Each item carries a comment saying what it is there for. They are laid out +around SITL's default CMAC home, `-35.363261 149.165230`, and every one of +them can be flown by the vehicle it is named for. + + wp load tests/missions/plane-mission-geometry.txt + +## plane-mission-geometry.txt + +Sixteen items covering the loiter drawing: + +- a loiter to altitude climbing 400m over a short approach, drawn as a + spiral of about two turns, and one descending 200m, drawn as about three +- three turns counter-clockwise and two clockwise, close enough together + that the leg between them is the tangent which crosses between the two + circles, and a loiter for time after them turning the same way, whose leg + is the tangent which does not cross +- a loiter with no radius of its own, which takes `WP_LOITER_RAD` +- param4 both ways: 0 leaves the next leg crosstracked from the loiter + centre, so the drawing pulls the vehicle back onto that track after it + leaves the circle; 1 crosstracks from the exit, and no pull-back is drawn +- AMSL, home-relative and terrain-frame altitudes, and five changes of + frame between one item and the next +- a loiter forever at the end. The mission parks there: switch out of AUTO + once there is nothing left to watch + +ArduPlane has no `NAV_ARC_WAYPOINT`, so the arc waypoints are in the copter +mission instead. Nothing here flies `MAV_CMD_DO_ORBIT` either: the maps +draw its circle, but no ArduPilot vehicle implements the command, so it +cannot go in a mission which is meant to be flown. + +## copter-mission-geometry.txt + +Thirteen items over a few hundred metres, covering what a multirotor draws +differently: + +- arc waypoints, one sweeping 90 degrees clockwise and one 270 + counter-clockwise. The second bulges a long way outside its chord, which + is what the 2D map has to bound it over to keep it on screen +- loiter turns both ways round, near enough that the leg between them is + the tangent which crosses between the two circles +- the loiters a multirotor holds position for instead of circling -- + unlimited, for time, and to altitude -- which are drawn as the points + they are even where the item carries a radius +- turns with no radius of its own: a multirotor has no `WP_LOITER_RAD` to + fall back on, so nothing is drawn (the vehicle flies its + `CIRCLE_RADIUS_M`, or `CIRCLE_RADIUS` in centimetres on older firmware) +- a takeoff carrying no position, drawn as the climb from where it was + flown +- a multirotor has no L1 controller, so nothing is drawn pulling it back + onto a track out of a loiter centre + +As with the plane mission, it parks at a loiter forever at the end. diff --git a/tests/missions/copter-mission-geometry.txt b/tests/missions/copter-mission-geometry.txt new file mode 100644 index 0000000000..a8c1a93aa5 --- /dev/null +++ b/tests/missions/copter-mission-geometry.txt @@ -0,0 +1,27 @@ +QGC WPL 110 +# a copter mission for the map's arcs, loiters and spirals +0 1 0 16 0 0 0 0 -35.3632610 149.1652300 584.000000 1 +# straight up. This one carries no position of its own, which the map draws as the climb from where it is flown +1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.0000000 0.0000000 30.000000 1 +# a plain leg, to give the first arc something to leave from +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3619135 149.1652300 30.000000 1 +# arc waypoint, 90 degrees clockwise (positive sweep) +3 0 3 36 90.000000 0.000000 0.000000 0.000000 -35.3619135 149.1674331 30.000000 1 +# arc waypoint sweeping more than half a turn counter-clockwise: the arc bulges a long way outside its chord +4 0 3 36 -270.000000 0.000000 0.000000 0.000000 -35.3635015 149.1693804 40.000000 1 +# two turns, 50m clockwise. A multirotor does fly this one as a circle, so the map draws it +5 0 3 18 2.000000 0.000000 50.000000 0.000000 -35.3654072 149.1670436 40.000000 1 +# one turn the other way round, near enough that the leg between the two circles is the tangent which crosses between them +6 0 3 18 1.000000 0.000000 -50.000000 1.000000 -35.3638191 149.1650962 40.000000 1 +# turns with no radius of its own. A multirotor has no WP_LOITER_RAD to fall back on, so there is no circle to draw: the item is drawn as the point it is (the vehicle flies its CIRCLE_RADIUS_M, or CIRCLE_RADIUS in centimetres on older firmware) +7 0 3 18 1.000000 0.000000 0.000000 0.000000 -35.3621676 149.1671214 40.000000 1 +# a multirotor holds position for LOITER_TIME, so no circle is drawn here either. It carries no radius: the autopilot has nowhere to keep one for this command, and hands back 0 whatever is uploaded +8 0 3 19 20.000000 0.000000 0.000000 0.000000 -35.3628435 149.1693987 40.000000 1 +# a multirotor climbs straight up for LOITER_TO_ALT rather than spiralling, so again no circle +9 0 3 31 0.000000 40.000000 0.000000 0.000000 -35.3649995 149.1693987 70.000000 1 +# an AMSL altitude, to mix the frames +10 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.3656754 149.1671213 654.000000 1 +# a terrain-frame altitude, 50m above the ground +11 0 10 16 0.000000 0.000000 0.000000 0.000000 -35.3642991 149.1657052 50.000000 1 +# hold here forever. A multirotor sits on the point, so no circle. The mission parks here: switch out of AUTO when done +12 0 3 17 0.000000 0.000000 40.000000 0.000000 -35.3627797 149.1663833 50.000000 1 diff --git a/tests/missions/kalaupapa-canyon-run.json b/tests/missions/kalaupapa-canyon-run.json new file mode 100644 index 0000000000..1c7354b0c1 --- /dev/null +++ b/tests/missions/kalaupapa-canyon-run.json @@ -0,0 +1 @@ +{"home":[21.165642,-156.8868012,165.31],"items":[[84,21.1656423,-156.8868016,215.31,[0.0,0.0,0.0,0.0]],[31,21.1773205,-156.8829482,600.0,[0.0,200.0,0.0,0.0]],[31,21.1719305,-156.8800583,180.0,[0.0,-150.0,0.0,0.0]],[178,0.0,0.0,0.0,[0.0,22.0,-1.0,0.0]],[16,21.1602524,-156.8810218,200.0,[0.0,0.0,0.0,0.0]],[16,21.1575574,-156.8810219,200.0,[0.0,0.0,0.0,0.0]],[16,21.1548624,-156.8790954,200.0,[0.0,0.0,0.0,0.0]],[16,21.1521675,-156.8776506,220.0,[0.0,0.0,0.0,0.0]],[16,21.1494725,-156.8757242,230.0,[0.0,0.0,0.0,0.0]],[16,21.1467776,-156.8757243,250.0,[0.0,0.0,0.0,0.0]],[16,21.1440826,-156.8776508,280.0,[0.0,0.0,0.0,0.0]],[16,21.1413876,-156.8762061,300.0,[0.0,0.0,0.0,0.0]],[16,21.135,-156.8768,420.0,[0.0,0.0,0.0,0.0]],[16,21.1285,-156.8735,555.0,[0.0,0.0,0.0,0.0]],[16,21.124,-156.8715,645.0,[0.0,0.0,0.0,0.0]],[31,21.118,-156.871,1120.0,[0.0,150.0,0.0,0.0]],[178,0.0,0.0,0.0,[0.0,25.0,-1.0,0.0]],[16,21.1175,-156.86,1120.0,[0.0,0.0,0.0,0.0]],[16,21.119,-156.847,900.0,[0.0,0.0,0.0,0.0]],[16,21.12,-156.835,730.0,[0.0,0.0,0.0,0.0]],[16,21.1205,-156.8268,570.0,[0.0,0.0,0.0,0.0]],[16,21.128,-156.8272,450.0,[0.0,0.0,0.0,0.0]],[16,21.1345,-156.8305,380.0,[0.0,0.0,0.0,0.0]],[16,21.1405,-156.8351999,330.0,[0.0,0.0,0.0,0.0]],[16,21.148125,-156.8357494,300.0,[0.0,0.0,0.0,0.0]],[16,21.153515,-156.8357485,250.0,[0.0,0.0,0.0,0.0]],[16,21.1589049,-156.8333394,220.0,[0.0,0.0,0.0,0.0]],[16,21.1692356,-156.8338192,200.0,[0.0,0.0,0.0,0.0]],[16,21.1737272,-156.8579016,180.0,[0.0,0.0,0.0,0.0]],[16,21.1728288,-156.8868016,235.31,[0.0,0.0,0.0,0.0]],[85,21.1656423,-156.8868016,165.31,[0.0,0.0,0.0,0.0]]],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[21.1669346,-156.886454,230.3],[21.1671238,-156.8863907,232.7],[21.1673443,-156.8863173,233.6],[21.16758,-156.886239,236.2],[21.1678016,-156.8861652,240.1],[21.1680504,-156.8860826,244.8],[21.1682773,-156.886007,249.0],[21.1685231,-156.8859249,253.3],[21.1687676,-156.8858434,257.2],[21.1689883,-156.8857698,260.8],[21.1692284,-156.8856894,265.1],[21.1694665,-156.8856098,269.5],[21.1697027,-156.8855306,274.1],[21.1699376,-156.885452,278.9],[21.1701718,-156.8853736,283.8],[21.1704051,-156.8852956,288.9],[21.1706374,-156.8852177,294.0],[21.1708703,-156.8851392,299.3],[21.1711038,-156.885061,304.6],[21.171317,-156.8849899,309.4],[21.1715525,-156.8849111,314.5],[21.1717879,-156.8848321,319.6],[21.1720018,-156.8847604,324.3],[21.1722365,-156.884682,329.5],[21.1724496,-156.8846102,334.3],[21.1726832,-156.8845312,339.6],[21.1729168,-156.8844525,344.9],[21.1731298,-156.8843809,349.6],[21.1733639,-156.8843023,354.9],[21.1735985,-156.8842234,360.2],[21.1738329,-156.8841443,365.4],[21.1740678,-156.8840654,370.6],[21.1742815,-156.8839936,375.3],[21.174515,-156.8839147,380.6],[21.1747485,-156.8838349,385.9],[21.174962,-156.8837615,390.7],[21.175199,-156.8836889,396.0],[21.1754222,-156.8836617,401.0],[21.1756631,-156.8837185,406.7],[21.1758627,-156.8838777,412.4],[21.1760041,-156.8840675,416.1],[21.1761423,-156.8842905,418.6],[21.1762883,-156.8845073,422.1],[21.1764632,-156.8846992,426.7],[21.1766693,-156.8848506,431.7],[21.1768775,-156.8849458,436.3],[21.177119,-156.8850018,441.3],[21.1773657,-156.8850106,446.2],[21.1775877,-156.8849832,450.6],[21.1778261,-156.8849195,455.7],[21.1780349,-156.8848354,460.5],[21.178253,-156.8847172,465.9],[21.1784577,-156.8845724,471.3],[21.1786294,-156.8844183,476.2],[21.1787989,-156.8842255,481.4],[21.1789436,-156.88401,486.8],[21.1790598,-156.8837759,492.1],[21.1791457,-156.883528,497.5],[21.1791968,-156.8832942,502.3],[21.1792228,-156.88303,507.7],[21.1792173,-156.8827646,513.0],[21.1791848,-156.8825255,517.7],[21.1791186,-156.8822694,523.0],[21.1790317,-156.882046,527.8],[21.1789087,-156.881815,533.1],[21.178759,-156.8816029,538.4],[21.1786017,-156.8814299,543.3],[21.1784084,-156.8812642,548.7],[21.1782165,-156.8811378,553.6],[21.1779921,-156.8810288,558.9],[21.1777559,-156.8809509,564.2],[21.1775333,-156.8809091,569.0],[21.1772854,-156.880895,574.4],[21.177038,-156.8809153,579.8],[21.1768167,-156.8809642,584.6],[21.1765812,-156.88105,589.9],[21.1763748,-156.8811537,593.7],[21.1761589,-156.8812955,596.4],[21.1759604,-156.88147,598.3],[21.1758003,-156.8816558,599.5],[21.1756518,-156.8818853,600.6],[21.1755355,-156.882134,601.2],[21.1754531,-156.8823955,601.5],[21.1754053,-156.8826642,601.7],[21.1753904,-156.8829121,601.6],[21.175405,-156.8831844,601.5],[21.1754522,-156.8834529,601.3],[21.1755228,-156.8836897,601.2],[21.1756298,-156.8839373,601.1],[21.1757527,-156.8841476,601.0],[21.175913,-156.8843574,600.8],[21.1760975,-156.8845432,600.7],[21.1762843,-156.884689,600.6],[21.1765064,-156.8848201,600.5],[21.1767434,-156.8849194,600.4],[21.17699,-156.8849846,600.4],[21.1772431,-156.885014,600.3],[21.1774741,-156.8850096,600.3],[21.1777243,-156.8849702,600.3],[21.1779675,-156.8848962,600.2],[21.1781804,-156.8847991,600.1],[21.1784001,-156.8846615,600.1],[21.1785839,-156.8845101,600.1],[21.1787638,-156.8843185,600.1],[21.1789183,-156.8841037,600.0],[21.1790351,-156.8838905,600.1],[21.1791361,-156.8836402,600.1],[21.1792057,-156.8833781,600.0],[21.1792425,-156.8831076,600.0],[21.1792457,-156.8828344,600.1],[21.1792193,-156.8825885,600.1],[21.1791589,-156.8823254,600.1],[21.1790665,-156.8820724,600.0],[21.1789563,-156.8818549,600.0],[21.1788083,-156.8816337,600.0],[21.1786512,-156.8814521,600.0],[21.1784557,-156.8812768,600.0],[21.1782409,-156.8811311,600.0],[21.1780317,-156.8810262,600.1],[21.1777911,-156.880943,600.0],[21.1775401,-156.8809093,599.3],[21.1772859,-156.8809355,597.1],[21.1770361,-156.8809965,593.1],[21.1768131,-156.8810689,589.0],[21.1765693,-156.8811569,584.4],[21.1763305,-156.8812454,579.6],[21.1761124,-156.8813262,575.0],[21.1758742,-156.8814129,569.8],[21.1756503,-156.8814569,565.2],[21.1753992,-156.8814365,560.6],[21.1751574,-156.881373,555.3],[21.1749434,-156.8812961,550.1],[21.1746891,-156.8811922,544.0],[21.1744768,-156.8811022,539.2],[21.1742447,-156.8810036,534.0],[21.1740117,-156.8809057,528.8],[21.1738016,-156.8808186,524.0],[21.1735664,-156.8807342,518.8],[21.1733203,-156.8807001,513.9],[21.1730962,-156.8807482,509.8],[21.1728839,-156.8808908,505.4],[21.1727223,-156.8810601,500.3],[21.172555,-156.8812508,494.1],[21.1723671,-156.8814245,488.7],[21.1721703,-156.8815445,484.5],[21.1719299,-156.8816192,480.0],[21.1717015,-156.8816292,475.8],[21.171456,-156.881578,470.9],[21.1712295,-156.8814703,465.7],[21.1710434,-156.8813313,460.8],[21.1708676,-156.881145,455.5],[21.1707209,-156.8809291,450.2],[21.1706165,-156.8807146,445.4],[21.1705352,-156.8804612,440.1],[21.1704955,-156.8802234,435.3],[21.1704915,-156.8799566,430.1],[21.1705293,-156.8796957,425.0],[21.1705992,-156.8794663,420.3],[21.1707115,-156.8792337,415.1],[21.1708612,-156.8790218,409.8],[21.1710428,-156.8788399,404.6],[21.1712509,-156.8786944,399.3],[21.1714576,-156.8785976,394.5],[21.1716984,-156.8785331,389.2],[21.1719431,-156.8785152,384.0],[21.1721657,-156.8785385,379.4],[21.1724048,-156.8786057,374.2],[21.1726106,-156.878703,369.5],[21.1728184,-156.8788488,364.3],[21.1729994,-156.8790307,359.1],[21.1731359,-156.8792227,354.3],[21.1732509,-156.8794571,349.0],[21.1733262,-156.8797085,343.6],[21.1733601,-156.8799708,338.2],[21.1733518,-156.8802348,332.9],[21.1733075,-156.8804707,328.2],[21.1732207,-156.8807175,323.0],[21.1730959,-156.8809452,317.8],[21.1729531,-156.881129,313.0],[21.1727674,-156.8813007,307.8],[21.1725759,-156.8814258,302.9],[21.172347,-156.8815242,297.5],[21.1721058,-156.8815784,292.1],[21.1718814,-156.8815876,287.3],[21.1716365,-156.8815537,282.1],[21.1714022,-156.8814758,276.9],[21.1711832,-156.8813558,271.5],[21.1709706,-156.8811824,265.7],[21.1708179,-156.881006,260.9],[21.1706823,-156.8807863,255.5],[21.1705836,-156.8805438,250.2],[21.1705264,-156.8802888,244.9],[21.1705112,-156.8800266,239.6],[21.1705339,-156.8797881,234.9],[21.1705987,-156.8795334,229.6],[21.1707148,-156.8792747,223.8],[21.1708445,-156.8790808,219.0],[21.1710184,-156.8788957,213.6],[21.1712013,-156.8787575,208.9],[21.1714207,-156.8786443,203.8],[21.171655,-156.878572,198.6],[21.1718751,-156.8785441,193.8],[21.1721216,-156.8785568,188.8],[21.1723402,-156.8786132,185.4],[21.1725649,-156.8787202,183.0],[21.1727676,-156.8788636,181.3],[21.1729308,-156.879022,180.1],[21.1730839,-156.8792242,179.2],[21.1732037,-156.879452,178.6],[21.1732801,-156.8796769,178.4],[21.1733245,-156.8799361,178.3],[21.1733273,-156.8801788,178.2],[21.173288,-156.8804413,178.4],[21.1732043,-156.8806934,178.6],[21.1730928,-156.8809053,178.7],[21.1729349,-156.8811117,178.9],[21.1727459,-156.881285,179.0],[21.1725305,-156.8814198,179.1],[21.1722977,-156.8815116,179.3],[21.1720747,-156.8815557,179.3],[21.1718322,-156.881551,179.5],[21.1716157,-156.8814489,181.4],[21.1714515,-156.8813114,184.1],[21.1712894,-156.8811537,187.8],[21.1711443,-156.8810096,191.7],[21.1709833,-156.8808519,195.6],[21.1708185,-156.8806935,198.0],[21.1706684,-156.8805511,199.4],[21.1704974,-156.8804028,200.4],[21.1703076,-156.8802837,201.0],[21.1700997,-156.8802091,201.4],[21.1698839,-156.8801793,201.4],[21.1696854,-156.880183,201.2],[21.1694691,-156.8802091,200.9],[21.169253,-156.8802457,200.8],[21.1690572,-156.8802799,200.7],[21.1688413,-156.8803134,200.6],[21.1686451,-156.8803385,200.5],[21.1684278,-156.8803605,200.4],[21.1682089,-156.8803782,200.4],[21.1680107,-156.8803922,200.4],[21.1677935,-156.8804069,200.3],[21.1675749,-156.8804221,200.2],[21.167356,-156.8804386,200.2],[21.1671376,-156.8804563,200.1],[21.1669381,-156.880473,200.1],[21.1667197,-156.8804917,200.1],[21.1665021,-156.8805101,200.1],[21.1663034,-156.8805267,200.1],[21.1660847,-156.8805446,200.1],[21.1658857,-156.8805607,200.1],[21.1656669,-156.8805784,200.1],[21.1654474,-156.8805963,200.1],[21.1652486,-156.8806125,200.0],[21.1650291,-156.8806306,200.0],[21.1648111,-156.8806486,200.0],[21.1645923,-156.8806668,200.0],[21.1643725,-156.8806852,200.0],[21.1641736,-156.8807018,200.0],[21.1639543,-156.8807201,200.0],[21.1637358,-156.8807383,200.0],[21.1635367,-156.8807549,200.0],[21.1633194,-156.8807724,200.0],[21.1631202,-156.8807887,199.9],[21.1629006,-156.8808067,200.0],[21.1626812,-156.8808247,200.0],[21.1624615,-156.8808428,200.0],[21.1622432,-156.8808605,200.1],[21.1620465,-156.8808764,200.0],[21.1618287,-156.8808939,199.9],[21.1616092,-156.8809118,200.0],[21.1614097,-156.8809282,200.0],[21.16119,-156.8809466,200.0],[21.1609701,-156.8809649,200.1],[21.1607724,-156.8809812,200.0],[21.1605552,-156.8809989,200.0],[21.1603559,-156.8810152,200.0],[21.1601366,-156.8810331,200.0],[21.1599173,-156.8810458,200.0],[21.1597162,-156.8810485,200.0],[21.1594979,-156.8810446,200.1],[21.1592975,-156.881038,200.0],[21.1590789,-156.8810304,200.0],[21.1588604,-156.8810244,200.0],[21.158662,-156.8810208,200.0],[21.1584424,-156.8810189,199.9],[21.1582214,-156.8810186,200.0],[21.1580204,-156.8810194,200.0],[21.1578018,-156.8810205,200.0],[21.1576028,-156.8810195,200.0],[21.1573874,-156.8809786,200.2],[21.1571863,-156.8808832,200.5],[21.15702,-156.8807634,200.3],[21.1568511,-156.8806128,200.1],[21.1566874,-156.8804557,199.9],[21.1565219,-156.8803018,199.8],[21.156351,-156.880155,199.8],[21.1561896,-156.8800276,199.8],[21.156008,-156.8798942,199.9],[21.1558241,-156.8797654,199.9],[21.1556561,-156.8796504,199.9],[21.1554706,-156.8795238,199.9],[21.1553027,-156.8794079,199.9],[21.1551198,-156.87928,199.9],[21.1549377,-156.8791508,199.9],[21.1547725,-156.8790324,199.9],[21.1545849,-156.8789095,200.1],[21.1543899,-156.878801,200.7],[21.1541923,-156.8787035,201.6],[21.1539932,-156.8786098,202.6],[21.153813,-156.8785246,203.6],[21.1536149,-156.8784279,204.8],[21.1534189,-156.8783281,206.1],[21.1532413,-156.8782344,207.4],[21.1530465,-156.8781293,208.8],[21.1528697,-156.878033,210.1],[21.1526747,-156.8779266,211.6],[21.1524799,-156.8778207,213.1],[21.1523019,-156.8777246,214.5],[21.1521058,-156.8776195,216.0],[21.1519129,-156.8775078,218.2],[21.1517155,-156.8773711,221.9],[21.1515416,-156.8772382,224.8],[21.1513835,-156.8771136,226.3],[21.1512071,-156.8769749,227.5],[21.1510259,-156.8768354,228.5],[21.1508427,-156.8766983,229.4],[21.1506406,-156.8765511,230.0],[21.1504722,-156.8764308,230.4],[21.1502865,-156.8762999,230.6],[21.1501013,-156.8761702,230.7],[21.1499329,-156.876052,230.8],[21.1497484,-156.875922,230.7],[21.1495812,-156.8758032,230.6],[21.1493897,-156.8756874,230.6],[21.1491774,-156.8756244,231.0],[21.1489779,-156.875613,231.5],[21.1487602,-156.875633,232.0],[21.1485638,-156.8756641,232.7],[21.1483484,-156.875698,233.8],[21.1481324,-156.8757229,235.1],[21.1479349,-156.8757359,236.4],[21.1477173,-156.8757407,237.9],[21.1474995,-156.875739,239.5],[21.1473014,-156.8757345,241.0],[21.1470829,-156.875729,242.7],[21.1468844,-156.8757253,244.2],[21.1466672,-156.875749,246.3],[21.1464607,-156.8758281,248.2],[21.1462901,-156.8759396,249.7],[21.1461199,-156.8760877,251.2],[21.1459569,-156.8762469,252.8],[21.1457931,-156.8764042,254.7],[21.1456248,-156.876553,256.8],[21.1454659,-156.8766807,258.6],[21.1452861,-156.8768135,260.7],[21.1451027,-156.8769412,262.7],[21.1449348,-156.8770551,264.6],[21.14475,-156.8771802,266.6],[21.1445824,-156.8772948,268.5],[21.144399,-156.8774226,270.6],[21.1442107,-156.8775412,272.9],[21.1440196,-156.877594,275.9],[21.1438019,-156.8775877,278.2],[21.1435913,-156.8775204,279.6],[21.1433966,-156.8774077,280.7],[21.1432152,-156.8772734,281.9],[21.143053,-156.8771463,283.1],[21.1428712,-156.8770114,284.7],[21.1426837,-156.8768868,286.2],[21.1425086,-156.8767831,287.6],[21.1423124,-156.8766773,289.1],[21.1421318,-156.8765856,290.4],[21.1419326,-156.8764868,291.9],[21.1417337,-156.8763874,293.4],[21.1415534,-156.8762954,294.7],[21.1413527,-156.8761991,296.3],[21.1411384,-156.8761472,298.2],[21.1409202,-156.8761472,300.3],[21.1407043,-156.8761837,302.9],[21.1405134,-156.8762301,305.5],[21.1403038,-156.8762813,308.8],[21.1400733,-156.876328,312.7],[21.1398606,-156.8763601,316.5],[21.1396466,-156.8763835,320.4],[21.1394509,-156.8764001,324.0],[21.1392364,-156.8764159,328.1],[21.1390205,-156.8764313,332.1],[21.1388227,-156.8764461,335.9],[21.1386057,-156.8764633,340.1],[21.1384089,-156.8764805,343.9],[21.1381927,-156.8765008,348.0],[21.1379565,-156.8765239,352.5],[21.1377593,-156.8765433,356.2],[21.1375429,-156.8765646,360.4],[21.1373462,-156.8765837,364.1],[21.1371287,-156.8766044,368.2],[21.1369104,-156.8766249,372.3],[21.1367135,-156.8766428,376.1],[21.1364958,-156.8766623,380.2],[21.1362799,-156.8766817,384.3],[21.1360824,-156.8766997,388.0],[21.1358657,-156.8767197,392.1],[21.135667,-156.8767381,395.9],[21.1354503,-156.8767586,399.9],[21.1352323,-156.8767788,404.0],[21.1350351,-156.8767959,407.8],[21.1348186,-156.876781,412.3],[21.1346095,-156.8767147,416.5],[21.1344132,-156.8766105,420.3],[21.1342271,-156.8764857,424.0],[21.1340615,-156.8763684,427.6],[21.1338764,-156.8762429,431.7],[21.1336882,-156.8761268,435.7],[21.1335119,-156.8760286,439.4],[21.1333146,-156.8759269,443.4],[21.133134,-156.8758379,447.2],[21.1329347,-156.8757422,451.3],[21.1327351,-156.8756463,455.4],[21.1325545,-156.8755582,459.1],[21.1323563,-156.8754596,463.2],[21.1321579,-156.8753589,467.3],[21.1319611,-156.8752581,471.4],[21.1317632,-156.8751565,475.6],[21.1315835,-156.8750642,479.3],[21.1313857,-156.874963,483.4],[21.1311877,-156.8748629,487.5],[21.1310083,-156.874772,491.3],[21.1308104,-156.8746719,495.4],[21.1306308,-156.8745813,499.1],[21.1304328,-156.8744815,503.2],[21.1302342,-156.8743813,507.4],[21.1300536,-156.8742895,511.1],[21.1298562,-156.8741889,515.2],[21.129658,-156.8740878,519.3],[21.1294604,-156.8739872,523.4],[21.1292617,-156.873886,527.6],[21.1290817,-156.8737947,531.3],[21.1288835,-156.8736942,535.4],[21.1286853,-156.8735937,539.5],[21.1285046,-156.8735026,543.2],[21.128306,-156.873403,547.3],[21.1281222,-156.8733161,551.1],[21.1279191,-156.8732265,555.2],[21.1277145,-156.8731399,559.3],[21.1275277,-156.8730618,563.0],[21.1273227,-156.8729751,567.0],[21.127137,-156.872895,570.8],[21.1269336,-156.8728057,574.8],[21.1267313,-156.8727157,578.9],[21.1265473,-156.8726335,582.6],[21.1263435,-156.8725431,586.6],[21.1261396,-156.8724529,590.7],[21.1259544,-156.8723707,594.4],[21.1257514,-156.8722806,598.5],[21.1255657,-156.872198,602.2],[21.125362,-156.8721074,606.2],[21.1251591,-156.8720164,610.3],[21.1249743,-156.8719334,614.0],[21.1247709,-156.8718426,618.1],[21.1245667,-156.8717517,622.1],[21.1243448,-156.8716532,626.6],[21.1241415,-156.871563,630.6],[21.1239558,-156.8714818,634.3],[21.1237437,-156.8714154,638.7],[21.1235254,-156.8713861,643.4],[21.1233264,-156.8713818,647.7],[21.1231087,-156.8713873,652.6],[21.1229111,-156.8713923,657.3],[21.1226942,-156.8713921,662.5],[21.1224781,-156.871384,667.8],[21.1222797,-156.8713709,672.6],[21.1220607,-156.8713523,677.7],[21.1218613,-156.8713343,682.4],[21.1216421,-156.8713148,687.6],[21.1214233,-156.8712961,692.8],[21.121225,-156.8712794,697.6],[21.1210071,-156.8712616,702.9],[21.1207883,-156.8712437,708.2],[21.1205896,-156.8712269,713.1],[21.1203707,-156.8712083,718.3],[21.1201716,-156.8711916,723.0],[21.1199521,-156.8711729,728.2],[21.1197343,-156.8711436,733.5],[21.119545,-156.8710768,738.5],[21.1193737,-156.8709297,744.3],[21.1192738,-156.8707192,750.5],[21.1192192,-156.8704914,755.2],[21.1191688,-156.8702621,759.5],[21.1191013,-156.8700611,764.1],[21.1189912,-156.8698596,769.5],[21.1188405,-156.8696866,774.8],[21.1186738,-156.8695637,779.5],[21.1184697,-156.8694718,784.5],[21.1182734,-156.8694247,789.1],[21.1180527,-156.8694097,794.3],[21.1178343,-156.8694281,799.5],[21.1176396,-156.8694719,804.2],[21.1174317,-156.8695492,809.6],[21.1172371,-156.8696555,815.0],[21.1170567,-156.8697917,820.3],[21.1168975,-156.8699535,825.6],[21.1167736,-156.8701221,830.4],[21.1166646,-156.8703259,835.7],[21.1165858,-156.8705457,841.1],[21.1165413,-156.8707555,845.9],[21.1165233,-156.8709917,851.2],[21.1165355,-156.8712064,856.1],[21.1165795,-156.8714376,861.4],[21.1166552,-156.8716596,866.7],[21.1167503,-156.8718483,871.5],[21.1168812,-156.8720373,876.9],[21.1170375,-156.872204,882.2],[21.117215,-156.8723446,887.6],[21.1174284,-156.872465,893.3],[21.1176186,-156.8725361,898.2],[21.1178357,-156.872581,903.6],[21.1180568,-156.8725904,908.9],[21.1182769,-156.872564,914.3],[21.118491,-156.8725031,919.7],[21.1186765,-156.8724184,924.6],[21.1188661,-156.872295,929.9],[21.1190511,-156.8721277,935.6],[21.1191832,-156.871966,940.5],[21.119304,-156.8717675,945.9],[21.1193899,-156.8715717,950.7],[21.1194555,-156.8713442,956.0],[21.1194889,-156.8711084,961.3],[21.1194904,-156.8708916,966.0],[21.11946,-156.8706557,971.3],[21.1194039,-156.8704475,976.1],[21.1193133,-156.8702319,981.5],[21.1191938,-156.8700328,986.8],[21.1190619,-156.8698694,991.6],[21.1188946,-156.8697127,997.0],[21.1187073,-156.8695836,1002.2],[21.1185228,-156.8694929,1007.0],[21.1183094,-156.8694261,1012.3],[21.1181099,-156.8693964,1017.1],[21.1178876,-156.869397,1022.4],[21.1176675,-156.8694323,1027.8],[21.117473,-156.8694941,1032.7],[21.1172703,-156.8695925,1037.9],[21.1170827,-156.8697226,1043.2],[21.1169154,-156.8698809,1048.4],[21.1167724,-156.8700644,1053.7],[21.116666,-156.8702498,1058.5],[21.1165769,-156.870469,1063.8],[21.1165187,-156.8707,1069.1],[21.1164937,-156.8709164,1073.8],[21.1164975,-156.8711565,1078.9],[21.1165303,-156.8713726,1083.8],[21.1165968,-156.8716003,1089.0],[21.1166942,-156.8718158,1094.2],[21.1168079,-156.8719965,1099.0],[21.1169577,-156.8721751,1104.3],[21.1171303,-156.8723287,1109.5],[21.1173223,-156.8724571,1113.8],[21.117532,-156.8725584,1116.5],[21.1177373,-156.87262,1118.3],[21.1179715,-156.8726484,1119.7],[21.1182075,-156.8726342,1120.8],[21.1184162,-156.8725858,1121.4],[21.1186347,-156.8724964,1121.8],[21.118819,-156.8723861,1121.9],[21.1190018,-156.8722365,1121.8],[21.1191618,-156.8720597,1121.7],[21.1192851,-156.8718784,1121.5],[21.119393,-156.8716611,1121.4],[21.1194642,-156.8714505,1121.2],[21.1195106,-156.8712101,1121.1],[21.1195229,-156.8709655,1121.0],[21.1195018,-156.87074,1120.8],[21.1193996,-156.8705031,1122.4],[21.1192411,-156.8702971,1123.5],[21.1190837,-156.8701255,1122.3],[21.1189052,-156.869948,1119.3],[21.1187333,-156.8697846,1117.3],[21.1185374,-156.8696002,1116.9],[21.1183215,-156.8693944,1117.4],[21.1181514,-156.8692098,1118.0],[21.1179982,-156.8689771,1118.8],[21.1179008,-156.8687417,1119.3],[21.1178386,-156.8684663,1119.7],[21.1178131,-156.8681851,1119.6],[21.1178101,-156.8679288,1119.5],[21.1178163,-156.8676479,1119.5],[21.1178229,-156.8673674,1119.6],[21.1178249,-156.8671127,1119.7],[21.1178211,-156.8668332,1119.9],[21.1178124,-156.8665793,1119.9],[21.1177988,-156.8663006,1120.0],[21.1177832,-156.8660222,1120.0],[21.1177688,-156.8657688,1120.0],[21.1177534,-156.8654901,1120.0],[21.1177394,-156.8652103,1120.0],[21.1177263,-156.8649306,1120.1],[21.1177137,-156.8646523,1120.1],[21.1177023,-156.8643997,1120.1],[21.1176899,-156.8641211,1120.0],[21.1176775,-156.8638421,1120.0],[21.1176663,-156.8635877,1120.1],[21.1176538,-156.8633076,1120.1],[21.1176423,-156.8630533,1120.1],[21.1176293,-156.8627739,1120.1],[21.1176165,-156.862495,1120.1],[21.117605,-156.8622419,1120.0],[21.1175921,-156.8619641,1120.0],[21.1175791,-156.8616847,1120.0],[21.1175663,-156.8614057,1120.0],[21.1175537,-156.8611261,1120.0],[21.1175424,-156.8608722,1120.0],[21.1175299,-156.8605923,1120.0],[21.1175171,-156.8603127,1120.0],[21.1175053,-156.8600597,1120.0],[21.1174952,-156.8597818,1119.9],[21.1175026,-156.8595273,1119.3],[21.1175299,-156.8592481,1117.3],[21.1175722,-156.8589688,1114.3],[21.1176172,-156.8587158,1111.1],[21.117667,-156.8584387,1107.3],[21.1177128,-156.8581631,1103.1],[21.1177535,-156.8578872,1098.8],[21.1177891,-156.8576117,1094.3],[21.1178181,-156.8573615,1090.0],[21.1178478,-156.857087,1085.3],[21.1178769,-156.8568123,1080.5],[21.1179035,-156.8565628,1076.1],[21.1179334,-156.8562892,1071.3],[21.1179614,-156.8560405,1067.0],[21.1179928,-156.8557666,1062.2],[21.1180247,-156.855493,1057.5],[21.1180536,-156.8552446,1053.2],[21.1180852,-156.8549719,1048.5],[21.1181166,-156.8546988,1043.7],[21.1181478,-156.854426,1039.0],[21.1181789,-156.8541548,1034.4],[21.1182073,-156.8539072,1030.1],[21.1182387,-156.8536337,1025.5],[21.1182702,-156.8533598,1020.8],[21.1182991,-156.8531107,1016.6],[21.1183307,-156.8528388,1012.0],[21.1183595,-156.8525919,1007.8],[21.1183908,-156.8523195,1003.1],[21.1184221,-156.8520465,998.5],[21.1184505,-156.8517982,994.3],[21.1184816,-156.8515254,989.7],[21.11851,-156.8512771,985.5],[21.1185411,-156.8510061,980.9],[21.1185724,-156.8507345,976.2],[21.1186041,-156.850462,971.6],[21.1186357,-156.850189,967.0],[21.1186671,-156.8499159,962.4],[21.1186955,-156.849668,958.2],[21.1187266,-156.8493961,953.6],[21.1187549,-156.8491488,949.4],[21.118786,-156.8488774,944.8],[21.1188169,-156.8486072,940.1],[21.1188453,-156.8483601,935.9],[21.1188769,-156.8480873,931.3],[21.1189058,-156.8478391,927.3],[21.1189377,-156.8475666,922.7],[21.1189692,-156.8472957,918.1],[21.1189978,-156.8470499,913.9],[21.1190288,-156.8467789,909.2],[21.1190563,-156.8465059,904.7],[21.119077,-156.8462573,900.9],[21.1190966,-156.8459836,896.8],[21.1191133,-156.8457354,893.1],[21.1191319,-156.8454643,889.1],[21.1191517,-156.8451926,885.1],[21.1191711,-156.8449442,881.6],[21.1191935,-156.8446711,877.7],[21.1192167,-156.8443972,873.9],[21.1192399,-156.8441244,870.0],[21.1192632,-156.8438514,866.1],[21.1192842,-156.843604,862.6],[21.1193072,-156.8433325,858.8],[21.1193301,-156.8430601,854.9],[21.1193508,-156.8428119,851.4],[21.1193735,-156.842539,847.6],[21.1193939,-156.8422908,844.1],[21.1194166,-156.8420182,840.3],[21.1194392,-156.8417458,836.4],[21.1194596,-156.8414982,832.9],[21.119482,-156.8412246,829.1],[21.1195042,-156.8409551,825.2],[21.1195267,-156.8406815,821.3],[21.1195492,-156.8404113,817.5],[21.1195698,-156.8401644,814.0],[21.1195926,-156.839892,810.1],[21.1196155,-156.8396194,806.3],[21.1196363,-156.8393733,802.9],[21.119659,-156.8391025,799.0],[21.1196798,-156.8388549,795.5],[21.1197025,-156.8385828,791.6],[21.119725,-156.8383108,787.8],[21.1197457,-156.8380634,784.3],[21.1197686,-156.8377913,780.5],[21.1197914,-156.8375192,776.6],[21.1198137,-156.8372474,772.7],[21.1198378,-156.8369511,768.5],[21.1198581,-156.8367042,765.0],[21.1198807,-156.8364328,761.2],[21.1199036,-156.8361612,757.3],[21.1199264,-156.8358899,753.5],[21.1199489,-156.8356197,749.6],[21.1199692,-156.8353731,746.1],[21.1199917,-156.8351018,742.3],[21.1200145,-156.83483,738.4],[21.1200338,-156.834583,734.8],[21.1200531,-156.8342873,730.0],[21.1200667,-156.8340404,725.7],[21.1200803,-156.833769,720.8],[21.1200938,-156.8334977,715.8],[21.1201068,-156.8332512,711.2],[21.1201217,-156.832981,706.0],[21.120136,-156.8327355,701.2],[21.1201525,-156.8324657,695.9],[21.1201696,-156.8321961,690.6],[21.1201852,-156.8319512,685.8],[21.1202024,-156.8316832,680.5],[21.1202193,-156.8314156,675.2],[21.1202344,-156.8311711,670.3],[21.1202508,-156.8309015,665.1],[21.1202654,-156.8306563,660.3],[21.1202812,-156.8303878,655.1],[21.1202969,-156.8301209,649.8],[21.1203114,-156.8298772,645.0],[21.1203276,-156.8296087,639.7],[21.1203441,-156.8293398,634.5],[21.1203608,-156.8290717,629.2],[21.1203777,-156.8288038,624.0],[21.1203929,-156.8285598,619.2],[21.1204096,-156.8282913,614.0],[21.1204261,-156.8280228,608.8],[21.1204408,-156.8277804,604.0],[21.1204566,-156.8275139,598.7],[21.1204813,-156.827271,593.8],[21.1205828,-156.8270222,589.2],[21.1207636,-156.8268305,584.6],[21.1209704,-156.8267249,580.0],[21.1212155,-156.8266833,574.4],[21.1214618,-156.8267024,568.7],[21.1217045,-156.8267546,563.1],[21.1219467,-156.8268157,558.0],[21.12217,-156.8268652,554.0],[21.1224173,-156.8269062,549.9],[21.1226663,-156.826933,546.0],[21.1228927,-156.826948,542.4],[21.1231426,-156.8269581,538.4],[21.1233698,-156.8269647,534.8],[21.1236208,-156.8269719,530.8],[21.1238716,-156.8269807,526.9],[21.1240988,-156.8269906,523.3],[21.124348,-156.8270031,519.3],[21.1245985,-156.8270171,515.3],[21.1248491,-156.8270315,511.4],[21.1250997,-156.8270459,507.5],[21.1253278,-156.8270588,503.9],[21.1255789,-156.8270723,499.9],[21.1258295,-156.8270854,496.0],[21.1260575,-156.8270972,492.4],[21.1263068,-156.8271104,488.4],[21.1265333,-156.8271225,484.7],[21.1267834,-156.8271359,480.7],[21.127057,-156.8271504,476.4],[21.127285,-156.8271623,472.8],[21.1275349,-156.8271753,468.8],[21.127761,-156.8271873,465.1],[21.1280101,-156.827201,461.1],[21.1282579,-156.8272427,457.0],[21.1284742,-156.8273252,454.0],[21.1286976,-156.8274491,450.9],[21.1289114,-156.8275886,447.7],[21.129104,-156.8277178,445.0],[21.1293184,-156.8278555,442.3],[21.1295174,-156.8279739,440.0],[21.12974,-156.8280966,437.5],[21.1299657,-156.8282132,435.1],[21.1301716,-156.8283157,432.9],[21.1303988,-156.8284267,430.5],[21.1306063,-156.8285281,428.3],[21.130835,-156.8286407,426.0],[21.1310632,-156.8287546,423.7],[21.1312702,-156.828859,421.5],[21.1314968,-156.8289744,419.1],[21.1317234,-156.8290899,416.6],[21.1319294,-156.8291947,414.5],[21.1321558,-156.82931,412.0],[21.1323622,-156.8294148,409.8],[21.1325894,-156.8295302,407.4],[21.1328166,-156.8296454,405.0],[21.1330235,-156.8297501,402.7],[21.1332505,-156.8298649,400.3],[21.1334778,-156.8299799,397.9],[21.1337031,-156.8300944,395.5],[21.1339292,-156.8302096,393.0],[21.1341352,-156.8303145,390.8],[21.1343622,-156.8304297,388.4],[21.1345886,-156.8305448,385.9],[21.1347896,-156.8306623,383.7],[21.1349972,-156.8308138,381.7],[21.1351764,-156.8309651,379.8],[21.135367,-156.8311364,377.9],[21.1355577,-156.8313088,376.0],[21.1357337,-156.8314632,374.3],[21.1359315,-156.83163,372.6],[21.136132,-156.8317921,370.9],[21.1363346,-156.8319509,369.2],[21.1365379,-156.832108,367.5],[21.1367233,-156.8322507,366.0],[21.1369274,-156.8324077,364.3],[21.1371311,-156.8325651,362.6],[21.137316,-156.8327087,361.1],[21.1375189,-156.8328672,359.4],[21.1377033,-156.8330114,357.9],[21.1379046,-156.833169,356.3],[21.1381067,-156.8333275,354.6],[21.1382912,-156.8334723,353.1],[21.1384938,-156.8336316,351.4],[21.1386966,-156.8337905,349.8],[21.1388995,-156.8339493,348.1],[21.1391024,-156.834108,346.4],[21.1392867,-156.8342522,344.9],[21.1394882,-156.8344098,343.2],[21.1396901,-156.8345678,341.4],[21.1398747,-156.8347121,339.9],[21.1400781,-156.8348707,338.3],[21.1402625,-156.835015,336.8],[21.1404654,-156.8351713,335.0],[21.1406896,-156.8352902,333.2],[21.1409129,-156.8353452,332.2],[21.1411633,-156.835361,330.8],[21.141414,-156.8353514,329.2],[21.1416873,-156.8353327,327.8],[21.1419377,-156.8353209,326.7],[21.1421648,-156.8353188,325.8],[21.1424139,-156.8353265,324.9],[21.1426635,-156.8353421,323.9],[21.1429132,-156.8353624,322.9],[21.1431626,-156.8353843,322.1],[21.1433898,-156.8354043,321.2],[21.1436398,-156.8354253,320.2],[21.1438897,-156.835445,319.3],[21.1441393,-156.8354636,318.3],[21.1443881,-156.8354811,317.4],[21.1446153,-156.8354968,316.5],[21.144866,-156.8355142,315.5],[21.1451168,-156.835532,314.5],[21.1453448,-156.8355486,313.7],[21.1455941,-156.8355669,312.7],[21.1458205,-156.8355835,311.8],[21.1460703,-156.8356016,310.8],[21.1463204,-156.8356198,309.8],[21.1465478,-156.835636,309.0],[21.1467987,-156.8356536,308.0],[21.1470491,-156.8356712,307.0],[21.1472755,-156.8356876,306.1],[21.1475244,-156.8357059,305.1],[21.1477517,-156.8357227,304.2],[21.1480013,-156.8357409,303.2],[21.1482514,-156.8357592,302.3],[21.1484789,-156.8357714,301.3],[21.14873,-156.835775,299.8],[21.1489813,-156.8357708,298.0],[21.1492326,-156.8357631,296.0],[21.1494836,-156.8357556,294.0],[21.1497123,-156.8357503,292.1],[21.1499628,-156.8357466,289.9],[21.1502132,-156.8357448,287.6],[21.1504404,-156.8357445,285.5],[21.1506899,-156.8357453,283.2],[21.1509146,-156.8357462,281.1],[21.1511647,-156.8357472,278.7],[21.1514132,-156.835748,276.3],[21.1516403,-156.8357485,274.2],[21.1518882,-156.8357487,271.9],[21.152137,-156.8357486,269.5],[21.1523866,-156.8357486,267.2],[21.1526369,-156.835748,264.9],[21.1528637,-156.8357472,262.8],[21.153113,-156.8357466,260.4],[21.1533622,-156.8357462,258.1],[21.1535877,-156.8357431,255.8],[21.1538335,-156.8357006,253.6],[21.1540482,-156.8356201,252.0],[21.1542725,-156.8355035,250.2],[21.1544901,-156.8353744,248.4],[21.1546876,-156.835256,246.9],[21.1549076,-156.8351315,245.5],[21.1551112,-156.8350253,244.3],[21.1553386,-156.8349162,243.0],[21.1555689,-156.834813,241.8],[21.1557796,-156.8347223,240.7],[21.1560113,-156.8346239,239.4],[21.156243,-156.8345249,238.2],[21.1564532,-156.8344336,237.0],[21.1566836,-156.8343321,235.8],[21.1568932,-156.8342387,234.6],[21.1571233,-156.8341356,233.3],[21.1573533,-156.8340319,232.1],[21.1575625,-156.8339374,231.0],[21.1577923,-156.8338337,229.7],[21.1580224,-156.8337301,228.4],[21.1582514,-156.8336279,227.2],[21.1584809,-156.833526,225.9],[21.1586905,-156.8334331,224.7],[21.1589221,-156.8333326,223.4],[21.1591653,-156.8332681,222.3],[21.1593929,-156.8332553,221.6],[21.1596409,-156.8332765,220.6],[21.1598633,-156.8333122,219.7],[21.1601087,-156.8333558,218.9],[21.1603551,-156.8333944,218.3],[21.1605803,-156.8334216,217.8],[21.1608282,-156.8334422,217.3],[21.1610777,-156.8334552,216.8],[21.1613267,-156.8334637,216.3],[21.1615762,-156.8334705,215.8],[21.1618028,-156.8334768,215.4],[21.1620512,-156.8334849,215.0],[21.1623002,-156.8334947,214.6],[21.1625275,-156.8335051,214.1],[21.1627774,-156.8335176,213.7],[21.1630045,-156.8335292,213.3],[21.1632519,-156.8335416,212.9],[21.1635003,-156.8335533,212.3],[21.163727,-156.8335636,211.9],[21.1639774,-156.833575,211.4],[21.1642271,-156.8335862,211.0],[21.1644766,-156.8335975,210.6],[21.1647255,-156.8336091,210.1],[21.1649524,-156.8336198,209.6],[21.1652012,-156.8336313,209.1],[21.1654505,-156.8336426,208.6],[21.165677,-156.8336531,208.2],[21.1659267,-156.8336646,207.7],[21.1661535,-156.833675,207.3],[21.1664015,-156.8336867,206.8],[21.1666504,-156.8336988,206.3],[21.1668775,-156.8337101,205.9],[21.1671273,-156.8337221,205.4],[21.1673773,-156.8337337,205.0],[21.1676265,-156.8337451,204.5],[21.1678753,-156.8337563,204.0],[21.168101,-156.8337667,203.6],[21.1683482,-156.833778,203.1],[21.1685965,-156.8337892,202.5],[21.1688234,-156.8338021,202.0],[21.1690653,-156.833872,201.6],[21.1692518,-156.8340147,202.0],[21.169398,-156.8342316,202.0],[21.1694893,-156.8345038,201.1],[21.1695241,-156.8347425,199.9],[21.1695382,-156.8350095,199.0],[21.1695461,-156.8352528,198.6],[21.1695621,-156.8355194,198.4],[21.1695906,-156.8357846,198.3],[21.1696268,-156.8360238,198.2],[21.1696747,-156.8362846,198.1],[21.1697276,-156.8365448,198.0],[21.1697772,-156.8367813,197.8],[21.1698316,-156.8370416,197.6],[21.1698797,-156.8372789,197.5],[21.1699306,-156.8375403,197.4],[21.1699796,-156.8378022,197.2],[21.1700233,-156.8380409,197.0],[21.1700715,-156.8383032,196.8],[21.1701155,-156.8385417,196.6],[21.170164,-156.8388042,196.4],[21.1702126,-156.8390653,196.2],[21.1702574,-156.8393025,196.0],[21.1703066,-156.8395637,195.8],[21.1703558,-156.8398259,195.6],[21.1704005,-156.8400642,195.4],[21.1704494,-156.8403266,195.2],[21.1704936,-156.8405651,195.0],[21.1705425,-156.8408276,194.8],[21.1705916,-156.84109,194.6],[21.1706363,-156.8413288,194.4],[21.1706854,-156.8415905,194.2],[21.1707347,-156.8418529,193.9],[21.1707835,-156.8421151,193.7],[21.1708318,-156.8423781,193.5],[21.1708758,-156.8426167,193.3],[21.1709246,-156.8428788,193.1],[21.1709738,-156.8431406,192.9],[21.1710184,-156.8433775,192.7],[21.1710674,-156.8436382,192.4],[21.171112,-156.8438765,192.2],[21.171161,-156.8441389,192.0],[21.1712101,-156.8444023,191.8],[21.1712545,-156.8446412,191.7],[21.1713033,-156.8449031,191.4],[21.1713517,-156.8451638,191.2],[21.1714003,-156.8454253,190.9],[21.1714491,-156.8456872,190.7],[21.1714933,-156.8459259,190.6],[21.1715421,-156.8461886,190.3],[21.1715909,-156.846451,190.1],[21.1716356,-156.8466893,190.0],[21.1716849,-156.8469508,189.7],[21.1717297,-156.8471888,189.5],[21.1717791,-156.8474511,189.3],[21.1718284,-156.8477137,189.1],[21.1718728,-156.8479523,188.9],[21.1719212,-156.8482142,188.7],[21.1719697,-156.8484765,188.4],[21.1720181,-156.8487383,188.2],[21.1720665,-156.8489988,188.0],[21.1721113,-156.8492366,187.8],[21.1721608,-156.849499,187.5],[21.1722103,-156.8497619,187.3],[21.172255,-156.850001,187.2],[21.1723035,-156.8502622,187.0],[21.1723472,-156.8504993,186.8],[21.1723953,-156.8507611,186.6],[21.1724441,-156.8510233,186.3],[21.172489,-156.8512618,186.1],[21.1725382,-156.8515241,185.9],[21.172587,-156.8517866,185.7],[21.1726398,-156.8520707,185.5],[21.1726885,-156.8523323,185.2],[21.1727329,-156.8525708,185.0],[21.1727819,-156.8528337,184.8],[21.1728308,-156.8530968,184.6],[21.1728797,-156.8533592,184.4],[21.1729287,-156.8536212,184.2],[21.1729734,-156.853859,183.9],[21.1730222,-156.8541194,183.7],[21.1730713,-156.8543808,183.5],[21.1731159,-156.8546193,183.3],[21.1731646,-156.8548819,183.1],[21.1732088,-156.855121,182.9],[21.1732575,-156.8553837,182.7],[21.1733061,-156.8556445,182.5],[21.1733503,-156.8558819,182.3],[21.1733994,-156.8561438,182.1],[21.1734487,-156.8564063,181.8],[21.1734979,-156.8566683,181.6],[21.1735469,-156.8569309,181.4],[21.1735912,-156.8571693,181.2],[21.1736393,-156.8574298,181.0],[21.1736876,-156.8576913,180.8],[21.1737319,-156.8579302,180.6],[21.1737666,-156.8581949,181.4],[21.1737684,-156.8584358,183.8],[21.1737498,-156.8586948,187.1],[21.1737233,-156.8589504,190.9],[21.1736999,-156.8591811,194.6],[21.173677,-156.8594391,196.4],[21.1736584,-156.8597066,195.9],[21.1736468,-156.8599811,196.1],[21.1736402,-156.8602557,197.0],[21.1736358,-156.8605036,197.6],[21.1736309,-156.8607731,198.1],[21.1736253,-156.8610413,198.4],[21.1736193,-156.8612842,198.6],[21.1736118,-156.8615523,198.9],[21.1736042,-156.8617904,199.1],[21.1735951,-156.8620595,199.3],[21.173586,-156.8623244,199.7],[21.173578,-156.8625676,200.0],[21.1735696,-156.8628347,200.4],[21.1735612,-156.8631018,200.7],[21.1735532,-156.863366,201.1],[21.1735454,-156.8636328,201.4],[21.1735384,-156.863876,201.7],[21.1735302,-156.8641446,202.1],[21.173522,-156.8644113,202.5],[21.1735143,-156.8646543,202.9],[21.1735058,-156.8649227,203.3],[21.1734983,-156.8651651,203.6],[21.1734898,-156.8654328,203.9],[21.1734813,-156.8656988,204.3],[21.1734732,-156.8659422,204.6],[21.1734648,-156.8662085,204.9],[21.1734564,-156.8664771,205.3],[21.1734484,-156.8667448,205.7],[21.1734403,-156.8670108,206.1],[21.1734328,-156.8672526,206.4],[21.173425,-156.8675198,206.8],[21.1734164,-156.8678125,207.2],[21.1734084,-156.8680797,207.7],[21.1734003,-156.868348,208.0],[21.1733925,-156.8685899,208.3],[21.1733837,-156.8688578,208.7],[21.1733753,-156.869124,209.1],[21.173368,-156.8693682,209.4],[21.1733598,-156.8696347,209.8],[21.1733522,-156.8698789,210.2],[21.1733437,-156.8701473,210.6],[21.1733353,-156.8704136,211.0],[21.1733277,-156.8706557,211.3],[21.1733196,-156.8709222,211.6],[21.1733117,-156.8711905,212.0],[21.1733039,-156.8714581,212.4],[21.1732955,-156.8717265,212.8],[21.1732878,-156.8719706,213.2],[21.1732793,-156.8722364,213.6],[21.1732708,-156.8725015,213.9],[21.1732631,-156.8727445,214.2],[21.1732551,-156.873013,214.6],[21.1732478,-156.8732557,215.0],[21.1732396,-156.8735234,215.4],[21.1732312,-156.8737895,215.8],[21.1732233,-156.8740323,216.1],[21.1732148,-156.8743001,216.5],[21.1732064,-156.8745679,216.9],[21.1731981,-156.8748347,217.2],[21.1731904,-156.8751014,217.6],[21.1731831,-156.8753452,218.0],[21.1731751,-156.8756131,218.4],[21.1731671,-156.8758803,218.7],[21.1731599,-156.876124,219.0],[21.1731518,-156.8763912,219.4],[21.1731444,-156.8766323,219.8],[21.1731358,-156.8769004,220.1],[21.1731271,-156.8771669,220.5],[21.1731191,-156.8774115,220.9],[21.1731105,-156.8776776,221.3],[21.173102,-156.8779467,221.7],[21.1730935,-156.8782135,222.1],[21.1730851,-156.8784814,222.5],[21.1730778,-156.8787235,222.8],[21.1730698,-156.8789925,223.1],[21.1730617,-156.8792596,223.5],[21.1730542,-156.8795027,223.8],[21.173046,-156.8797696,224.2],[21.1730384,-156.8800132,224.5],[21.17303,-156.8802798,224.9],[21.1730216,-156.880548,225.3],[21.1730142,-156.8807905,225.7],[21.173006,-156.8810577,226.0],[21.1729977,-156.8813267,226.4],[21.1729891,-156.8815955,226.8],[21.1729808,-156.8818604,227.3],[21.1729734,-156.8821024,227.5],[21.172965,-156.8823704,227.9],[21.1729569,-156.8826381,228.3],[21.1729495,-156.8828818,228.6],[21.1729412,-156.8831498,229.1],[21.1729336,-156.8833927,229.4],[21.1729254,-156.88366,229.8],[21.1729174,-156.8839269,230.1],[21.1729104,-156.884171,230.5],[21.1729026,-156.8844389,230.9],[21.1728952,-156.884683,231.2],[21.1728867,-156.8849508,231.6],[21.172877,-156.8852428,232.0],[21.1728689,-156.8854867,232.3],[21.17286,-156.8857545,232.7],[21.1728519,-156.8860223,233.1],[21.1728403,-156.8862608,233.5],[21.1727675,-156.886502,234.2],[21.1726389,-156.886682,235.0],[21.1724462,-156.8868204,235.3],[21.1722262,-156.8868908,235.1],[21.1720205,-156.886906,234.6],[21.1717929,-156.886889,234.0],[21.1715877,-156.8868602,233.8],[21.1713665,-156.8868291,233.7],[21.1711494,-156.8868059,233.7],[21.1709554,-156.8867931,233.7],[21.170747,-156.8867873,233.7],[21.1705449,-156.8867873,233.8],[21.1703631,-156.88679,233.8],[21.1701686,-156.8867938,233.9],[21.1699926,-156.886797,234.0],[21.1698045,-156.8867994,234.1],[21.1696159,-156.8868004,234.3],[21.1694437,-156.8868004,234.5],[21.169256,-156.8868,234.7],[21.1690686,-156.8867994,234.9],[21.1688814,-156.8867991,235.0],[21.1686951,-156.886799,235.1],[21.1685269,-156.8867989,235.2],[21.1683425,-156.8867991,235.3],[21.1681554,-156.8867994,235.4],[21.1679859,-156.8867997,235.5],[21.1677986,-156.8868002,235.5],[21.1676274,-156.8868008,235.6],[21.1674402,-156.8868012,235.6],[21.1672517,-156.8868013,235.6],[21.1670808,-156.8868013,235.7],[21.1668926,-156.8868013,235.7],[21.1667027,-156.8868013,235.7],[21.1665209,-156.8868013,235.7],[21.1663514,-156.8868012,236.0],[21.1662176,-156.8868012,236.4],[21.1660859,-156.8868012,236.1],[21.1659672,-156.8868013,235.9],[21.165872,-156.8868014,236.1],[21.1657836,-156.8868013,236.3],[21.1657189,-156.8868013,236.2],[21.1656873,-156.8868009,235.0],[21.1656823,-156.886801,235.7],[21.1656782,-156.8868011,235.7],[21.1656701,-156.8868011,235.7]],"source":"ArduPilot autotest QuadPlane.KalaupapaCanyonRun (SITL, ArduPlane V4.8.0-dev, ardupilot 3cf2e903d7, from ArduPilot PR 34424), flown track sampled once a second between the end of the transition and the start of the VTOL landing descent"} \ No newline at end of file diff --git a/tests/missions/plane-mission-geometry.txt b/tests/missions/plane-mission-geometry.txt new file mode 100644 index 0000000000..88ee54b50e --- /dev/null +++ b/tests/missions/plane-mission-geometry.txt @@ -0,0 +1,33 @@ +QGC WPL 110 +# a plane mission for the map's loiters, spirals and tangents +0 1 0 16 0 0 0 0 -35.3632610 149.1652300 584.000000 1 +# climb out to the north +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.3597223 149.1659951 100.000000 1 +# a plain leg, to settle onto a track before anything interesting +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3491062 149.1682903 120.000000 1 +# plain legs out to the west. ArduPlane has no NAV_ARC_WAYPOINT, so the arcs are in the copter mission instead +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3477023 149.1780519 120.000000 1 +# turning back south +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3552996 149.1814422 120.000000 1 +# and out towards the loiters +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3583721 149.1917926 130.000000 1 +# switch to an AMSL altitude: the climb into the next loiter is measured across the frame change +6 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.3529821 149.2032393 750.000000 1 +# loiter to altitude, 100m clockwise, climbing 400m over a short approach: most of that is left to do on arrival, so it is drawn as a spiral of about two turns. param4 0 leaves the next leg crosstracked from this centre, so the vehicle pulls back onto it +7 0 0 31 0.000000 100.000000 0.000000 0.000000 -35.3483143 149.2065435 1150.000000 1 +# a long leg, so there is room to see the vehicle rejoin the track out of the loiter centre +8 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.3288651 149.1927782 1150.000000 1 +# three turns, 100m counter-clockwise (negative radius). param4 1 crosstracks the next leg from where the circle is left instead +9 0 0 18 3.000000 0.000000 -100.000000 1.000000 -35.3288651 149.1762618 1150.000000 1 +# two turns the other way round, close enough that the leg between the two circles is the tangent which crosses between them +10 0 0 18 2.000000 0.000000 100.000000 0.000000 -35.3350889 149.1718573 1150.000000 1 +# loiter for 45s with no radius of its own, so it takes WP_LOITER_RAD, clockwise. The autopilot has nowhere to keep a radius for this command and hands back +-1 for the direction alone. Turning the same way as the loiter before it, the leg between them is the tangent which does not cross. Back to a home-relative altitude, and a descent +11 0 3 19 45.000000 0.000000 0.000000 0.000000 -35.3413126 149.1674524 300.000000 1 +# a terrain-frame altitude, 300m above the ground +12 0 10 16 0.000000 0.000000 0.000000 0.000000 -35.3391287 149.1522692 300.000000 1 +# loiter to altitude descending, 100m counter-clockwise: a plane comes down at TECS_SINK_MIN, so 200m of descent is several turns of spiral. param4 1, so no pull-back onto a track from here +13 0 3 31 0.000000 -100.000000 0.000000 1.000000 -35.3362416 149.1480513 90.000000 1 +# a plain leg out of the descending spiral +14 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.3286443 149.1446618 90.000000 1 +# circle here forever, 80m clockwise. The mission parks here: switch out of AUTO when there is nothing left to look at +15 0 3 17 0.000000 0.000000 80.000000 0.000000 -35.3232543 149.1446618 90.000000 1 diff --git a/tests/missions/record_flight.py b/tests/missions/record_flight.py new file mode 100644 index 0000000000..10e92454a4 --- /dev/null +++ b/tests/missions/record_flight.py @@ -0,0 +1,222 @@ +#!/usr/bin/env python3 +''' +record a SITL flight of a mission for the tests which check the path drawn +for it against the path flown + + python3 tests/missions/record_flight.py LOG OUT.json --source "..." + +LOG is the flight's dataflash log. OUT gets what MAVExplorer hands +plane_track.mission_track() for it -- home, the mission, the vehicle's +parameters, and where there are any, the takeoff course, where the flight +started, the rally points, the approach course and the course the takeoff +was flown on -- the firmware which +flew it, since what it does is what the drawing follows, and the path +flown, +once a second, from when the aircraft is flying as a plane -- the end of a +QuadPlane's transition, or the first POS of anything else -- to when it +starts to land: a QuadPlane's landing descent, a plane's flare, or the end +of the log -- or to when anything but the mission takes over, as a test +does to end a mission which circles for ever. --start and --end, in +seconds of the log's TimeUS, override either. + +AP_FLAKE8_CLEAN +''' + +import argparse +import importlib.util +import json +import os + +from pymavlink import mavutil + +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import plane_track + +HERE = os.path.dirname(os.path.abspath(__file__)) +# how the firmware names itself, in a log with no VER message +FIRMWARES = ('ArduPlane', 'ArduCopter', 'ArduRover', 'ArduSub', 'APM:') +# what the autopilot says as a flight starts and stops being fixed-wing +STARTS = ('Transition done',) +ENDS = ('Land descend started', 'Flare ') +# ArduPlane's modes a mission is flown in: AUTO, and the RTL and QRTL a +# return to launch switches to. Anything else, or AUTO again after those, +# is the flight being taken over +AUTO = 10 +RETURNING = (11, 21) + + +def explorer(): + path = os.path.join(HERE, '..', '..', 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def firmware(mlog): + '''the firmware which flew the log, as it names itself: whatever the + drawing is checked against has to follow what that version does, so a + recording says which it was. VER says which commit it was built from, + which is what tells one development build from another, so it is taken + over the message the firmware starts with wherever in the log it comes. + None where the log says neither''' + banner = None + mlog.rewind() + while True: + m = mlog.recv_match(type=['VER', 'MSG']) + if m is None: + return banner + if m.get_type() == 'VER': + # FWS is the firmware's own name for itself, GH its commit + name = getattr(m, 'FWS', None) + if name: + commit = getattr(m, 'GH', None) + if commit: + return '%s (%x)' % (name, commit) + return str(name) + elif banner is None and m.Message.startswith(FIRMWARES): + # a log with no VER says it in the message the firmware starts + # with, which a development build names its commit in too + banner = m.Message + + +def extras(heading, start, rally, approach, takeoff_course=None): + '''what the drawing is given beyond the mission, where there is + anything to say: a course of 0 is north, which is something''' + out = {} + for (name, value) in (('heading', heading), ('start', start), + ('rally', rally), ('approach', approach), + ('takeoff_course', takeoff_course)): + if value is not None and value != []: + out[name] = value + return out + + +def times(mlog): + '''when the aircraft starts flying the mission as a plane -- the end of + a QuadPlane's transition, or arming -- and when it starts to land or + stops flying the mission''' + transition = None + armed = None + first = None + last = None + events = [] + mlog.rewind() + while True: + m = mlog.recv_match(type=['MSG', 'POS', 'MODE', 'ARM']) + if m is None: + break + t = m.TimeUS * 1.0e-6 + kind = m.get_type() + if kind == 'POS': + if first is None: + first = t + last = t + elif kind == 'ARM': + if armed is None and m.ArmState: + armed = t + elif kind == 'MODE': + events.append((t, m.ModeNum)) + elif transition is None and m.Message.startswith(STARTS): + transition = t + elif m.Message.startswith(ENDS): + events.append((t, None)) + start = transition + if start is None: + start = armed if armed is not None else first + end = last + returned = False + for (t, mode) in events: + if t < start: + continue + if mode is None or (mode != AUTO and mode not in RETURNING) or ( + mode == AUTO and returned): + end = t + break + if mode in RETURNING: + returned = True + return (start, end) + + +def record(log, out, source, start=None, end=None): + mx = explorer() + mlog = mavutil.mavlink_connection(log) + (path, mission, cmds, started, rally, origin, + approach) = mx.mission_from_log(mlog) + params = mp_util.log_params(mlog) + ground0 = min(p[2] for p in path) + mission = mx.resolve_mission_amsl(mission, ground0, params, mlog.mav_type) + # what the drawing is given, taken as MAVExplorer gives it + captured = {} + real = plane_track.mission_track + + def capture(home, items, params, heading=None, start=None, rally=None, + approach=None, takeoff_course=None): + captured['home'] = list(home) + captured['items'] = [[c, la, lo, a, list(p)] + for (c, la, lo, a, p) in items] + captured.update(extras(heading, start, rally, approach, + takeoff_course)) + return real(home, items, params, heading, start, rally, approach, + takeoff_course) + plane_track.mission_track = capture + try: + mx.plane_mission_track(cmds, mission, + (path[0][0], path[0][1], ground0), params, + mlog.mav_type, path, started, rally, origin, + approach) + finally: + plane_track.mission_track = real + if 'items' not in captured: + raise ValueError('the log has no mission a plane flies') + names = sorted(set(name for (names, _) in plane_track.PARAMETERS.values() + for (name, _) in names)) + captured['params'] = dict((name, params[name]) for name in names + if name in params) + (flying, landing) = times(mlog) + if start is not None: + flying = start + if end is not None: + landing = end + flown = [] + last = None + mlog.rewind() + while True: + m = mlog.recv_match(type='POS') + if m is None: + break + t = m.TimeUS * 1.0e-6 + if t < flying or t > landing: + continue + if last is not None and t - last < 1.0: + continue + last = t + flown.append([round(m.Lat, 7), round(m.Lng, 7), round(m.Alt, 1)]) + captured['flown'] = flown + version = firmware(mlog) + if version is not None: + captured['firmware'] = version + captured['source'] = source + with open(out, 'w') as f: + json.dump(captured, f, separators=(',', ':')) + return (captured, flying, landing) + + +def main(): + parser = argparse.ArgumentParser(description=__doc__.split('\n\n')[0]) + parser.add_argument('log') + parser.add_argument('out') + parser.add_argument('--source', required=True, + help='where the flight came from, for the record') + parser.add_argument('--start', type=float, default=None) + parser.add_argument('--end', type=float, default=None) + args = parser.parse_args() + (captured, flying, landing) = record(args.log, args.out, args.source, + args.start, args.end) + print('%s: %u items, %u points flown from %.1fs to %.1fs, %u bytes' % ( + args.out, len(captured['items']), len(captured['flown']), flying, + landing, os.path.getsize(args.out))) + + +if __name__ == '__main__': + main() diff --git a/tests/missions/sitl-plane-loiter-unlimited.json b/tests/missions/sitl-plane-loiter-unlimited.json new file mode 100644 index 0000000000..e166e8b97c --- /dev/null +++ b/tests/missions/sitl-plane-loiter-unlimited.json @@ -0,0 +1 @@ +{"home":[-35.3629381,149.165085,585.0999755859375],"items":[[22,-35.361279,149.16423,625.0999755859375,[15.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.0999755859375,[0.0,0.0,0.0,0.0]],[17,-35.3557514,149.178303,685.0999755859375,[0.0,0.0,-120.0,0.0]],[189,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[16,-35.365467,149.164215,640.0999755859375,[0.0,0.0,0.0,0.0]],[16,-35.365009,149.165482,625.0999755859375,[0.0,0.0,0.0,0.0]],[21,-35.363041,149.165222,585.0999755859375,[0.0,0.0,0.0,1.0]]],"takeoff_course":353.9666672289162,"start":[-35.3629381,149.165085,585.0700073242188],"params":{"AIRSPEED_CRUISE":22.0,"AIRSPEED_MAX":30.0,"AIRSPEED_MIN":10.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":15.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":0.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.25,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":100.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":80.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":50.0},"flown":[[-35.3629381,149.165085,585.1],[-35.3629247,149.1650834,585.1],[-35.362823,149.1650703,584.8],[-35.3626503,149.1650477,590.3],[-35.3624278,149.1650183,598.6],[-35.3621831,149.1649865,608.4],[-35.3619268,149.164953,618.8],[-35.3616633,149.1649181,628.9],[-35.3613887,149.1648885,634.9],[-35.3611217,149.1648716,637.7],[-35.3608757,149.1648643,637.9],[-35.3606484,149.1648634,636.3],[-35.3604223,149.1648684,636.0],[-35.3601905,149.1648764,637.2],[-35.3599601,149.1648856,639.1],[-35.3597287,149.1648955,641.2],[-35.3595006,149.1649058,643.4],[-35.359271,149.1649159,645.6],[-35.3590449,149.1649262,647.7],[-35.3588173,149.1649367,649.9],[-35.3585932,149.1649469,652.1],[-35.3583671,149.1649574,654.2],[-35.3581442,149.1649674,656.4],[-35.3579199,149.1649774,658.6],[-35.3576983,149.1649875,660.7],[-35.3574753,149.164998,662.8],[-35.3572548,149.1650088,665.0],[-35.3570326,149.1650202,667.0],[-35.3568123,149.1650315,669.1],[-35.3565913,149.1650426,671.3],[-35.356372,149.1650537,673.3],[-35.3561515,149.1650645,675.4],[-35.3559341,149.165106,677.8],[-35.3557516,149.165274,682.0],[-35.3556076,149.1654866,683.1],[-35.3555283,149.1657445,683.8],[-35.3555058,149.1660172,683.8],[-35.3555263,149.1662879,683.7],[-35.3555705,149.166554,683.7],[-35.3556221,149.1668162,683.8],[-35.355671,149.1670813,684.1],[-35.3557112,149.1673471,684.4],[-35.3557408,149.1676168,684.6],[-35.3557596,149.1678845,684.8],[-35.3557697,149.168155,684.8],[-35.3557731,149.1684235,684.9],[-35.355772,149.168695,685.0],[-35.3557681,149.1689633,685.0],[-35.3557632,149.1692349,685.0],[-35.355758,149.1695034,685.0],[-35.3557536,149.1697751,685.0],[-35.3557504,149.170044,685.1],[-35.3557483,149.1703152,685.1],[-35.355747,149.1705839,685.1],[-35.3557465,149.1708554,685.1],[-35.3557464,149.1711246,685.1],[-35.3557466,149.1713956,685.1],[-35.3557475,149.1716647,685.0],[-35.3557484,149.1719355,685.1],[-35.3557492,149.1722055,685.0],[-35.3557497,149.1724762,685.0],[-35.3557502,149.1727466,685.0],[-35.3557507,149.1730168,685.0],[-35.3557508,149.1732872,685.0],[-35.3557509,149.1735576,685.0],[-35.3557509,149.1738286,685.0],[-35.3557509,149.1740985,685.1],[-35.3557509,149.1743697,685.0],[-35.3557509,149.1746391,685.0],[-35.3557507,149.174911,685.0],[-35.3557505,149.1751804,685.0],[-35.3557504,149.1754522,685.0],[-35.3557503,149.1757208,685.0],[-35.3557503,149.1759926,685.0],[-35.3557512,149.1762618,685.0],[-35.3557743,149.1765331,685.0],[-35.35585,149.1767901,685.3],[-35.3559904,149.1770107,685.8],[-35.3561624,149.1771819,685.6],[-35.356344,149.1773327,684.9],[-35.3565195,149.1774901,684.6],[-35.3566802,149.1776767,684.6],[-35.3568107,149.1778965,684.7],[-35.3569008,149.1781485,684.9],[-35.3569413,149.1784167,685.0],[-35.3569304,149.1786896,685.0],[-35.3568691,149.1789508,684.9],[-35.3567624,149.1791904,684.8],[-35.3566183,149.1793969,684.7],[-35.3564444,149.1795647,684.6],[-35.3562484,149.1796899,684.6],[-35.3560377,149.17977,684.6],[-35.3558178,149.1798035,684.6]],"source":"ArduPilot autotest Plane.MissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot bcb5e0c597, from ArduPilot PR 34424): sitl-plane-loiter-unlimited"} \ No newline at end of file diff --git a/tests/missions/sitl-plane-mission-items.json b/tests/missions/sitl-plane-mission-items.json new file mode 100644 index 0000000000..dcf2ea8065 --- /dev/null +++ b/tests/missions/sitl-plane-mission-items.json @@ -0,0 +1 @@ +{"home":[-35.3625914,149.1651656,584.09],"items":[[22,-35.361279,149.16423,624.09,[15.0,0.0,0.0,0.0]],[178,0.0,0.0,0.0,[0.0,20.0,-1.0,0.0]],[16,-35.3557514,149.165085,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3494631,149.1716937,684.09,[0.0,10.0,0.0,0.0]],[16,-35.3557514,149.178303,684.09,[0.0,0.0,80.0,0.0]],[36,-35.3674296,149.1816087,684.09,[90.0,0.0,0.0,0.0]],[16,-35.362938,149.1849129,694.09,[0.0,0.0,0.0,0.0]],[18,-35.3701245,149.1849138,694.09,[2.0,0.0,100.0,0.0]],[18,-35.3710228,149.1772027,694.09,[1.0,0.0,-80.0,1.0]],[19,-35.3674296,149.1716945,695.0,[22.0,0.0,1.0,0.0]],[31,-35.3737178,149.1683898,834.09,[0.0,120.0,0.0,0.0]],[31,-35.3737178,149.1595768,684.09,[0.0,-120.0,0.0,1.0]],[18,0.0,0.0,684.09,[1.0,0.0,80.0,0.0]],[600,0.0,0.0,0.0,[7.0,0.0,0.0,0.0]],[16,-35.3674296,149.1551707,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3656329,149.1617802,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3710228,149.1595769,684.09,[0.0,0.0,0.0,0.0]],[601,0.0,0.0,0.0,[7.0,1.0,0.0,0.0]],[177,0.0,0.0,0.0,[3.0,0.0,0.0,0.0]],[16,-35.360243,149.1573742,684.09,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1617804,674.09,[0.0,0.0,0.0,0.0]],[16,-35.3566497,149.1551713,674.09,[0.0,0.0,0.0,0.0]],[177,0.0,0.0,0.0,[20.0,1.0,0.0,0.0]],[189,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[16,-35.365467,149.164215,639.09,[0.0,0.0,0.0,0.0]],[16,-35.365009,149.165482,624.09,[0.0,0.0,0.0,0.0]],[21,-35.363041,149.165222,584.09,[0.0,0.0,0.0,1.0]]],"takeoff_course":353.99782242424425,"start":[-35.3629381,149.165085,585.239990234375],"params":{"AIRSPEED_CRUISE":22.0,"AIRSPEED_MAX":30.0,"AIRSPEED_MIN":10.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":15.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":0.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.25,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":100.0,"RTL_RADIUS":150.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":80.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":50.0},"flown":[[-35.3629381,149.165085,585.2],[-35.3629302,149.165084,585.0],[-35.36284,149.1650725,584.8],[-35.3626772,149.1650514,589.4],[-35.3624587,149.1650232,597.2],[-35.3622167,149.1649916,606.7],[-35.3619588,149.164958,617.0],[-35.3616969,149.1649242,627.3],[-35.3614222,149.1648928,634.4],[-35.3611553,149.1648756,638.5],[-35.3609047,149.1648677,639.8],[-35.3606811,149.164866,638.5],[-35.3604667,149.1648694,637.2],[-35.3602514,149.1648761,637.6],[-35.3600349,149.1648845,639.1],[-35.3598205,149.1648936,640.9],[-35.3596078,149.1649032,642.9],[-35.3593973,149.1649128,644.9],[-35.359189,149.1649224,647.0],[-35.3589816,149.1649319,649.0],[-35.3587761,149.1649412,651.1],[-35.3585711,149.1649507,653.1],[-35.3583682,149.1649599,655.1],[-35.358165,149.1649691,657.1],[-35.3579635,149.1649783,659.1],[-35.3577614,149.1649873,661.1],[-35.3575618,149.1649962,663.0],[-35.3573604,149.1650055,665.0],[-35.3571618,149.1650151,666.8],[-35.356961,149.1650251,668.7],[-35.3567627,149.1650352,670.6],[-35.3565622,149.1650451,672.5],[-35.3563642,149.1650549,674.4],[-35.3561642,149.1650647,676.2],[-35.3559661,149.1650739,678.0],[-35.3557663,149.165089,680.0],[-35.3555738,149.1651586,682.1],[-35.3553933,149.1652744,683.3],[-35.3552324,149.1654232,684.0],[-35.3550853,149.1655933,684.4],[-35.3549482,149.1657731,684.6],[-35.3548142,149.1659573,684.9],[-35.3546796,149.1661401,685.0],[-35.3545414,149.1663202,685.2],[-35.3543995,149.1664952,685.2],[-35.3542541,149.1666648,685.3],[-35.3541054,149.1668304,685.3],[-35.3539548,149.1669923,685.3],[-35.3538016,149.1671529,685.3],[-35.3536483,149.1673109,685.3],[-35.3534935,149.1674697,685.3],[-35.35334,149.1676265,685.2],[-35.3531855,149.1677853,685.2],[-35.3530326,149.1679432,685.2],[-35.3528787,149.1681031,685.2],[-35.3527269,149.1682615,685.2],[-35.3525735,149.1684222,685.2],[-35.352422,149.1685812,685.2],[-35.3522687,149.1687423,685.2],[-35.3521176,149.1689018,685.2],[-35.3519652,149.169063,685.2],[-35.3518139,149.1692228,685.2],[-35.351661,149.1693838,685.2],[-35.3515094,149.1695433,685.2],[-35.3513568,149.1697043,685.1],[-35.3512052,149.1698638,685.1],[-35.3510524,149.1700246,685.2],[-35.3509002,149.1701844,685.2],[-35.3507475,149.1703447,685.2],[-35.3505954,149.1705046,685.2],[-35.3504431,149.1706649,685.2],[-35.3502906,149.170825,685.2],[-35.350138,149.170985,685.2],[-35.3499853,149.1711453,685.2],[-35.3498336,149.1713048,685.2],[-35.3496812,149.1714652,685.1],[-35.3495291,149.171625,685.2],[-35.3493802,149.1717916,685.1],[-35.349307,149.1720282,686.1],[-35.3493329,149.1722903,687.1],[-35.3494371,149.1725109,686.8],[-35.3496029,149.1726601,686.0],[-35.3497904,149.17274,684.8],[-35.3499873,149.1727771,683.9],[-35.3501825,149.1728091,683.5],[-35.3503791,149.172861,683.6],[-35.3505682,149.1729397,683.8],[-35.3507501,149.1730469,684.1],[-35.3509196,149.1731756,684.3],[-35.3510809,149.1733226,684.5],[-35.3512331,149.1734795,684.6],[-35.3513812,149.1736446,684.7],[-35.3515256,149.1738118,684.8],[-35.3516701,149.1739812,684.8],[-35.3518149,149.1741497,684.9],[-35.3519618,149.1743176,685.0],[-35.3521097,149.1744828,685.0],[-35.352259,149.174646,685.1],[-35.3524097,149.1748072,685.1],[-35.3525616,149.1749672,685.1],[-35.3527145,149.1751265,685.1],[-35.3528672,149.175285,685.1],[-35.3530205,149.1754437,685.2],[-35.3531728,149.1756018,685.2],[-35.3533261,149.1757615,685.2],[-35.353478,149.17592,685.2],[-35.3536313,149.1760802,685.2],[-35.3537831,149.1762388,685.1],[-35.3539365,149.1763996,685.1],[-35.3540882,149.1765588,685.1],[-35.3542417,149.1767198,685.1],[-35.3543932,149.1768789,685.2],[-35.3545462,149.1770403,685.2],[-35.3546975,149.1772001,685.2],[-35.3548505,149.1773616,685.2],[-35.3550021,149.1775211,685.2],[-35.3551552,149.1776818,685.2],[-35.355307,149.1778412,685.2],[-35.3554601,149.1780022,685.2],[-35.3556121,149.1781617,685.2],[-35.3557648,149.1783221,685.2],[-35.3559167,149.1784816,685.2],[-35.3560696,149.178642,685.2],[-35.3562219,149.1788018,685.1],[-35.3563745,149.1789619,685.2],[-35.3565322,149.1791126,686.5],[-35.356694,149.1792502,689.0],[-35.3568578,149.1793834,690.8],[-35.3570233,149.1795166,692.1],[-35.357191,149.1796529,693.1],[-35.3573573,149.179791,693.8],[-35.3575235,149.179933,694.3],[-35.3576873,149.1800765,694.7],[-35.357851,149.1802233,694.9],[-35.3580117,149.1803701,695.1],[-35.3581729,149.1805188,695.2],[-35.3583323,149.180667,695.2],[-35.3584935,149.1808174,695.3],[-35.3586529,149.1809656,695.3],[-35.3588139,149.1811151,695.3],[-35.3589732,149.1812629,695.3],[-35.3591341,149.1814124,695.3],[-35.3592935,149.1815603,695.3],[-35.3594546,149.1817091,695.3],[-35.3596143,149.1818562,695.2],[-35.3597758,149.1820041,695.2],[-35.3599362,149.1821511,695.2],[-35.3600972,149.1822992,695.2],[-35.3602575,149.1824468,695.2],[-35.3604181,149.1825948,695.2],[-35.3605783,149.1827423,695.2],[-35.3607391,149.1828902,695.2],[-35.3608997,149.1830381,695.2],[-35.3610599,149.1831852,695.2],[-35.3612207,149.1833328,695.2],[-35.3613812,149.1834803,695.2],[-35.3615425,149.1836283,695.2],[-35.3617025,149.1837754,695.2],[-35.3618634,149.1839234,695.2],[-35.3620232,149.1840706,695.2],[-35.3621846,149.184219,695.2],[-35.3623445,149.1843661,695.1],[-35.362506,149.1845147,695.2],[-35.3626656,149.1846616,695.2],[-35.3628272,149.1848099,695.2],[-35.3629942,149.1849439,695.1],[-35.3631891,149.1850231,695.5],[-35.3633885,149.185057,695.5],[-35.3635896,149.1850569,695.3],[-35.3637874,149.185037,695.2],[-35.3639863,149.1850073,695.1],[-35.3641839,149.1849765,695.1],[-35.3643828,149.184949,695.1],[-35.3645814,149.1849275,695.1],[-35.3647821,149.1849122,695.1],[-35.3649822,149.184903,695.1],[-35.3651829,149.1848987,695.1],[-35.3653834,149.1848977,695.1],[-35.3655846,149.1848989,695.1],[-35.3657855,149.1849013,695.1],[-35.365986,149.1849044,695.1],[-35.3661872,149.1849073,695.1],[-35.3663871,149.1849096,695.2],[-35.3665882,149.1849117,695.1],[-35.3667883,149.1849131,695.1],[-35.3669897,149.184914,695.2],[-35.3671891,149.1849143,695.1],[-35.3673907,149.1849145,695.1],[-35.3675905,149.1849146,695.2],[-35.3677921,149.1849149,695.2],[-35.3679916,149.1849151,695.2],[-35.368193,149.1849153,695.1],[-35.368393,149.1849156,695.1],[-35.368595,149.1849157,695.1],[-35.368795,149.1849246,695.1],[-35.3689941,149.1849755,695.2],[-35.3691738,149.1850961,695.5],[-35.3693192,149.1852763,695.8],[-35.3694386,149.1854714,695.5],[-35.3695585,149.1856642,695.1],[-35.3696953,149.1858394,695.0],[-35.3698582,149.185985,695.0],[-35.3700438,149.1860839,695.1],[-35.3702441,149.1861256,695.3],[-35.3704456,149.1861051,695.4],[-35.3706356,149.1860249,695.4],[-35.3708043,149.1858925,695.3],[-35.3709442,149.1857177,695.3],[-35.3710513,149.1855108,695.2],[-35.3711223,149.1852829,695.1],[-35.371156,149.1850412,695.0],[-35.3711522,149.1847967,695.1],[-35.3711109,149.1845557,695.1],[-35.371034,149.1843302,695.1],[-35.3709229,149.1841252,695.1],[-35.3707831,149.1839511,695.1],[-35.370617,149.1838113,695.1],[-35.3704339,149.1837148,695.2],[-35.3702368,149.1836629,695.1],[-35.3700377,149.1836601,695.2],[-35.3698397,149.1837048,695.2],[-35.3696542,149.1837948,695.2],[-35.3694852,149.1839287,695.2],[-35.369342,149.1840983,695.2],[-35.3692272,149.1843006,695.2],[-35.3691474,149.1845252,695.2],[-35.3691041,149.1847662,695.2],[-35.3690996,149.1850114,695.2],[-35.3691342,149.1852537,695.2],[-35.3692066,149.1854819,695.2],[-35.3693139,149.1856899,695.2],[-35.3694521,149.1858677,695.2],[-35.3696157,149.1860093,695.2],[-35.3697986,149.1861096,695.1],[-35.3699941,149.1861644,695.2],[-35.3701947,149.186172,695.2],[-35.3703919,149.1861322,695.2],[-35.3705798,149.1860449,695.2],[-35.3707487,149.1859158,695.2],[-35.3708955,149.1857479,695.2],[-35.3710121,149.1855495,695.2],[-35.3710959,149.1853252,695.2],[-35.3711422,149.1850879,695.2],[-35.3711501,149.1848415,695.2],[-35.3711196,149.1846003,695.2],[-35.3710508,149.1843682,695.2],[-35.3709476,149.1841594,695.2],[-35.3708118,149.1839778,695.2],[-35.3706512,149.1838328,695.2],[-35.3704689,149.1837275,695.2],[-35.3702754,149.183668,695.1],[-35.3700746,149.1836557,695.1],[-35.3698772,149.1836912,695.2],[-35.3696876,149.1837737,695.2],[-35.3695161,149.183899,695.1],[-35.3693666,149.1840634,695.1],[-35.3692466,149.1842595,695.1],[-35.3691593,149.1844813,695.1],[-35.3691093,149.1847197,695.1],[-35.3690983,149.1849655,695.2],[-35.3691261,149.1852089,695.2],[-35.3691914,149.1854405,695.2],[-35.3692931,149.185653,695.1],[-35.3694259,149.1858368,695.2],[-35.3695859,149.1859864,695.1],[-35.3697653,149.1860942,695.2],[-35.3699597,149.1861575,695.1],[-35.3701589,149.1861732,695.2],[-35.3703583,149.186141,695.2],[-35.3705469,149.1860623,695.2],[-35.3707215,149.1859388,695.1],[-35.3708713,149.185778,695.2],[-35.3709945,149.1855821,695.1],[-35.3710835,149.1853631,695.2],[-35.3711366,149.1851246,695.2],[-35.3711481,149.1848803,695.1],[-35.3710974,149.1846379,695.6],[-35.3710169,149.1844151,695.3],[-35.3709174,149.1842045,695.1],[-35.3708092,149.1840031,695.0],[-35.3706949,149.1838037,694.9],[-35.3705803,149.1836046,694.9],[-35.3704802,149.1833912,695.0],[-35.3704072,149.1831616,695.1],[-35.3703645,149.1829198,695.1],[-35.3703503,149.1826743,695.2],[-35.3703585,149.1824287,695.1],[-35.3703831,149.1821851,695.1],[-35.3704178,149.1819444,695.1],[-35.3704578,149.1817039,695.1],[-35.3704992,149.1814646,695.1],[-35.3705399,149.1812237,695.1],[-35.3705781,149.1809836,695.1],[-35.3706139,149.1807408,695.1],[-35.3706466,149.1804998,695.1],[-35.3706775,149.1802557,695.2],[-35.3707062,149.1800136,695.1],[-35.3707341,149.1797687,695.1],[-35.3707614,149.1795262,695.1],[-35.3707888,149.179281,695.2],[-35.3708161,149.1790386,695.2],[-35.3708435,149.1787936,695.2],[-35.3708608,149.1785495,695.2],[-35.3708405,149.1783015,695.2],[-35.3707604,149.1780704,695.6],[-35.3706372,149.1778698,695.7],[-35.3705096,149.1776845,695.3],[-35.3703969,149.177484,695.1],[-35.3703182,149.1772584,695.1],[-35.3702865,149.1770119,695.3],[-35.3703121,149.1767649,695.5],[-35.370395,149.1765376,695.6],[-35.3705271,149.1763512,695.6],[-35.370696,149.1762185,695.5],[-35.3708879,149.1761473,695.3],[-35.3710879,149.1761387,695.1],[-35.3712841,149.176191,695.0],[-35.3714637,149.1762972,695.0],[-35.3716196,149.1764522,694.9],[-35.3717419,149.1766459,695.0],[-35.3718257,149.1768702,695.0],[-35.3718649,149.1771096,695.0],[-35.3718585,149.1773557,695.0],[-35.3718064,149.1775916,695.1],[-35.3717096,149.1778086,695.1],[-35.3715754,149.1779894,695.2],[-35.3714083,149.1781277,695.1],[-35.3712218,149.1782126,695.2],[-35.3710219,149.1782415,695.2],[-35.3708242,149.1782121,695.2],[-35.370636,149.1781259,695.2],[-35.3704712,149.1779893,695.2],[-35.3703352,149.1778075,695.2],[-35.3702271,149.1776036,695.1],[-35.3701251,149.1773959,694.9],[-35.3700271,149.1771857,694.8],[-35.3699306,149.1769725,694.8],[-35.3698346,149.1767589,694.8],[-35.3697379,149.1765444,694.8],[-35.3696401,149.1763306,694.9],[-35.3695412,149.1761176,694.9],[-35.3694412,149.1759049,694.9],[-35.3693406,149.1756929,695.0],[-35.3692391,149.1754808,694.9],[-35.3691379,149.1752695,695.0],[-35.3690359,149.175057,695.0],[-35.3689344,149.1748456,695.0],[-35.3688321,149.1746325,695.0],[-35.3687311,149.1744214,695.0],[-35.3686293,149.1742081,695.0],[-35.3685287,149.1739972,695.0],[-35.368427,149.1737837,695.0],[-35.3683262,149.173573,695.0],[-35.3682238,149.1733597,695.0],[-35.3681225,149.1731489,695.0],[-35.3680238,149.1729339,695.0],[-35.3679511,149.1727041,695.0],[-35.3679319,149.1724524,695.3],[-35.3679714,149.1722064,695.7],[-35.3680331,149.1719728,695.3],[-35.3680829,149.1717395,695.0],[-35.3681003,149.1714942,694.9],[-35.3680697,149.171249,695.0],[-35.3679845,149.1710215,695.2],[-35.3678496,149.170835,695.3],[-35.3676762,149.1707045,695.3],[-35.3674812,149.170641,695.3],[-35.3672802,149.1706448,695.1],[-35.3670874,149.1707134,694.9],[-35.3669152,149.1708391,694.8],[-35.3667719,149.1710124,694.7],[-35.3666654,149.1712207,694.7],[-35.3666004,149.1714546,694.7],[-35.3665808,149.1716981,694.8],[-35.366607,149.1719427,694.8],[-35.3666781,149.1721718,694.8],[-35.3667917,149.1723762,694.8],[-35.3669396,149.1725401,694.9],[-35.3671162,149.1726583,694.9],[-35.3673089,149.172722,694.9],[-35.3675108,149.1727287,694.9],[-35.367706,149.1726777,695.0],[-35.3678862,149.1725673,695.1],[-35.3680256,149.1723906,696.3],[-35.3681335,149.1721876,697.7],[-35.3682216,149.1719783,699.8],[-35.368302,149.1717625,702.2],[-35.3683828,149.1715474,704.8],[-35.3684833,149.1713403,707.7],[-35.3686088,149.1711547,710.8],[-35.368757,149.1709939,714.1],[-35.3689205,149.1708604,717.6],[-35.3690936,149.1707493,721.2],[-35.3692725,149.1706541,724.9],[-35.3694549,149.1705683,728.7],[-35.3696392,149.170487,732.6],[-35.3698235,149.1704067,736.6],[-35.3700084,149.1703242,740.5],[-35.3701923,149.1702393,744.7],[-35.3703764,149.1701511,748.7],[-35.3705581,149.1700606,752.8],[-35.3707406,149.1699671,756.8],[-35.3709214,149.1698725,760.9],[-35.371104,149.1697766,764.9],[-35.3712843,149.1696814,769.0],[-35.3714665,149.1695847,773.1],[-35.3716462,149.169489,777.1],[-35.3718283,149.1693918,781.2],[-35.3720085,149.1692955,785.3],[-35.3721907,149.1691984,789.3],[-35.3723757,149.1691188,793.8],[-35.3725731,149.1690906,798.6],[-35.3727679,149.1691422,804.0],[-35.3729437,149.1692579,809.1],[-35.3731005,149.1693967,813.9],[-35.3732549,149.1695364,818.9],[-35.3734185,149.1696599,824.0],[-35.3735986,149.1697618,827.8],[-35.3737953,149.1698249,830.5],[-35.3740024,149.1698363,832.5],[-35.3742069,149.169791,833.9],[-35.3743974,149.1696899,834.8],[-35.3745646,149.1695431,835.3],[-35.374703,149.16936,835.6],[-35.3748113,149.1691491,835.6],[-35.3748881,149.1689212,835.7],[-35.3749344,149.1686793,835.6],[-35.3749495,149.1684332,835.7],[-35.3749318,149.1681856,835.4],[-35.37486,149.1679511,834.8],[-35.3747585,149.1677335,832.5],[-35.374639,149.1675357,829.8],[-35.3745079,149.1673469,826.9],[-35.3743729,149.1671667,823.9],[-35.3742336,149.1669881,820.8],[-35.3740939,149.1668135,817.8],[-35.3739589,149.1666306,814.8],[-35.3738451,149.1664293,811.7],[-35.3737576,149.1662062,808.6],[-35.3736994,149.1659724,805.2],[-35.3736663,149.16573,801.5],[-35.3736538,149.1654878,797.7],[-35.3736555,149.1652444,793.7],[-35.3736655,149.1650026,789.6],[-35.3736791,149.1647593,785.5],[-35.3736928,149.1645165,781.4],[-35.3737048,149.1642732,777.3],[-35.3737142,149.1640299,773.1],[-35.3737208,149.1637869,769.0],[-35.3737246,149.1635434,764.8],[-35.3737264,149.1632999,760.7],[-35.373727,149.1630554,756.5],[-35.3737269,149.1628128,752.4],[-35.3737263,149.1625691,748.2],[-35.3737256,149.1623263,744.0],[-35.3737248,149.1620824,739.9],[-35.3737238,149.1618408,735.7],[-35.3737225,149.1615967,731.5],[-35.3737067,149.161356,727.0],[-35.3736485,149.161119,722.2],[-35.3735325,149.1609143,717.7],[-35.3733757,149.1607561,712.9],[-35.3732118,149.1606329,707.8],[-35.3730481,149.1605147,702.6],[-35.3728935,149.1603826,697.7],[-35.3727522,149.1602195,693.3],[-35.3726406,149.1600217,690.4],[-35.3725621,149.1597987,688.5],[-35.372487,149.159579,686.8],[-35.3723855,149.1593723,685.8],[-35.3722347,149.15921,685.4],[-35.3720347,149.1591442,685.7],[-35.3718323,149.1592089,686.4],[-35.3716781,149.159377,686.5],[-35.3716016,149.1596039,686.2],[-35.3715908,149.1598406,685.1],[-35.371634,149.1600702,684.2],[-35.3717143,149.1602854,683.7],[-35.371824,149.1604855,683.5],[-35.3719578,149.1606634,683.6],[-35.3721153,149.1608145,683.8],[-35.3722918,149.1609278,684.1],[-35.3724854,149.1609968,684.3],[-35.3726851,149.161013,684.5],[-35.3728845,149.1609724,684.7],[-35.3730683,149.1608761,684.8],[-35.3732293,149.1607264,684.9],[-35.3733547,149.1605361,685.0],[-35.3734398,149.160312,685.0],[-35.3734779,149.1600717,685.0],[-35.3734681,149.1598253,685.0],[-35.3734116,149.1595911,685.0],[-35.3733108,149.1593774,685.0],[-35.373173,149.1592005,685.1],[-35.3730043,149.1590673,685.1],[-35.3728161,149.158987,685.1],[-35.3726162,149.1589627,685.1],[-35.3724186,149.1589967,685.1],[-35.372232,149.1590863,685.1],[-35.3720685,149.1592266,685.1],[-35.3719358,149.1594103,685.1],[-35.371842,149.1596274,685.1],[-35.3717922,149.1598653,685.1],[-35.3717894,149.160111,685.1],[-35.3718334,149.1603494,685.2],[-35.3719225,149.1605695,685.1],[-35.3720509,149.1607573,685.2],[-35.3722124,149.1609039,685.1],[-35.3723958,149.1609989,685.2],[-35.3725938,149.161039,685.2],[-35.3727924,149.1610213,685.2],[-35.372984,149.1609462,685.1],[-35.3731542,149.1608194,685.2],[-35.373297,149.1606451,685.1],[-35.3734009,149.1604369,685.2],[-35.373463,149.1602018,685.1],[-35.3734779,149.1599582,685.2],[-35.3734456,149.159715,685.2],[-35.3733683,149.1594903,685.2],[-35.3732487,149.1592919,685.1],[-35.373096,149.1591346,685.2],[-35.3729104,149.1590362,685.3],[-35.3727135,149.1589975,685.3],[-35.3725163,149.1589913,685.1],[-35.3723205,149.1590032,684.9],[-35.3721239,149.1590241,684.9],[-35.3719261,149.1590316,684.9],[-35.3717275,149.159007,685.0],[-35.3715336,149.1589453,685.1],[-35.3713488,149.1588502,685.2],[-35.3711753,149.1587288,685.2],[-35.3710124,149.1585897,685.2],[-35.3708562,149.1584383,685.1],[-35.3707045,149.1582811,685.1],[-35.3705527,149.1581202,685.1],[-35.3704017,149.1579605,685.1],[-35.3702482,149.1578011,685.1],[-35.3700945,149.1576458,685.1],[-35.3699371,149.1574914,685.1],[-35.3697799,149.1573409,685.2],[-35.3696199,149.1571905,685.1],[-35.369461,149.1570431,685.1],[-35.3692996,149.1568947,685.1],[-35.3691396,149.1567482,685.1],[-35.3689782,149.1566,685.1],[-35.3688182,149.156453,685.1],[-35.3686571,149.156305,685.2],[-35.3684974,149.156158,685.2],[-35.3683366,149.1560093,685.2],[-35.3681772,149.1558615,685.2],[-35.3680171,149.1557129,685.2],[-35.3678576,149.1555649,685.2],[-35.3676969,149.1554165,685.2],[-35.3675038,149.1553337,685.4],[-35.3672952,149.1553844,687.0],[-35.3671187,149.1555144,686.9],[-35.3669975,149.1557102,686.5],[-35.3669298,149.1559368,685.7],[-35.3669031,149.1561728,685.0],[-35.3668967,149.1564121,684.5],[-35.3668935,149.1566518,684.3],[-35.3668829,149.1568953,684.3],[-35.3668604,149.1571378,684.4],[-35.3668247,149.1573804,684.6],[-35.366778,149.1576175,684.7],[-35.3667219,149.1578539,684.8],[-35.3666599,149.1580861,684.9],[-35.3665935,149.1583194,684.9],[-35.3665259,149.1585486,685.0],[-35.3664571,149.1587802,685.0],[-35.3663894,149.1590097,685.0],[-35.3663216,149.1592424,685.1],[-35.3662556,149.1594735,685.1],[-35.3661901,149.1597067,685.1],[-35.3661262,149.1599379,685.1],[-35.3660624,149.1601718,685.2],[-35.3659995,149.1604044,685.1],[-35.3659366,149.1606384,685.1],[-35.365874,149.1608715,685.1],[-35.3658113,149.1611051,685.2],[-35.3657483,149.1613381,685.2],[-35.3657206,149.1615838,685.0],[-35.3658169,149.1618176,686.9],[-35.3659775,149.1619843,687.4],[-35.3661755,149.1620462,687.3],[-35.3663712,149.1620084,686.7],[-35.3665424,149.1618939,685.7],[-35.3666879,149.1617407,684.8],[-35.3668249,149.1615731,684.2],[-35.3669672,149.1614096,684.0],[-35.3671239,149.1612575,684.1],[-35.3672912,149.1611254,684.3],[-35.3674698,149.1610117,684.4],[-35.3676526,149.1609164,684.5],[-35.3678419,149.1608329,684.6],[-35.3680314,149.1607591,684.7],[-35.3682243,149.1606892,684.8],[-35.3684154,149.1606219,684.8],[-35.3686082,149.1605538,684.9],[-35.3687994,149.1604847,685.0],[-35.3689918,149.1604129,685.0],[-35.3691823,149.1603397,685.1],[-35.3693734,149.1602644,685.1],[-35.3695629,149.1601883,685.1],[-35.3697534,149.1601104,685.1],[-35.3699432,149.1600322,685.1],[-35.3701333,149.1599534,685.1],[-35.3703227,149.1598744,685.1],[-35.3705129,149.159795,685.2],[-35.3707028,149.1597153,685.2],[-35.3708685,149.1595712,685.3],[-35.3709492,149.1593283,687.1],[-35.370955,149.1590749,687.1],[-35.3708777,149.158846,686.8],[-35.3707464,149.1586675,686.1],[-35.3705836,149.1585369,685.2],[-35.3704098,149.1584357,684.6],[-35.3702304,149.1583383,684.2],[-35.3700537,149.1582318,684.2],[-35.3698806,149.1581078,684.3],[-35.3697173,149.1579686,684.4],[-35.369561,149.1578136,684.5],[-35.3694137,149.1576494,684.7],[-35.3692705,149.1574762,684.8],[-35.3691327,149.1573004,684.8],[-35.3689956,149.1571204,684.9],[-35.3688602,149.1569417,685.0],[-35.3687227,149.1567616,685.0],[-35.3685856,149.156584,685.0],[-35.3684465,149.1564064,685.1],[-35.368307,149.1562312,685.1],[-35.368166,149.1560562,685.1],[-35.3680247,149.1558823,685.1],[-35.3678824,149.1557085,685.1],[-35.3677401,149.1555352,685.1],[-35.3675868,149.1553767,685.0],[-35.3673788,149.1553404,686.6],[-35.3671751,149.1554077,687.1],[-35.367016,149.1555649,687.1],[-35.3669179,149.1557752,686.5],[-35.3668755,149.1560085,685.6],[-35.3668698,149.1562454,684.8],[-35.3668775,149.1564846,684.3],[-35.3668824,149.1567246,684.2],[-35.3668755,149.1569694,684.2],[-35.3668533,149.1572115,684.4],[-35.3668157,149.157454,684.5],[-35.3667663,149.1576905,684.6],[-35.366707,149.1579263,684.7],[-35.3666423,149.1581572,684.8],[-35.3665733,149.1583894,684.8],[-35.3665038,149.1586188,684.9],[-35.366434,149.1588504,685.0],[-35.3663657,149.1590801,685.0],[-35.3662978,149.159312,685.0],[-35.366232,149.1595431,685.1],[-35.366167,149.1597763,685.1],[-35.3661033,149.1600087,685.1],[-35.36604,149.160242,685.2],[-35.3659775,149.1604745,685.1],[-35.3659149,149.1607085,685.2],[-35.3658527,149.1609422,685.1],[-35.3657905,149.1611757,685.1],[-35.3657295,149.1614097,685.1],[-35.3657384,149.1616598,685.5],[-35.3658592,149.1618778,687.1],[-35.3660349,149.1620143,687.4],[-35.3662365,149.1620432,687.2],[-35.3664252,149.1619792,686.4],[-35.3665878,149.1618495,685.4],[-35.3667285,149.1616902,684.6],[-35.3668668,149.1615223,684.1],[-35.3670127,149.1613629,684.0],[-35.3671734,149.1612171,684.1],[-35.3673438,149.1610915,684.3],[-35.3675248,149.1609835,684.4],[-35.3677093,149.1608923,684.6],[-35.3678988,149.1608119,684.7],[-35.3680889,149.1607397,684.8],[-35.368282,149.1606709,684.8],[-35.3684739,149.1606039,684.9],[-35.368667,149.1605356,685.0],[-35.3688579,149.1604661,685.0],[-35.36905,149.1603936,685.0],[-35.3692403,149.1603195,685.1],[-35.3694309,149.1602432,685.1],[-35.3696204,149.160166,685.1],[-35.3698105,149.1600876,685.1],[-35.3700005,149.1600086,685.1],[-35.3701905,149.1599294,685.2],[-35.3703805,149.1598504,685.2],[-35.3705698,149.159772,685.2],[-35.3707587,149.1596891,685.2],[-35.3709077,149.1595155,685.8],[-35.3709684,149.1592628,687.2],[-35.3709384,149.1590111,687.5],[-35.3708176,149.1588093,687.4],[-35.3706384,149.1587024,687.0],[-35.3704416,149.1586914,686.0],[-35.3702561,149.158747,684.9],[-35.3700796,149.1588436,684.1],[-35.3699104,149.1589587,683.8],[-35.3697331,149.159068,683.8],[-35.3695462,149.1591505,683.9],[-35.3693484,149.1592004,684.2],[-35.3691492,149.159217,684.3],[-35.368948,149.1592063,684.5],[-35.3687508,149.159175,684.6],[-35.3685538,149.1591301,684.6],[-35.3683597,149.1590783,684.8],[-35.3681646,149.1590233,684.8],[-35.3679701,149.1589686,684.9],[-35.3677741,149.1589154,685.0],[-35.3675786,149.1588656,685.0],[-35.3673822,149.158819,685.1],[-35.3671857,149.1587753,685.1],[-35.3669884,149.1587336,685.1],[-35.3667909,149.1586934,685.1],[-35.3665935,149.1586542,685.2],[-35.3663957,149.1586155,685.2],[-35.3661984,149.1585769,685.2],[-35.3659998,149.1585381,685.2],[-35.3658024,149.1584992,685.1],[-35.3656036,149.1584595,685.1],[-35.3654069,149.1584201,685.2],[-35.3652081,149.1583801,685.2],[-35.365011,149.1583402,685.2],[-35.3648121,149.1582993,685.2],[-35.3646155,149.1582588,685.2],[-35.3644163,149.1582179,685.2],[-35.3642192,149.1581779,685.2],[-35.3640205,149.1581375,685.2],[-35.3638237,149.1580974,685.2],[-35.3636251,149.1580572,685.2],[-35.3634281,149.1580174,685.1],[-35.3632294,149.1579772,685.2],[-35.3630323,149.1579373,685.2],[-35.3628341,149.157897,685.2],[-35.3626367,149.1578571,685.2],[-35.362438,149.1578172,685.2],[-35.3622405,149.1577771,685.1],[-35.3620425,149.1577368,685.2],[-35.3618451,149.1576965,685.2],[-35.3616476,149.1576561,685.2],[-35.3614495,149.1576157,685.1],[-35.3612518,149.157575,685.2],[-35.3610533,149.1575344,685.2],[-35.3608561,149.1574942,685.2],[-35.3606578,149.1574537,685.2],[-35.3604606,149.1574135,685.2],[-35.360261,149.1573834,684.5],[-35.3600581,149.1574181,682.8],[-35.3598655,149.1575132,680.5],[-35.3597001,149.1576566,679.0],[-35.3595539,149.1578241,677.7],[-35.3594209,149.1579999,676.7],[-35.3592892,149.1581811,676.0],[-35.3591566,149.1583589,675.6],[-35.3590176,149.1585346,675.3],[-35.3588747,149.158703,675.1],[-35.3587256,149.1588673,675.0],[-35.3585738,149.1590251,675.0],[-35.3584176,149.1591808,675.0],[-35.3582612,149.1593327,674.9],[-35.3581028,149.1594841,675.0],[-35.3579447,149.1596344,675.0],[-35.3577863,149.1597851,675.0],[-35.3576286,149.1599361,675.0],[-35.3574707,149.1600886,675.0],[-35.3573133,149.1602415,675.0],[-35.3571565,149.1603945,675.1],[-35.3569998,149.1605479,675.1],[-35.3568433,149.1607014,675.1],[-35.3566867,149.1608555,675.1],[-35.3565311,149.1610089,675.1],[-35.3563748,149.1611632,675.1],[-35.3562191,149.1613167,675.1],[-35.3560618,149.1614716,675.1],[-35.3558908,149.161601,674.9],[-35.3556767,149.1615897,676.8],[-35.355489,149.1614721,677.4],[-35.3553671,149.1612664,677.5],[-35.3553354,149.1610247,677.1],[-35.3553844,149.1607898,676.1],[-35.3554847,149.1605895,675.1],[-35.3556128,149.1604129,674.2],[-35.3557435,149.1602387,673.8],[-35.3558653,149.1600472,673.8],[-35.3559682,149.1598386,674.0],[-35.3560509,149.1596142,674.2],[-35.3561131,149.1593823,674.4],[-35.3561593,149.1591432,674.5],[-35.3561933,149.1589033,674.7],[-35.35622,149.1586613,674.8],[-35.356243,149.1584192,674.8],[-35.3562651,149.1581756,674.9],[-35.356288,149.1579326,674.9],[-35.3563127,149.1576894,675.0],[-35.3563396,149.157446,675.0],[-35.3563685,149.1572042,675.1],[-35.3563996,149.1569612,675.1],[-35.3564317,149.1567192,675.1],[-35.3564651,149.1564759,675.1],[-35.3564991,149.1562349,675.1],[-35.3565333,149.1559918,675.2],[-35.356567,149.1557506,675.2],[-35.3566058,149.1555085,675.4],[-35.3567323,149.1553152,678.1],[-35.3569245,149.1552207,681.7],[-35.3571216,149.1551934,682.8],[-35.3573159,149.1552483,683.5],[-35.3574904,149.1553579,683.7],[-35.3576489,149.1555024,683.7],[-35.3577971,149.1556604,683.8],[-35.357947,149.1558221,684.0],[-35.3581007,149.1559772,684.3],[-35.3582623,149.1561239,684.6],[-35.3584285,149.156259,684.8],[-35.3586009,149.1563849,685.0],[-35.3587764,149.1565017,685.0],[-35.3589552,149.1566129,685.1],[-35.3591351,149.1567197,685.2],[-35.3593157,149.1568244,685.2],[-35.3594966,149.1569284,685.2],[-35.3596778,149.1570327,685.2],[-35.3598588,149.1571384,685.2],[-35.3600335,149.1572581,684.3],[-35.3601521,149.1574746,683.5],[-35.3601894,149.1577327,681.8],[-35.3601258,149.1579764,680.6],[-35.3599991,149.1581627,679.0],[-35.3598352,149.1582884,677.2],[-35.3596563,149.1583693,675.8],[-35.35947,149.158435,674.8],[-35.3592845,149.1585059,674.4],[-35.3590992,149.1585946,674.3],[-35.3589213,149.1587009,674.3],[-35.3587483,149.1588265,674.4],[-35.3585843,149.1589643,674.5],[-35.3584247,149.1591136,674.6],[-35.358271,149.1592675,674.7],[-35.3581181,149.1594265,674.8],[-35.3579672,149.1595859,674.8],[-35.3578152,149.1597468,674.9],[-35.3576635,149.1599057,674.9],[-35.3575099,149.1600637,675.0],[-35.3573559,149.1602191,675.0],[-35.3571998,149.160374,675.1],[-35.3570438,149.1605273,675.1],[-35.3568872,149.1606803,675.1],[-35.3567305,149.1608328,675.1],[-35.3565733,149.1609856,675.1],[-35.3564162,149.1611384,675.1],[-35.3562596,149.161291,675.2],[-35.3561025,149.1614446,675.2],[-35.3559397,149.1615862,675.0],[-35.3557308,149.1616091,676.4],[-35.3555331,149.1615144,677.3],[-35.3553931,149.1613258,677.5],[-35.3553405,149.1610883,677.2],[-35.3553745,149.1608483,676.5],[-35.3554654,149.1606406,675.4],[-35.3555893,149.1604592,674.4],[-35.355719,149.1602855,673.9],[-35.3558419,149.1600963,673.8],[-35.3559468,149.1598899,673.9],[-35.3560317,149.1596657,674.2],[-35.3560957,149.1594343,674.4],[-35.356143,149.1591955,674.5],[-35.3561778,149.1589561,674.6],[-35.3562052,149.1587127,674.7],[-35.3562285,149.1584704,674.8],[-35.3562507,149.1582266,674.8],[-35.3562733,149.1579839,674.9],[-35.3562979,149.15774,675.0],[-35.3563246,149.1574968,675.0],[-35.3563539,149.1572537,675.1],[-35.356385,149.1570115,675.1],[-35.3564176,149.15677,675.1],[-35.3564515,149.1565281,675.1],[-35.3564859,149.1562864,675.1],[-35.3565205,149.1560437,675.1],[-35.3565549,149.1558023,675.1],[-35.3565895,149.1555597,675.1],[-35.3566833,149.1553388,675.1],[-35.3568704,149.1552095,676.9],[-35.3570753,149.1551743,676.9],[-35.3572703,149.1552439,676.4],[-35.3574269,149.1553875,675.3],[-35.3575454,149.1555777,674.0],[-35.3576378,149.155785,672.8],[-35.3577263,149.1559998,671.9],[-35.3578228,149.1562094,671.4],[-35.3579347,149.1564133,670.9],[-35.3580598,149.156603,670.6],[-35.3581979,149.1567818,670.1],[-35.3583433,149.156948,669.6],[-35.3584957,149.1571072,669.2],[-35.3586509,149.1572603,668.6],[-35.358809,149.1574119,668.1],[-35.358967,149.1575618,667.5],[-35.3591252,149.1577127,667.0],[-35.3592821,149.1578642,666.4],[-35.3594385,149.1580178,665.8],[-35.3595936,149.1581727,665.2],[-35.3597477,149.1583289,664.6],[-35.3599014,149.1584862,664.0],[-35.3600542,149.1586438,663.4],[-35.3602077,149.1588028,662.8],[-35.3603603,149.1589611,662.2],[-35.3605139,149.1591206,661.6],[-35.3606659,149.1592783,661.0],[-35.3608196,149.1594373,660.4],[-35.3609724,149.1595946,659.8],[-35.3611266,149.1597535,659.1],[-35.3612793,149.1599105,658.5],[-35.3614339,149.1600692,658.0],[-35.3615867,149.1602258,657.4],[-35.3617412,149.1603842,656.8],[-35.3618941,149.1605407,656.1],[-35.3620484,149.1606987,655.5],[-35.3622009,149.1608554,654.9],[-35.3623551,149.1610137,654.3],[-35.3625083,149.161171,653.7],[-35.3626623,149.161329,653.1],[-35.3628151,149.161486,652.5],[-35.3629692,149.1616444,651.8],[-35.3631222,149.1618018,651.2],[-35.3632757,149.1619598,650.6],[-35.3634287,149.1621169,650.0],[-35.3635822,149.1622745,649.4],[-35.3637359,149.1624324,648.8],[-35.3638896,149.16259,648.2],[-35.3640435,149.1627476,647.5],[-35.3641968,149.1629045,647.0],[-35.3643506,149.1630622,646.4],[-35.3645039,149.1632193,645.8],[-35.3646579,149.1633773,645.1],[-35.3648107,149.1635345,644.5],[-35.3649647,149.1636929,643.9],[-35.3651174,149.1638504,643.3],[-35.3652718,149.1640097,642.6],[-35.3653918,149.164208,641.9],[-35.3654417,149.1644567,642.0],[-35.3654408,149.1647034,640.8],[-35.3653921,149.1649423,638.9],[-35.3653108,149.1651623,636.7],[-35.3652094,149.1653686,633.5],[-35.3650683,149.1655252,628.5],[-35.3648689,149.1656247,623.9],[-35.3646454,149.1656466,620.5],[-35.3644306,149.1656273,615.7],[-35.3642223,149.1655746,610.9],[-35.3640292,149.1655118,604.9]],"source":"ArduPilot autotest Plane.MissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot bcb5e0c597, from ArduPilot PR 34424): sitl-plane-mission-items"} \ No newline at end of file diff --git a/tests/missions/sitl-plane-rtl-rally.json b/tests/missions/sitl-plane-rtl-rally.json new file mode 100644 index 0000000000..9992bc1b62 --- /dev/null +++ b/tests/missions/sitl-plane-rtl-rally.json @@ -0,0 +1 @@ +{"home":[-35.3629381,149.165085,585.0999755859375],"items":[[22,-35.361279,149.16423,625.0999755859375,[15.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.0999755859375,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.0999755859375,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[189,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]],[16,-35.365467,149.164215,640.0999755859375,[0.0,0.0,0.0,0.0]],[16,-35.365009,149.165482,625.0999755859375,[0.0,0.0,0.0,0.0]],[21,-35.363041,149.165222,585.0999755859375,[0.0,0.0,0.0,1.0]]],"takeoff_course":354.0334197600823,"start":[-35.3629381,149.165085,585.0599975585938],"rally":[[-35.3683278,149.1716947,665.0999755859375]],"params":{"AIRSPEED_CRUISE":22.0,"AIRSPEED_MAX":30.0,"AIRSPEED_MIN":10.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":15.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":0.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.25,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":100.0,"RTL_RADIUS":150.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":80.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":50.0},"flown":[[-35.3629381,149.165085,585.0],[-35.3629306,149.1650841,585.0],[-35.3628442,149.165073,584.8],[-35.3626795,149.1650518,589.3],[-35.3624628,149.1650237,597.1],[-35.3622193,149.1649921,606.6],[-35.3619649,149.1649589,617.0],[-35.3617015,149.164924,627.2],[-35.3614287,149.164892,634.3],[-35.3611572,149.1648728,637.8],[-35.3609061,149.1648635,638.6],[-35.3606756,149.1648614,637.1],[-35.3604506,149.1648652,636.4],[-35.3602189,149.1648729,637.4],[-35.3599891,149.1648818,639.2],[-35.3597573,149.1648916,641.2],[-35.3595293,149.1649019,643.3],[-35.3592996,149.164912,645.5],[-35.3590734,149.1649225,647.7],[-35.3588461,149.164933,649.9],[-35.3586217,149.1649433,652.0],[-35.3583958,149.1649539,654.2],[-35.3581727,149.164964,656.3],[-35.357949,149.1649742,658.5],[-35.3577269,149.1649845,660.6],[-35.3575046,149.1649952,662.7],[-35.3572835,149.1650063,664.8],[-35.357062,149.1650178,666.9],[-35.356841,149.1650292,669.0],[-35.3566208,149.1650405,671.1],[-35.3564008,149.1650516,673.1],[-35.3561811,149.1650625,675.2],[-35.3559619,149.1650917,677.5],[-35.3557729,149.1652442,681.6],[-35.3556217,149.1654525,683.0],[-35.3555335,149.1657049,683.8],[-35.3555041,149.1659782,683.8],[-35.3555201,149.1662483,683.7],[-35.3555627,149.1665154,683.7],[-35.3556142,149.1667768,683.8],[-35.3556644,149.1670422,684.1],[-35.3557059,149.1673068,684.3],[-35.3557371,149.1675764,684.6],[-35.3557573,149.1678437,684.7],[-35.3557688,149.1681142,684.8],[-35.3557731,149.1683829,684.9],[-35.3557726,149.168654,685.0],[-35.3557691,149.168923,685.0],[-35.3557641,149.1691937,685.0],[-35.3557588,149.1694627,685.0],[-35.3557543,149.169734,685.0],[-35.3557508,149.1700035,685.0],[-35.3557485,149.1702739,685.1],[-35.3557471,149.1705433,685.1],[-35.3557465,149.1708141,685.1],[-35.3557463,149.1710842,685.1],[-35.3557466,149.1713543,685.1],[-35.3557475,149.1716243,685.0],[-35.3557485,149.1718942,685.1],[-35.3557492,149.1721651,685.1],[-35.3557496,149.1724349,685.0],[-35.3557502,149.1727063,685.0],[-35.3557506,149.1729755,685.0],[-35.3557508,149.1732469,685.0],[-35.3557509,149.1735163,685.0],[-35.3557509,149.1737881,685.0],[-35.3557509,149.1740572,685.0],[-35.3557509,149.1743289,685.0],[-35.3557509,149.1745979,685.0],[-35.3557508,149.1748702,685.0],[-35.3557506,149.1751396,685.0],[-35.3557506,149.1754111,685.0],[-35.3557507,149.1756799,685.0],[-35.3557507,149.1759517,685.0],[-35.3557506,149.1762213,685.0],[-35.3557506,149.1764928,685.0],[-35.3557506,149.176762,685.0],[-35.3557505,149.1770335,685.1],[-35.3557503,149.177303,685.0],[-35.3557503,149.177574,685.0],[-35.3557503,149.1778442,685.0],[-35.3557503,149.1781147,685.0],[-35.3557504,149.1783851,685.0],[-35.3557503,149.1786558,685.0],[-35.3557503,149.1789265,685.0],[-35.3557503,149.1791963,685.0],[-35.3557504,149.1794673,685.0],[-35.3557508,149.1797376,685.0],[-35.3557512,149.1800091,685.0],[-35.3557515,149.1802783,685.0],[-35.3557516,149.1805493,685.1],[-35.3557515,149.1808185,685.0],[-35.3557512,149.1810907,685.0],[-35.3557686,149.1813598,684.8],[-35.3559111,149.181586,686.6],[-35.3561164,149.18172,687.6],[-35.3563391,149.1817639,686.6],[-35.3565541,149.1817256,685.0],[-35.3567584,149.1816308,683.8],[-35.3569503,149.1815071,683.1],[-35.3571378,149.1813666,682.8],[-35.3573208,149.1812194,682.7],[-35.3575033,149.1810667,682.7],[-35.3576832,149.1809131,682.6],[-35.3578634,149.1807577,682.5],[-35.3580425,149.1806029,682.4],[-35.3582226,149.1804469,682.2],[-35.3584022,149.1802916,682.0],[-35.3585822,149.180136,681.8],[-35.358762,149.179981,681.5],[-35.3589427,149.179826,681.3],[-35.3591235,149.1796711,681.0],[-35.3593039,149.1795163,680.8],[-35.3594849,149.1793606,680.5],[-35.3596647,149.1792058,680.2],[-35.3598458,149.1790502,679.9],[-35.3600258,149.1788953,679.6],[-35.3602071,149.1787392,679.3],[-35.3603865,149.1785848,679.0],[-35.3605678,149.1784286,678.7],[-35.3607473,149.1782738,678.5],[-35.3609287,149.1781173,678.1],[-35.3611081,149.1779628,677.8],[-35.3612896,149.1778069,677.5],[-35.3614694,149.1776523,677.2],[-35.3616508,149.1774956,677.0],[-35.3618304,149.1773405,676.6],[-35.3620112,149.1771839,676.3],[-35.3621903,149.1770284,676.0],[-35.362371,149.1768716,675.8],[-35.3625505,149.1767159,675.5],[-35.3627309,149.1765595,675.2],[-35.3629101,149.176404,674.9],[-35.3630901,149.1762479,674.6],[-35.3632703,149.1760921,674.2],[-35.3634508,149.1759354,674.0],[-35.3636309,149.1757788,673.7],[-35.3638107,149.1756229,673.4],[-35.363991,149.1754668,673.1],[-35.3641712,149.1753112,672.8],[-35.364352,149.1751552,672.4],[-35.3645316,149.1749997,672.2],[-35.3647121,149.1748433,671.9],[-35.3648915,149.1746873,671.6],[-35.3650724,149.1745304,671.3],[-35.3652515,149.1743753,671.0],[-35.3654328,149.1742186,670.7],[-35.3656118,149.1740638,670.4],[-35.3657928,149.1739069,670.1],[-35.3659719,149.1737518,669.8],[-35.3661528,149.173595,669.5],[-35.3663315,149.1734396,669.2],[-35.3665123,149.1732825,668.9],[-35.3666914,149.1731269,668.5],[-35.3668795,149.1729828,667.3],[-35.3670901,149.1728914,666.7],[-35.3673182,149.1728869,666.7],[-35.3675342,149.1729607,666.5],[-35.3677339,149.1730713,665.7],[-35.3679271,149.17319,665.1],[-35.368127,149.1732964,664.7],[-35.3683378,149.1733725,664.7],[-35.3685577,149.1734063,664.8],[-35.3687789,149.1733895,664.9],[-35.3689926,149.17332,665.0],[-35.3691911,149.1732018,665.0],[-35.3693672,149.1730411,665.0],[-35.3695183,149.1728444,665.0],[-35.3696399,149.1726207,665.0],[-35.3697318,149.172375,664.9],[-35.3697922,149.1721182,665.0],[-35.3698222,149.1718505,664.9],[-35.369821,149.1715824,664.9],[-35.3697884,149.1713146,665.0],[-35.3697258,149.1710578,664.9],[-35.3696323,149.1708116,665.0],[-35.3695123,149.1705872,665.0],[-35.3693653,149.1703842,665.0],[-35.3691975,149.1702116,665.0],[-35.3690092,149.1700698,665.0],[-35.368808,149.1699643,665.0],[-35.3685942,149.1698955,665.0],[-35.3683762,149.1698663,665.0],[-35.3681558,149.1698771,665.0]],"source":"ArduPilot autotest Plane.MissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot bcb5e0c597, from ArduPilot PR 34424): sitl-plane-rtl-rally"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-rtl-q-rtl-mode-1.json b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-1.json new file mode 100644 index 0000000000..461d0ea851 --- /dev/null +++ b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-1.json @@ -0,0 +1 @@ +{"home":[-35.3629376,149.1650845,585.0499877929688],"items":[[84,-35.3629376,149.1650846,615.0499877929688,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.0499877929688,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.0499877929688,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]]],"heading":131.4656109106957,"start":[-35.3629376,149.1650846,585.0700073242188],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":1.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3624932,149.1662196,618.2],[-35.3622365,149.1661816,618.2],[-35.362006,149.1661368,618.2],[-35.3617507,149.166085,619.1],[-35.3614935,149.166035,620.8],[-35.3612595,149.1659919,622.9],[-35.3610033,149.1659467,625.4],[-35.3607482,149.1659029,628.1],[-35.3605176,149.1658641,630.5],[-35.3602655,149.165823,633.1],[-35.3600339,149.1657855,635.5],[-35.3597822,149.1657444,638.2],[-35.3595297,149.1657027,640.8],[-35.3593015,149.1656647,643.1],[-35.3590491,149.1656233,645.7],[-35.3587985,149.1655825,648.2],[-35.3585475,149.1655417,650.8],[-35.3582958,149.1655008,653.2],[-35.3580678,149.1654637,655.5],[-35.3578156,149.1654225,658.0],[-35.3575655,149.1653815,660.4],[-35.3573373,149.165344,662.7],[-35.3570876,149.1653028,665.1],[-35.3568591,149.1652651,667.3],[-35.3566072,149.1652237,669.8],[-35.3563546,149.1651834,672.3],[-35.356124,149.1651921,675.1],[-35.3558993,149.1653336,679.9],[-35.3557333,149.1655678,683.5],[-35.3556283,149.1658494,684.5],[-35.3555922,149.1661591,684.3],[-35.3556091,149.1664465,684.1],[-35.3556551,149.1667588,684.0],[-35.3557052,149.1670701,684.3],[-35.3557416,149.1673535,684.7],[-35.3557666,149.1676662,685.1],[-35.3557765,149.1679502,685.3],[-35.355777,149.1682627,685.5],[-35.3557711,149.1685748,685.5],[-35.3557637,149.1688583,685.5],[-35.3557563,149.16917,685.5],[-35.3557512,149.1694821,685.5],[-35.355749,149.1697939,685.4],[-35.3557488,149.1701058,685.4],[-35.3557494,149.1703898,685.3],[-35.3557506,149.1707021,685.3],[-35.355752,149.1710145,685.3],[-35.3557531,149.1712989,685.2],[-35.3557539,149.1716116,685.2],[-35.3557541,149.1718961,685.2],[-35.3557538,149.1722068,685.2],[-35.3557527,149.1725466,685.2],[-35.3557518,149.1728311,685.1],[-35.3557511,149.1731447,685.1],[-35.3557506,149.1734301,685.1],[-35.3557506,149.1737437,685.1],[-35.3557508,149.1740554,685.1],[-35.3557513,149.174339,685.1],[-35.3557518,149.1746524,685.1],[-35.355752,149.1749659,685.1],[-35.3557521,149.1752508,685.1],[-35.3557521,149.1755641,685.1],[-35.3557521,149.1758491,685.1],[-35.3557519,149.1761618,685.1],[-35.3557514,149.1764744,685.1],[-35.3557512,149.1767577,685.0],[-35.3557509,149.1770696,685.0],[-35.3557508,149.1773542,685.0],[-35.3557507,149.1776677,685.1],[-35.3557505,149.1779814,685.1],[-35.3557506,149.1782661,685.1],[-35.3557508,149.1785769,685.0],[-35.3557515,149.1788892,685.0],[-35.3557519,149.1791745,685.0],[-35.3557523,149.1794884,685.1],[-35.3557525,149.1797733,685.1],[-35.3557525,149.180086,685.1],[-35.3557522,149.1803986,685.0],[-35.355752,149.1806835,685.0],[-35.3557599,149.1809956,685.0],[-35.3558678,149.1812814,685.8],[-35.3560884,149.1814461,687.7],[-35.3563474,149.181442,689.1],[-35.3565486,149.1812987,689.3],[-35.3566774,149.1810327,688.6],[-35.3567078,149.1807283,686.8],[-35.356684,149.1804528,683.8],[-35.3566236,149.1801529,681.0],[-35.356556,149.179879,679.7],[-35.3564959,149.1795734,679.2],[-35.3564809,149.17926,679.0],[-35.3565134,149.1789794,678.8],[-35.3565935,149.1786853,678.1],[-35.3567075,149.1784081,676.9],[-35.3568428,149.1781436,675.6],[-35.3569866,149.1778859,674.3],[-35.3571176,149.1776518,673.2],[-35.3572569,149.1773927,671.9],[-35.3573901,149.1771291,670.7],[-35.3575069,149.1768846,669.4],[-35.357632,149.1766117,668.1],[-35.3577437,149.1763624,666.9],[-35.3578655,149.1760883,665.5],[-35.3579878,149.1758145,664.1],[-35.3580998,149.1755666,662.9],[-35.358224,149.1752953,661.5],[-35.3583495,149.1750239,660.1],[-35.3584755,149.1747531,658.7],[-35.3586015,149.1744819,657.3],[-35.3587162,149.174235,656.0],[-35.3588421,149.1739636,654.6],[-35.358968,149.1736923,653.3],[-35.3590823,149.1734449,652.0],[-35.3592071,149.173173,650.6],[-35.3593207,149.1729257,649.3],[-35.3594457,149.1726537,647.9],[-35.3595703,149.1723833,646.5],[-35.3596835,149.1721371,645.2],[-35.3598086,149.1718658,643.8],[-35.3599342,149.1715939,642.5],[-35.3600709,149.1712979,641.0],[-35.3601966,149.1710267,639.5],[-35.360311,149.1707798,638.3],[-35.3604366,149.1705081,636.9],[-35.3605622,149.1702358,635.5],[-35.3606874,149.169964,634.1],[-35.3608123,149.1696925,632.7],[-35.3609261,149.1694453,631.4],[-35.3610519,149.1691731,630.0],[-35.3611779,149.1689014,628.6],[-35.3612922,149.1686544,627.3],[-35.3614174,149.1683829,626.0],[-35.3615422,149.1681114,624.6],[-35.3616663,149.1678416,623.1],[-35.361791,149.1675707,621.7],[-35.3619051,149.1673234,620.5],[-35.3620308,149.167051,619.1],[-35.362145,149.1668037,617.9],[-35.3622705,149.1665322,616.5],[-35.3623888,149.1662759,616.6],[-35.3624809,149.1660763,618.9],[-35.3625671,149.1658902,619.9],[-35.3626447,149.1657225,619.1],[-35.3627097,149.1655817,618.7],[-35.3627733,149.1654433,618.9],[-35.362823,149.1653343,619.4],[-35.3628678,149.1652353,620.0],[-35.3629023,149.1651585,619.8],[-35.3629172,149.1651245,616.4],[-35.3629188,149.1651208,614.0],[-35.3629187,149.1651212,612.1],[-35.3629205,149.1651184,609.8],[-35.3629249,149.1651099,607.6]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-rtl-q-rtl-mode-1"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-rtl-q-rtl-mode-2.json b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-2.json new file mode 100644 index 0000000000..dbfe736987 --- /dev/null +++ b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-2.json @@ -0,0 +1 @@ +{"home":[-35.3629371,149.1650848,585.010009765625],"items":[[84,-35.3629371,149.1650848,615.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.010009765625,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]]],"heading":257.1149493194462,"start":[-35.3629371,149.1650848,585.0399780273438],"approach":-153.7,"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":2.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3621436,149.1641285,619.9],[-35.361899,149.1641717,620.5],[-35.3616726,149.1642121,620.3],[-35.3614178,149.1642552,621.2],[-35.3611813,149.1642928,623.0],[-35.3609215,149.1643331,625.6],[-35.360662,149.1643721,628.5],[-35.3604276,149.1644066,631.0],[-35.360171,149.1644441,633.9],[-35.3599392,149.164478,636.4],[-35.359685,149.1645154,639.1],[-35.3594319,149.1645521,641.7],[-35.3592018,149.1645855,644.1],[-35.3589489,149.1646221,646.7],[-35.3586965,149.1646585,649.2],[-35.3584685,149.1646914,651.5],[-35.3582173,149.1647279,654.0],[-35.357988,149.1647614,656.2],[-35.3577355,149.1647983,658.8],[-35.3574829,149.1648344,661.3],[-35.3572542,149.1648672,663.6],[-35.3570036,149.1649031,666.0],[-35.3567517,149.1649392,668.4],[-35.3564995,149.1649755,670.8],[-35.3562254,149.1650244,673.7],[-35.3560148,149.1651392,677.2],[-35.3558316,149.165352,681.3],[-35.3557032,149.1656197,683.5],[-35.3556397,149.1658941,684.1],[-35.3556259,149.1662078,684.4],[-35.3556468,149.1664934,684.4],[-35.3556856,149.1668063,684.5],[-35.3557235,149.1671193,684.9],[-35.3557494,149.1674045,685.2],[-35.3557661,149.1677178,685.4],[-35.3557718,149.1680024,685.5],[-35.355771,149.1683126,685.5],[-35.3557661,149.1686244,685.5],[-35.3557601,149.1689368,685.4],[-35.3557548,149.1692493,685.4],[-35.355751,149.169562,685.4],[-35.3557491,149.1698744,685.4],[-35.3557488,149.1701868,685.3],[-35.3557489,149.1704712,685.3],[-35.3557492,149.1707841,685.3],[-35.3557499,149.1710971,685.3],[-35.3557505,149.1713815,685.2],[-35.3557509,149.1716923,685.2],[-35.355751,149.1719758,685.1],[-35.355751,149.1722892,685.1],[-35.355751,149.1726028,685.1],[-35.3557507,149.172888,685.1],[-35.3557502,149.1732005,685.1],[-35.35575,149.1735137,685.1],[-35.35575,149.1738269,685.1],[-35.35575,149.17414,685.1],[-35.35575,149.1744239,685.0],[-35.3557499,149.1747348,685.0],[-35.35575,149.1750473,685.0],[-35.3557501,149.1753329,685.0],[-35.3557501,149.1756466,685.0],[-35.3557503,149.1759323,685.1],[-35.3557505,149.1762433,685.1],[-35.3557506,149.1765558,685.0],[-35.3557506,149.1768405,685.0],[-35.3557508,149.1771542,685.0],[-35.3557512,149.1774678,685.0],[-35.3557516,149.1777811,685.0],[-35.355752,149.1780922,685.0],[-35.3557519,149.1783762,685.0],[-35.3557517,149.1786885,685.0],[-35.3557511,149.1790015,685.0],[-35.3557505,149.1792856,685.0],[-35.3557501,149.1795991,685.0],[-35.3557503,149.1798814,685.0],[-35.3557507,149.1801957,685.0],[-35.355751,149.1805075,685.0],[-35.355751,149.1807927,685.0],[-35.3557741,149.1811015,685.0],[-35.355916,149.181363,686.3],[-35.3561538,149.1814881,688.3],[-35.3564085,149.181447,689.3],[-35.3566038,149.1812976,688.9],[-35.3567686,149.1810695,686.5],[-35.356895,149.1808059,683.7],[-35.3569931,149.1805495,681.9],[-35.3570964,149.1802612,680.8],[-35.357191,149.1799988,680.3],[-35.3572965,149.1797123,679.6],[-35.3574035,149.1794281,678.8],[-35.3575015,149.1791707,677.8],[-35.3576098,149.1788891,676.7],[-35.3577181,149.1786087,675.4],[-35.3578266,149.1783277,674.1],[-35.3579353,149.1780457,672.8],[-35.3580342,149.1777887,671.6],[-35.3581427,149.1775057,670.3],[-35.358251,149.1772235,668.9],[-35.35835,149.176966,667.6],[-35.3584686,149.1766573,666.1],[-35.3585673,149.1764012,664.9],[-35.3586753,149.1761206,663.5],[-35.3587838,149.175839,662.0],[-35.3588827,149.1755822,660.7],[-35.3589916,149.1752999,659.3],[-35.3590909,149.1750432,658.1],[-35.3592001,149.1747606,656.7],[-35.359309,149.1744782,655.3],[-35.3594082,149.1742214,654.1],[-35.3595171,149.1739391,652.6],[-35.3596262,149.1736572,651.3],[-35.3597257,149.1734003,650.0],[-35.3598349,149.1731186,648.6],[-35.3599338,149.1728636,647.3],[-35.3600425,149.1725827,645.9],[-35.3601514,149.1723011,644.5],[-35.3602509,149.1720442,643.3],[-35.3603602,149.171762,641.9],[-35.3604596,149.171505,640.6],[-35.3605688,149.1712227,639.2],[-35.3606784,149.1709401,637.8],[-35.3607777,149.1706843,636.5],[-35.3608865,149.1704038,635.2],[-35.3609954,149.1701228,633.7],[-35.3610948,149.1698661,632.5],[-35.3612043,149.1695833,631.1],[-35.3613038,149.169326,629.9],[-35.3614127,149.1690442,628.5],[-35.3615211,149.1687635,627.1],[-35.3616195,149.1685082,625.8],[-35.3617287,149.1682255,624.3],[-35.361838,149.1679433,623.0],[-35.3619471,149.1676621,621.6],[-35.3620558,149.1673814,620.3],[-35.3621546,149.1671254,619.0],[-35.3622631,149.1668435,617.6],[-35.3623715,149.1665615,616.2],[-35.3624708,149.1663051,614.9],[-35.3625955,149.1660337,612.4],[-35.3627489,149.1658192,609.4],[-35.3629759,149.1656652,606.6],[-35.3632201,149.1655655,604.4],[-35.3634139,149.1654142,602.8],[-35.3635465,149.1651503,602.4],[-35.363565,149.1648406,602.6],[-35.3634744,149.1645534,602.2],[-35.3632939,149.1643404,601.3],[-35.3630797,149.1642442,600.5],[-35.36283,149.164255,599.8],[-35.3625997,149.1643782,599.3],[-35.3624327,149.1645725,598.9],[-35.3623212,149.1648521,598.9],[-35.3622992,149.1651341,599.0],[-35.3623665,149.1654326,599.3],[-35.3625262,149.1656723,599.4],[-35.3627484,149.1657541,600.5],[-35.3629924,149.1656632,602.2],[-35.3631649,149.1654552,602.7],[-35.3632765,149.1652242,602.0],[-35.3633706,149.1650232,603.2],[-35.3634469,149.1648672,604.0],[-35.363526,149.1647244,604.8],[-35.3635995,149.164612,605.9],[-35.3636581,149.1645348,606.8],[-35.3637111,149.1644745,606.9],[-35.3637375,149.1644481,603.6],[-35.363737,149.1644572,601.1],[-35.3637292,149.164479,600.1],[-35.3637024,149.1645026,599.7],[-35.3636544,149.1645385,600.0],[-35.3635988,149.1645804,600.2],[-35.3635449,149.1646226,600.2],[-35.3634955,149.164662,600.0],[-35.3634547,149.1646946,600.0],[-35.3634132,149.1647271,600.0],[-35.3633735,149.1647575,600.0],[-35.3633372,149.1647847,600.0],[-35.3632966,149.1648147,600.0],[-35.3632588,149.1648427,600.0],[-35.3632164,149.164874,600.0],[-35.3631737,149.1649059,600.0],[-35.3631348,149.164935,600.0],[-35.3630927,149.1649667,600.0],[-35.3630542,149.1649959,600.0],[-35.3630216,149.1650206,600.0],[-35.3629931,149.1650422,600.0],[-35.3629715,149.1650586,600.0],[-35.3629563,149.1650701,600.0]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-rtl-q-rtl-mode-2"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-rtl-q-rtl-mode-3.json b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-3.json new file mode 100644 index 0000000000..a5035c3d16 --- /dev/null +++ b/tests/missions/sitl-quadplane-rtl-q-rtl-mode-3.json @@ -0,0 +1 @@ +{"home":[-35.3629365,149.1650852,585.1199951171875],"items":[[84,-35.3629365,149.1650852,615.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.1816075,685.1199951171875,[0.0,0.0,0.0,0.0]],[20,0.0,0.0,0.0,[0.0,0.0,0.0,0.0]]],"heading":22.350890943519392,"start":[-35.3629365,149.1650852,585.0800170898438],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":3.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":60.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3615629,149.1652298,625.1],[-35.3613209,149.1652224,625.8],[-35.361093,149.1652165,625.5],[-35.3608337,149.1652108,626.5],[-35.3605697,149.1652042,628.7],[-35.360329,149.1651976,631.2],[-35.3600658,149.1651902,634.1],[-35.3598046,149.165183,637.0],[-35.3595471,149.1651764,639.9],[-35.3592911,149.1651703,642.6],[-35.3590583,149.1651646,645.0],[-35.358803,149.1651586,647.7],[-35.3585483,149.1651524,650.3],[-35.3583171,149.1651469,652.6],[-35.3580631,149.1651409,655.2],[-35.357832,149.1651355,657.5],[-35.3575782,149.1651294,660.0],[-35.357324,149.1651232,662.6],[-35.357093,149.1651173,664.9],[-35.3568393,149.1651111,667.4],[-35.3565859,149.1651053,669.9],[-35.3563549,149.1651001,672.2],[-35.3561071,149.1651461,675.4],[-35.3559114,149.1652929,679.7],[-35.355747,149.1655291,683.0],[-35.3556423,149.1658136,684.2],[-35.3556061,149.1660964,684.4],[-35.3556187,149.1664135,684.3],[-35.3556557,149.1666986,684.3],[-35.3557022,149.1670108,684.5],[-35.35574,149.1673234,684.9],[-35.3557622,149.1676077,685.2],[-35.3557736,149.1679182,685.5],[-35.3557747,149.1682302,685.6],[-35.3557708,149.1685136,685.6],[-35.3557642,149.1688253,685.6],[-35.3557583,149.1691085,685.6],[-35.3557531,149.1694211,685.6],[-35.3557501,149.1697332,685.5],[-35.3557494,149.1700172,685.5],[-35.3557499,149.1703293,685.4],[-35.3557512,149.1706419,685.4],[-35.3557529,149.1709542,685.4],[-35.3557541,149.1712649,685.4],[-35.3557546,149.1715487,685.3],[-35.3557546,149.1718612,685.3],[-35.355754,149.1721741,685.3],[-35.3557533,149.1724585,685.3],[-35.3557523,149.1727713,685.3],[-35.3557516,149.1730562,685.2],[-35.355751,149.1733697,685.2],[-35.3557509,149.173712,685.2],[-35.3557508,149.173996,685.2],[-35.3557509,149.1743072,685.2],[-35.3557511,149.1745918,685.2],[-35.3557511,149.1749051,685.2],[-35.3557512,149.1752186,685.2],[-35.3557513,149.1755038,685.2],[-35.3557514,149.1758156,685.1],[-35.3557518,149.1761268,685.1],[-35.355752,149.1764113,685.1],[-35.3557522,149.1767251,685.1],[-35.3557521,149.1770105,685.1],[-35.3557517,149.1773238,685.2],[-35.3557512,149.1776351,685.1],[-35.3557507,149.1779188,685.1],[-35.3557504,149.1782312,685.1],[-35.3557502,149.1785444,685.1],[-35.3557502,149.1788578,685.1],[-35.3557505,149.1791713,685.1],[-35.355751,149.1794563,685.1],[-35.3557516,149.1797697,685.1],[-35.3557519,149.1800834,685.1],[-35.355752,149.1803669,685.1],[-35.355752,149.1806784,685.1],[-35.3557574,149.1809632,685.1],[-35.3558541,149.1812557,685.7],[-35.3560694,149.1814333,687.6],[-35.3563056,149.181448,689.1],[-35.3565389,149.1813234,689.3],[-35.3567208,149.1811157,687.5],[-35.3568604,149.1808644,684.6],[-35.356975,149.1805874,682.5],[-35.3570714,149.1803268,681.6],[-35.357176,149.1800389,681.2],[-35.3572805,149.179754,680.8],[-35.3573767,149.1794954,680.4],[-35.357483,149.1792122,679.8],[-35.3575801,149.1789547,679.2],[-35.3576872,149.1786718,678.4],[-35.3577948,149.178389,677.5],[-35.3578927,149.1781322,676.7],[-35.3580003,149.1778498,675.8],[-35.3581081,149.1775674,674.8],[-35.3582165,149.1772851,673.8],[-35.3583242,149.1770049,672.7],[-35.3584216,149.1767518,671.6],[-35.3585289,149.1764732,670.5],[-35.3586359,149.1761958,669.7],[-35.3587323,149.1759458,669.0],[-35.358837,149.1756743,668.2],[-35.3589312,149.1754303,667.5],[-35.3590342,149.1751631,666.7],[-35.3591363,149.1748983,665.8],[-35.3592284,149.1746597,665.1],[-35.3593287,149.1744002,664.2],[-35.3594184,149.1741683,663.3],[-35.359516,149.1739154,662.4],[-35.359613,149.173664,661.4],[-35.3597008,149.1734364,660.4],[-35.3597966,149.1731874,659.5],[-35.3598917,149.1729406,658.5],[-35.3599775,149.1727181,657.7],[-35.3600711,149.1724754,656.7],[-35.3601556,149.1722563,655.8],[-35.3602478,149.1720175,654.9],[-35.3603394,149.1717809,654.0],[-35.3604221,149.171567,653.2],[-35.3605125,149.1713332,652.3],[-35.3606021,149.1711014,651.4],[-35.3606986,149.1708514,650.5],[-35.360786,149.1706251,649.6],[-35.3608648,149.1704214,648.8],[-35.3609508,149.1701987,648.0],[-35.3610363,149.1699774,647.2],[-35.3611133,149.1697778,646.4],[-35.3611976,149.1695594,645.2],[-35.3612748,149.1693598,643.7],[-35.3613597,149.1691406,641.9],[-35.3614439,149.1689227,639.7],[-35.3615205,149.1687247,637.5],[-35.3616046,149.1685075,635.0],[-35.3616805,149.1683114,632.6],[-35.3617642,149.1680954,629.9],[-35.361847,149.1678817,627.3],[-35.3619227,149.1676865,624.8],[-35.3620061,149.1674714,622.2],[-35.3620896,149.1672565,619.5],[-35.362165,149.1670629,617.1],[-35.362248,149.1668498,614.5],[-35.3623236,149.1666557,612.1],[-35.362407,149.166442,609.5],[-35.3624894,149.1662303,607.3],[-35.3625591,149.1660509,606.9],[-35.3626279,149.1658742,606.5],[-35.3626918,149.1657105,605.5],[-35.3627503,149.1655609,605.3],[-35.3628019,149.1654287,605.5],[-35.3628426,149.1653245,605.8],[-35.3628801,149.1652284,606.0],[-35.3629084,149.1651552,605.0],[-35.3629173,149.1651318,602.3],[-35.3629189,149.1651286,600.6],[-35.36292,149.1651264,600.1],[-35.3629228,149.1651183,600.2],[-35.3629277,149.1651044,600.1]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-rtl-q-rtl-mode-3"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-vtol-land-approach.json b/tests/missions/sitl-quadplane-vtol-land-approach.json new file mode 100644 index 0000000000..0ffb1054de --- /dev/null +++ b/tests/missions/sitl-quadplane-vtol-land-approach.json @@ -0,0 +1 @@ +{"home":[-35.362938,149.165085,584.9500122070312],"items":[[84,-35.362938,149.165085,614.9500122070312,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,684.9500122070312,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.178303,684.9500122070312,[0.0,0.0,0.0,0.0]],[85,-35.362938,149.165085,664.9500122070312,[1.0,0.0,0.0,0.0]]],"heading":355.1348879668895,"start":[-35.362938,149.165085,585.0700073242188],"approach":168.6,"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":150.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.361542,149.1650471,625.4],[-35.3613051,149.1650476,625.9],[-35.36108,149.1650485,625.3],[-35.3608211,149.1650494,626.1],[-35.360556,149.1650503,628.3],[-35.3603147,149.1650512,631.0],[-35.3600517,149.1650524,634.0],[-35.3597901,149.1650534,637.0],[-35.3595534,149.1650547,639.7],[-35.3592946,149.165056,642.5],[-35.3590607,149.1650568,645.0],[-35.3588051,149.1650581,647.6],[-35.3585503,149.1650594,650.3],[-35.3583196,149.1650606,652.6],[-35.358067,149.1650622,655.2],[-35.3578129,149.1650638,657.7],[-35.3575584,149.1650656,660.3],[-35.3573038,149.1650675,662.8],[-35.3570726,149.1650697,665.1],[-35.3568191,149.1650721,667.6],[-35.3565658,149.1650743,670.1],[-35.356335,149.165077,672.4],[-35.3560895,149.1651413,675.7],[-35.3558996,149.1652996,679.9],[-35.3557419,149.1655427,683.0],[-35.3556433,149.1658307,684.0],[-35.3556119,149.1661144,684.3],[-35.3556263,149.1664302,684.3],[-35.355666,149.1667427,684.3],[-35.3557093,149.1670539,684.5],[-35.3557433,149.1673657,684.9],[-35.3557627,149.1676505,685.1],[-35.3557718,149.167963,685.3],[-35.3557717,149.1682758,685.4],[-35.355767,149.1685597,685.4],[-35.3557606,149.1688724,685.4],[-35.3557555,149.1691561,685.4],[-35.3557515,149.1694675,685.4],[-35.355749,149.1697785,685.3],[-35.355748,149.1700623,685.3],[-35.355748,149.1703745,685.2],[-35.3557491,149.1706873,685.2],[-35.3557502,149.1710003,685.2],[-35.355751,149.1713133,685.2],[-35.3557515,149.1715982,685.2],[-35.3557515,149.1719113,685.1],[-35.3557514,149.1722244,685.1],[-35.3557511,149.1725076,685.1],[-35.3557505,149.172819,685.1],[-35.3557501,149.1731035,685.0],[-35.35575,149.1734164,685.0],[-35.3557501,149.1737296,685.0],[-35.35575,149.1740147,685.0],[-35.3557498,149.1743286,685.0],[-35.3557498,149.1746421,685.1],[-35.35575,149.1749529,685.0],[-35.3557503,149.1752644,684.9],[-35.3557505,149.175549,684.9],[-35.3557509,149.1758625,684.9],[-35.3557511,149.1761767,685.0],[-35.3557513,149.1764612,685.0],[-35.3557513,149.176773,685.0],[-35.3557513,149.1770565,685.0],[-35.355751,149.1773977,684.9],[-35.3557586,149.177709,684.8],[-35.3558501,149.1779722,685.2],[-35.3560627,149.1781534,687.1],[-35.3562965,149.1781737,688.6],[-35.3565335,149.1780601,688.7],[-35.3567246,149.1778666,686.6],[-35.3568647,149.1776501,683.9],[-35.3569958,149.1773874,681.9],[-35.3571202,149.1771135,681.2],[-35.3572336,149.1768634,681.2],[-35.3573601,149.1765906,681.3],[-35.3574764,149.1763443,681.3],[-35.3576051,149.1760744,681.2],[-35.3577343,149.1758058,681.1],[-35.3578524,149.1755617,680.9],[-35.3579815,149.175295,680.6],[-35.3580997,149.1750514,680.3],[-35.3582299,149.1747827,680.0],[-35.3583607,149.1745131,679.7],[-35.3584793,149.1742689,679.4],[-35.3586092,149.1740011,679.0],[-35.3587385,149.1737337,678.7],[-35.3588569,149.1734897,678.3],[-35.3589872,149.1732216,677.9],[-35.3591061,149.1729767,677.6],[-35.3592368,149.1727076,677.2],[-35.3593677,149.1724382,676.8],[-35.3594863,149.1721943,676.5],[-35.3596164,149.1719265,676.1],[-35.3597467,149.1716584,675.7],[-35.3598773,149.1713898,675.3],[-35.3600078,149.1711212,674.9],[-35.3601268,149.1708768,674.5],[-35.360258,149.1706081,674.2],[-35.360389,149.1703393,673.8],[-35.360508,149.170095,673.4],[-35.3606381,149.1698279,673.0],[-35.3607568,149.1695844,672.6],[-35.3608881,149.1693156,672.2],[-35.3610194,149.1690464,671.8],[-35.3611384,149.1688014,671.5],[-35.3612683,149.1685335,671.1],[-35.3613983,149.1682652,670.7],[-35.3615286,149.1679965,670.3],[-35.3616593,149.1677276,669.9],[-35.3617782,149.167483,669.6],[-35.3619094,149.1672135,669.2],[-35.3620507,149.1669526,668.0],[-35.3622155,149.1667508,667.1],[-35.362451,149.1666229,667.1],[-35.3626855,149.1666107,667.4],[-35.3629369,149.1666501,666.4],[-35.3631868,149.1666876,664.9],[-35.3634177,149.1666842,664.4],[-35.3636668,149.1666159,664.5],[-35.3638952,149.1664759,664.8],[-35.3641039,149.1662526,665.0],[-35.3642496,149.1659992,664.9],[-35.364343,149.1657402,664.7],[-35.3644031,149.1654366,664.5],[-35.3644201,149.1651244,664.5],[-35.3643958,149.1648134,664.6],[-35.3643234,149.1644871,664.7],[-35.364228,149.1642301,664.7],[-35.3640875,149.1639705,664.7],[-35.3639131,149.163743,664.8],[-35.3637287,149.1635703,664.8],[-35.363503,149.1634242,664.9],[-35.3632831,149.1633353,665.0],[-35.3630331,149.1632883,665.0],[-35.3627792,149.1632954,664.9],[-35.3625517,149.1633484,664.9],[-35.3623129,149.1634567,664.9],[-35.3621116,149.163598,664.9],[-35.3619142,149.1637961,664.9],[-35.361748,149.1640328,664.9],[-35.3616277,149.1642754,664.9],[-35.3615336,149.164565,665.0],[-35.3614841,149.1648707,664.9],[-35.3615363,149.1651483,665.4],[-35.3617286,149.1653583,667.6],[-35.3619568,149.1654249,669.0],[-35.3622055,149.1653979,669.8],[-35.3624082,149.1653306,673.2],[-35.3625447,149.165259,677.3],[-35.3626613,149.1651872,679.9],[-35.3627525,149.1651318,681.4],[-35.3628257,149.1650963,681.7],[-35.3628799,149.1650784,679.9],[-35.3628979,149.165083,678.8],[-35.3629099,149.1650977,679.4],[-35.3629196,149.1651059,679.2],[-35.3629272,149.1651033,679.2]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-vtol-land-approach"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-vtol-land-q-options.json b/tests/missions/sitl-quadplane-vtol-land-q-options.json new file mode 100644 index 0000000000..87b190eafc --- /dev/null +++ b/tests/missions/sitl-quadplane-vtol-land-q-options.json @@ -0,0 +1 @@ +{"home":[-35.3629385,149.1650851,585.1199951171875],"items":[[22,-35.3629385,149.1650851,615.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.1199951171875,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.178303,685.1199951171875,[0.0,0.0,0.0,0.0]],[85,-35.362938,149.165085,645.1199951171875,[0.0,0.0,0.0,0.0]]],"heading":251.93458190675557,"start":[-35.3629385,149.1650851,585.0499877929688],"approach":169.5,"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":-120.0,"Q_OPTIONS":16.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3622274,149.1640704,619.5],[-35.36198,149.1641154,620.0],[-35.3617544,149.1641587,619.8],[-35.3615,149.1642052,620.5],[-35.3612411,149.1642494,622.5],[-35.3610044,149.1642874,624.8],[-35.3607463,149.1643271,627.7],[-35.360513,149.1643624,630.3],[-35.3602568,149.1644009,633.1],[-35.3600012,149.1644394,635.9],[-35.3597695,149.1644739,638.3],[-35.359516,149.1645123,641.0],[-35.3592864,149.1645474,643.4],[-35.3590355,149.1645856,646.0],[-35.3587837,149.1646243,648.5],[-35.3585551,149.1646593,650.8],[-35.3583029,149.164698,653.3],[-35.35805,149.1647368,655.8],[-35.3577987,149.164775,658.3],[-35.3575463,149.1648132,660.8],[-35.3573185,149.1648475,663.0],[-35.3570669,149.1648858,665.4],[-35.3568147,149.1649241,667.9],[-35.3565855,149.164959,670.1],[-35.3563339,149.1649975,672.6],[-35.3561114,149.1650701,675.3],[-35.3559048,149.1652471,679.8],[-35.3557518,149.165492,682.9],[-35.3556622,149.1657546,684.0],[-35.3556241,149.1660663,684.4],[-35.3556354,149.1663815,684.5],[-35.355668,149.1666671,684.5],[-35.3557086,149.1669793,684.7],[-35.3557391,149.1672623,685.1],[-35.3557612,149.1675746,685.3],[-35.3557711,149.1678885,685.5],[-35.3557719,149.1681731,685.6],[-35.3557675,149.1684859,685.6],[-35.3557612,149.1687985,685.6],[-35.3557553,149.1691094,685.5],[-35.3557509,149.1694215,685.5],[-35.3557486,149.1697057,685.5],[-35.355748,149.1700185,685.5],[-35.3557487,149.170332,685.4],[-35.3557498,149.1706162,685.4],[-35.3557512,149.1709287,685.4],[-35.3557522,149.1712128,685.3],[-35.3557528,149.1715253,685.3],[-35.3557529,149.1718379,685.3],[-35.3557527,149.1721226,685.2],[-35.355752,149.1724353,685.2],[-35.3557512,149.1727472,685.2],[-35.3557505,149.1730598,685.2],[-35.35575,149.1733721,685.2],[-35.3557496,149.1736557,685.2],[-35.3557494,149.1739687,685.2],[-35.3557495,149.1742822,685.1],[-35.3557497,149.1745666,685.2],[-35.3557498,149.1748796,685.1],[-35.35575,149.1751646,685.2],[-35.3557503,149.1754767,685.2],[-35.3557506,149.1757889,685.1],[-35.3557507,149.1760742,685.1],[-35.3557507,149.1763869,685.2],[-35.3557509,149.1767004,685.1],[-35.355751,149.1770132,685.1],[-35.3557511,149.1773261,685.1],[-35.3557526,149.1776103,685.0],[-35.3558217,149.1779104,684.9],[-35.3560392,149.17813,686.6],[-35.356297,149.1781563,688.4],[-35.3565325,149.178041,688.5],[-35.356708,149.1778677,686.5],[-35.3568649,149.1776327,683.3],[-35.3569977,149.1773691,681.0],[-35.357112,149.1771193,680.1],[-35.3572371,149.1768445,679.8],[-35.357352,149.1765962,679.5],[-35.3574796,149.1763257,679.2],[-35.3576203,149.1760318,678.6],[-35.3577381,149.1757872,678.1],[-35.3578673,149.1755198,677.4],[-35.3579861,149.1752747,676.8],[-35.358116,149.1750071,676.1],[-35.3582459,149.174739,675.4],[-35.3583645,149.1744945,674.7],[-35.3584944,149.1742258,674.1],[-35.3586244,149.1739575,673.3],[-35.3587425,149.1737143,672.7],[-35.358873,149.1734468,671.8],[-35.358991,149.173204,671.1],[-35.3591215,149.1729349,670.3],[-35.3592512,149.1726672,669.6],[-35.35937,149.1724226,668.8],[-35.3595004,149.1721548,668.0],[-35.3596321,149.1718847,667.3],[-35.3597632,149.1716157,666.5],[-35.3598933,149.1713479,665.7],[-35.3600116,149.171104,665.0],[-35.3601422,149.1708358,664.2],[-35.3602733,149.170567,663.3],[-35.3603928,149.1703225,662.6],[-35.3605239,149.170054,661.9],[-35.360643,149.16981,661.2],[-35.3607737,149.1695419,660.4],[-35.3609041,149.1692738,659.6],[-35.3610231,149.1690294,658.9],[-35.3611536,149.1687613,658.1],[-35.3612844,149.1684923,657.3],[-35.3614151,149.168224,656.5],[-35.3615451,149.1679574,655.8],[-35.361664,149.1677138,655.0],[-35.361795,149.1674455,654.2],[-35.3619258,149.1671775,653.5],[-35.3620446,149.1669331,652.8],[-35.3621667,149.1666593,651.2],[-35.3622411,149.1663898,649.3],[-35.3622593,149.1660748,649.2],[-35.3622813,149.1657921,651.9],[-35.3622867,149.1655806,654.6],[-35.3622843,149.1653779,653.9],[-35.3622775,149.1651827,653.0],[-35.3622692,149.1650129,654.1],[-35.3622616,149.1648723,655.2],[-35.3622561,149.1647707,656.1],[-35.3622516,149.1646871,655.8],[-35.3622525,149.1646636,653.6],[-35.3622691,149.164679,653.8],[-35.3623002,149.1647074,653.9],[-35.3623369,149.1647379,653.9],[-35.3623813,149.1647701,653.9],[-35.3624273,149.1647989,653.9],[-35.3624693,149.1648224,653.9],[-35.3625148,149.1648465,653.8],[-35.3625555,149.1648677,653.8],[-35.3625999,149.1648915,653.9],[-35.3626483,149.1649185,653.8],[-35.3626887,149.1649418,653.9],[-35.3627333,149.1649681,653.9],[-35.3627771,149.164994,653.9],[-35.3628138,149.1650157,653.9],[-35.3628484,149.165036,653.8],[-35.3628761,149.1650519,653.8],[-35.3629017,149.1650663,653.9],[-35.3629178,149.1650751,653.8]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-vtol-land-q-options"} \ No newline at end of file diff --git a/tests/missions/sitl-quadplane-vtol-land.json b/tests/missions/sitl-quadplane-vtol-land.json new file mode 100644 index 0000000000..e0b37eaedb --- /dev/null +++ b/tests/missions/sitl-quadplane-vtol-land.json @@ -0,0 +1 @@ +{"home":[-35.3629381,149.1650858,585.010009765625],"items":[[22,-35.3629381,149.1650858,615.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.165085,685.010009765625,[0.0,0.0,0.0,0.0]],[16,-35.3557514,149.178303,685.010009765625,[0.0,0.0,0.0,0.0]],[85,-35.362938,149.165085,645.010009765625,[0.0,0.0,0.0,0.0]]],"heading":156.55184140685728,"start":[-35.3629381,149.1650858,585.0899658203125],"params":{"AIRSPEED_CRUISE":25.0,"AIRSPEED_MAX":35.0,"AIRSPEED_MIN":13.0,"ALT_SLOPE_MIN":15.0,"CLIMB_SLOPE_HGT":25.0,"NAVL1_DAMPING":0.75,"NAVL1_LIM_BANK":0.0,"NAVL1_PERIOD":14.0,"NAVL1_XTRACK_I":0.019999999552965164,"Q_ENABLE":1.0,"Q_FW_LND_APR_RAD":0.0,"Q_OPTIONS":0.0,"Q_RTL_ALT":15.0,"Q_RTL_MODE":0.0,"Q_TRANS_DECEL":2.0,"RALLY_INCL_HOME":0.0,"RALLY_LIMIT_KM":5.0,"RLL2SRV_TCONST":0.5,"ROLL_LIMIT_DEG":65.0,"RTL_ALTITUDE":20.0,"RTL_RADIUS":0.0,"TECS_CLMB_MAX":5.0,"TECS_SINK_MAX":5.0,"TECS_TIME_CONST":5.0,"WP_LOITER_RAD":60.0,"WP_MAX_RADIUS":0.0,"WP_RADIUS":90.0},"flown":[[-35.3629418,149.1662625,618.1],[-35.3627057,149.1662692,617.5],[-35.362448,149.1662393,615.8],[-35.36219,149.166189,615.6],[-35.3619558,149.1661397,616.7],[-35.3617,149.1660876,618.7],[-35.3614686,149.1660434,620.9],[-35.3612143,149.1659975,623.4],[-35.3609609,149.1659533,626.1],[-35.3607311,149.1659142,628.5],[-35.3604792,149.1658719,631.1],[-35.3602268,149.1658297,633.7],[-35.3599979,149.1657912,636.1],[-35.3597457,149.1657487,638.6],[-35.3595169,149.1657106,641.0],[-35.359267,149.165669,643.5],[-35.3590161,149.165627,646.0],[-35.3587871,149.1655889,648.3],[-35.3585353,149.1655468,650.8],[-35.3583059,149.1655087,653.1],[-35.3580546,149.1654671,655.7],[-35.3578047,149.1654259,658.1],[-35.3575766,149.1653883,660.3],[-35.3573249,149.1653467,662.8],[-35.3570727,149.1653048,665.3],[-35.3568435,149.1652669,667.5],[-35.3565926,149.1652255,670.0],[-35.356365,149.1651879,672.1],[-35.3561123,149.165193,675.1],[-35.3558882,149.1653366,679.9],[-35.3557351,149.1655485,683.2],[-35.3556258,149.1658303,684.5],[-35.3555861,149.166141,684.4],[-35.3556082,149.1664858,684.1],[-35.3556572,149.1667979,684.0],[-35.3557033,149.1670801,684.3],[-35.3557426,149.1673903,684.7],[-35.3557661,149.1677022,685.1],[-35.3557757,149.1680154,685.3],[-35.3557754,149.1683283,685.4],[-35.3557703,149.1686122,685.4],[-35.3557626,149.1689513,685.5],[-35.3557565,149.1692609,685.4],[-35.3557527,149.1695441,685.3],[-35.3557505,149.1698568,685.3],[-35.35575,149.1701416,685.3],[-35.3557506,149.1704548,685.3],[-35.3557518,149.1707659,685.3],[-35.3557527,149.1710489,685.3],[-35.3557534,149.171361,685.2],[-35.3557538,149.1716454,685.2],[-35.3557538,149.1719586,685.1],[-35.3557537,149.1722718,685.2],[-35.3557533,149.1725563,685.1],[-35.3557525,149.1728687,685.1],[-35.3557513,149.173181,685.1],[-35.3557504,149.1734655,685.1],[-35.3557503,149.1737778,685.1],[-35.3557506,149.1740627,685.0],[-35.3557513,149.1743762,685.1],[-35.355752,149.1746902,685.1],[-35.3557524,149.1749749,685.1],[-35.3557524,149.1752876,685.0],[-35.3557521,149.1755987,685.0],[-35.3557515,149.1759112,685.0],[-35.355751,149.1762244,685.0],[-35.3557508,149.1765097,685.0],[-35.3557508,149.1768213,685.0],[-35.3557508,149.1771335,685.0],[-35.3557508,149.1774172,685.0],[-35.3557682,149.1777292,684.8],[-35.3558816,149.1779801,685.6],[-35.3561119,149.1781405,687.1],[-35.3563756,149.1781288,688.2],[-35.3565835,149.1779723,688.3],[-35.356722,149.1776953,687.8],[-35.3567672,149.177379,686.4],[-35.3567689,149.1770659,684.3],[-35.3567757,149.1767546,682.7],[-35.3568117,149.176479,682.3],[-35.3568897,149.1761897,682.4],[-35.3569992,149.1759208,682.5],[-35.3571178,149.1756903,682.7],[-35.3572597,149.175448,682.9],[-35.3573915,149.1752344,683.2],[-35.3575369,149.1749987,683.5],[-35.3576779,149.1747626,683.8],[-35.3578033,149.1745443,684.1],[-35.357936,149.174304,684.3],[-35.3580667,149.1740606,684.4],[-35.3581945,149.1738192,684.5],[-35.3583224,149.1735774,684.6],[-35.3584383,149.1733603,684.7],[-35.3585658,149.173124,684.7],[-35.3587047,149.1728688,684.7],[-35.3588304,149.1726393,684.7],[-35.3589561,149.1724097,684.8],[-35.3590688,149.1722034,684.8],[-35.3591922,149.1719771,684.8],[-35.3593135,149.171754,684.8],[-35.3594235,149.1715513,684.7],[-35.3595426,149.1713319,684.8],[-35.3596508,149.1711327,684.8],[-35.3597682,149.1709165,684.7],[-35.3598947,149.1706833,684.7],[-35.3599994,149.1704909,684.7],[-35.3601137,149.1702815,684.7],[-35.3602169,149.1700924,684.7],[-35.3603291,149.1698862,684.7],[-35.3604408,149.1696806,684.7],[-35.3605412,149.1694959,684.7],[-35.3606505,149.1692943,684.7],[-35.3607594,149.1690938,684.8],[-35.3608583,149.1689113,684.8],[-35.3609669,149.1687109,684.9],[-35.3610657,149.1685289,684.9],[-35.361173,149.1683324,684.9],[-35.3612802,149.1681363,684.9],[-35.3613783,149.1679567,685.0],[-35.361485,149.1677606,685.0],[-35.3615922,149.1675634,685.0],[-35.361699,149.1673666,685.1],[-35.3618053,149.1671704,685.1],[-35.3619023,149.1669911,685.2],[-35.3620079,149.1667962,685.2],[-35.3621149,149.1665993,685.2],[-35.362212,149.1664209,685.2],[-35.3623196,149.1662236,685.2],[-35.3624136,149.166051,685.2],[-35.3625108,149.1658722,685.5],[-35.3625955,149.1657161,685.8],[-35.3626656,149.165587,685.6],[-35.3627346,149.16546,685.5],[-35.362796,149.1653468,685.6],[-35.3628475,149.1652516,685.9],[-35.3628889,149.1651747,685.8],[-35.3629077,149.1651392,684.5],[-35.3629118,149.165132,685.2],[-35.3629154,149.1651261,685.2],[-35.3629202,149.1651168,685.2],[-35.3629271,149.1651029,685.3]],"source":"ArduPilot autotest QuadPlane.VTOLMissionItemTypes (SITL, ArduPlane V4.8.0-dev, ardupilot 48c91a4c9e, from ArduPilot PR 34424): sitl-quadplane-vtol-land"} \ No newline at end of file diff --git a/tests/test_map3d_mission_labels.py b/tests/test_map3d_mission_labels.py new file mode 100644 index 0000000000..2851aa6b42 --- /dev/null +++ b/tests/test_map3d_mission_labels.py @@ -0,0 +1,309 @@ +'''the 3D map labelling each mission item, as the 2D map does''' + +import pytest + +from pymavlink import mavutil + +mavlink = mavutil.mavlink + +HOME = (-35.363262, 149.165238, 584.0) + + +def items(): + '''home, a waypoint, a loiter and a land start, as the map is given them''' + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + return [ + MissionItem(HOME[0], HOME[1], HOME[2], 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 0), + MissionItem(HOME[0] + 0.01, HOME[1], HOME[2] + 100, 0, + mavlink.MAV_CMD_NAV_LOITER_TURNS, 1), + MissionItem(HOME[0] + 0.02, HOME[1] + 0.01, HOME[2] + 100, 0, + mavlink.MAV_CMD_DO_LAND_START, 2), + ] + + +LABELS = ['0', '1(LT)', '2(DLS)'] + + +def elements(): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + return em + + +def labels(em): + '''the text and position of each number drawn''' + import vtk + return [(a.GetInput(), a.GetPosition()) + for a in em.actors.get('mission', []) + if isinstance(a, vtk.vtkBillboardTextActor3D)] + + +def label_sizes(em): + '''how big each of those is drawn''' + import vtk + return [a.GetTextProperty().GetFontSize() + for a in em.actors.get('mission', []) + if isinstance(a, vtk.vtkBillboardTextActor3D)] + + +def recording_map(pushed): + '''a map which notes the label settings it is sent''' + from types import SimpleNamespace + return SimpleNamespace( + is_alive=lambda: True, set_fpv_fov=lambda fov: None, + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: pushed.append(('labels', enable)), + set_mission_label_size=lambda size: pushed.append(('size', size)), + set_mission_style=lambda style: None, + set_render_settings=lambda *args: None) + + +class TestMissionLabels(object): + + def test_the_labels_are_drawn_only_when_they_are_asked_for(self): + em = elements() + em.set_mission(items()) + assert labels(em) == [] + em.set_mission_labels(True) + drawn = labels(em) + assert [text for (text, _) in drawn] == LABELS + # each beside the item it numbers + for ((_, at), marker) in zip(drawn, em.mission_markers): + assert at == pytest.approx(marker) + # and they go again when they are not wanted + em.set_mission_labels(False) + assert labels(em) == [] + + def test_the_labels_are_drawn_the_size_they_are_asked_for(self): + from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_LABEL_SIZES) + em = elements() + em.set_mission(items()) + em.set_mission_labels(True) + assert label_sizes(em) == [MISSION_LABEL_SIZE] * len(LABELS) + em.set_mission_label_size(24) + assert label_sizes(em) == [24] * len(LABELS) + # and only to a size there is some reading + (smallest, largest) = MISSION_LABEL_SIZES + em.set_mission_label_size(smallest - 1) + assert label_sizes(em) == [smallest] * len(LABELS) + em.set_mission_label_size(largest + 1) + assert label_sizes(em) == [largest] * len(LABELS) + # a size set while they are not drawn is the size they come back at + em.set_mission_labels(False) + em.set_mission_label_size(18) + em.set_mission_labels(True) + assert label_sizes(em) == [18] * len(LABELS) + + def test_a_mission_drawn_again_keeps_its_labels(self): + em = elements() + em.set_mission_labels(True) + em.set_mission(items()) + assert [text for (text, _) in labels(em)] == LABELS + # an item with no position of its own is not drawn, so it is not + # labelled either, and the numbers stay those of the items + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + mission = items() + mission[1] = MissionItem(0, 0, 0, 3, mavlink.MAV_CMD_DO_CHANGE_SPEED, 1) + em.set_mission(mission) + assert [text for (text, _) in labels(em)] == ['0', '2(DLS)'] + + def test_the_labels_are_the_ones_the_2d_map_uses(self): + """both maps call an item the same thing""" + from MAVProxy.modules.lib import mp_util + assert mp_util.mission_item_label( + 3, mavlink.MAV_CMD_NAV_WAYPOINT) == '3' + for (command, suffix) in ( + (mavlink.MAV_CMD_DO_LAND_START, 'DLS'), + (mavlink.MAV_CMD_NAV_TAKEOFF, 'TOff'), + (mavlink.MAV_CMD_NAV_LOITER_TIME, 'LTime'), + (mavlink.MAV_CMD_NAV_VTOL_LAND, 'VL'), + (mp_util.MAV_CMD_NAV_ARC_WAYPOINT, 'AW'), + (mp_util.MAV_CMD_DO_ORBIT, 'Orbit')): + assert mp_util.mission_item_label( + 4, command) == '4(%s)' % suffix + # and the 2D map's own labelling is the same labelling + pytest.importorskip("wx") + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_map + module = mavproxy_map.MapModule.__new__(mavproxy_map.MapModule) + wp = SimpleNamespace(command=mavlink.MAV_CMD_DO_LAND_START) + module.mpstate = SimpleNamespace( + module=lambda name: (SimpleNamespace( + wploader=SimpleNamespace(wp=lambda i: wp)) + if name == 'wp' else None)) + assert module.label_for_waypoint(2) == '2(DLS)' + + def test_the_viewer_is_told_to_label_the_mission(self, map3d_frame): + pytest.importorskip("vtk") + pytest.importorskip("wx") + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.map3d import Map3D + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + sent = [] + viewer = Map3D.__new__(Map3D) + viewer.child = SimpleNamespace(is_alive=lambda: True) + viewer.object_queue = SimpleNamespace(put=sent.append) + em = elements() + frame = map3d_frame(em) + viewer.set_mission(items()) + viewer.set_mission_labels(True) + viewer.set_mission_label_size(22) + for msg in sent: + Map3DFrame.handle(frame, msg) + assert [text for (text, _) in labels(em)] == LABELS + assert label_sizes(em) == [22] * len(LABELS) + # and the checkbox shows what the map is doing + assert frame.labels_check.GetValue() is True + + def test_the_live_map_settings(self, map3d_module): + from MAVProxy.modules.mavproxy_map3d.map3d import ( + MISSION_LABEL_SIZE, MISSION_LABEL_SIZES) + pushed = [] + module = map3d_module(recording_map(pushed)) + # off unless it is asked for, since a mission of any size is busy + assert module.map3d_settings.showlabels is False + assert module.map3d_settings.labelsize == MISSION_LABEL_SIZE + module.cmd_map3d(['set', 'showlabels', 'true']) + assert ('labels', True) in pushed + assert module.map3d_settings.showlabels is True + module.cmd_map3d(['set', 'labelsize', '20']) + assert ('size', 20) in pushed + assert module.map3d_settings.labelsize == 20 + # and a size there is no reading is not taken + module.cmd_map3d(['set', 'labelsize', + str(MISSION_LABEL_SIZES[1] + 1)]) + assert module.map3d_settings.labelsize == 20 + + def test_the_checkbox_on_the_map_labels_the_mission(self, map3d_frame, + map3d_module): + em = elements() + em.set_mission(items()) + events = [] + frame = map3d_frame(em, events) + frame.labels_check.SetValue(True) + frame.on_labels_toggle(None) + assert [text for (text, _) in labels(em)] == LABELS + # and the setting follows the box, so it is what a view opened next + # is drawn with + assert events == [('mission_labels', True)] + pushed = [] + module = map3d_module(recording_map(pushed)) + module.map.check_events = lambda: events + module.idle_task() + assert module.map3d_settings.showlabels is True + # and the view is told what the setting now is + assert pushed == [('labels', True)] + + def test_a_map_with_no_scene_yet_keeps_what_it_is_told(self, map3d_frame): + '''the live map is started before the vehicle has said where it is, + so what it is told to draw waits for a scene to draw it in''' + # the fixture skips without wx, which the frame's module needs + frame = map3d_frame() + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + Map3DFrame.handle(frame, ('mission_labels', True)) + Map3DFrame.handle(frame, ('mission_label_size', 20)) + Map3DFrame.handle(frame, ('mission_style', 'plain')) + Map3DFrame.handle(frame, ('mission_arrows', True)) + assert frame.labels_check.GetValue() is True + assert frame.style_choice.GetStringSelection() == 'plain' + # which is what the scene is built with, once there is one + Map3DFrame.handle(frame, ('origin', HOME[0], HOME[1], HOME[2])) + Map3DFrame.handle(frame, ('mission', items(), None)) + em = frame.elements + assert label_sizes(em) == [20] * len(LABELS) + assert em.mission_style == 'plain' + assert em.mission_arrows is True + + def test_mavexplorer_labels_a_view_it_opens(self, monkeypatch, mavexplorer): + mx = mavexplorer + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + labelled = [] + + class Viewer(object): + def __init__(self, title=None): + pass + + def is_alive(self): + return True + + def set_mission_labels(self, enable): + labelled.append(enable) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + + def message(kind, **fields): + m = SimpleNamespace(_timestamp=0, **fields) + m.get_type = lambda: kind + return m + rows = [message('POS', Lat=HOME[0], Lng=HOME[1], Alt=HOME[2]), + message('POS', Lat=HOME[0] + 0.01, Lng=HOME[1], + Alt=HOME[2] + 50)] + log = SimpleNamespace(rewind=lambda: None, mav_type=None) + log.recv_match = lambda **kwargs: rows.pop(0) if rows else None + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=SimpleNamespace( + condition=None, showdirection=True, showlabels=True, + labelsize=20, sync_xmap=False, missionpath='geometry')), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + assert labelled == [True] + + def test_mavexplorer_labels_a_mission_it_draws(self, monkeypatch, mavexplorer): + from types import SimpleNamespace + mx = mavexplorer + labelled = [] + sizes = [] + view = SimpleNamespace( + is_alive=lambda: True, + set_mission_arrows=lambda enable: None, + set_mission_labels=labelled.append, + set_mission_label_size=sizes.append, + set_mission_style=lambda style: None, + mission_to_fly=None) + settings = SimpleNamespace(showdirection=True, showlabels=True, + labelsize=20, missionpath='geometry', + command=lambda args: None) + monkeypatch.setattr(mx, 'mestate', SimpleNamespace(settings=settings), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', [view]) + mx.cmd_set(['showlabels', 'true']) + assert labelled == [True] + assert sizes == [20] + + def test_mavexplorer_takes_the_checkbox_on_a_view(self, monkeypatch, mavexplorer): + '''a view labelled from its own checkbox sets the setting, so every + view is labelled and one opened later is too''' + from types import SimpleNamespace + mx = mavexplorer + labelled = [] + other = SimpleNamespace( + is_alive=lambda: True, check_events=lambda: [], + set_mission_arrows=lambda enable: None, + set_mission_labels=labelled.append, + set_mission_label_size=lambda size: None, + set_mission_style=lambda style: None, mission_to_fly=None) + view = SimpleNamespace( + is_alive=lambda: True, + check_events=lambda: [('mission_labels', True)], + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_mission_style=lambda style: None, mission_to_fly=None) + settings = SimpleNamespace(showdirection=True, showlabels=False, + labelsize=14, missionpath='geometry') + monkeypatch.setattr(mx, 'mestate', SimpleNamespace(settings=settings), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', [view, other]) + mx.poll_map3d_views() + assert settings.showlabels is True + assert labelled == [True] diff --git a/tests/test_map3d_settings_sync.py b/tests/test_map3d_settings_sync.py new file mode 100644 index 0000000000..59c6a05499 --- /dev/null +++ b/tests/test_map3d_settings_sync.py @@ -0,0 +1,300 @@ +'''the 3D map's own controls and the settings they stand for. The view runs +in a process of its own, so what it is told and what it tells come and go +on two queues; however those cross, the view and whatever started it -- +the map3d module, or MAVExplorer -- end up agreeing''' + +import os +import queue +import threading +from types import SimpleNamespace + +import pytest + +HOME = (-35.363262, 149.165238, 584.0) + + +def connect(map3d_frame): + '''a Map3D with plain queues, and the frame at the other end of them''' + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.map3d import Map3D + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + viewer = Map3D.__new__(Map3D) + viewer.child = SimpleNamespace(is_alive=lambda: True) + viewer.object_queue = queue.Queue() + viewer.event_queue = queue.Queue() + viewer._origin_set = True + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + return (viewer, map3d_frame(em, state=viewer)) + + +def deliver(viewer, frame): + '''the view takes everything it has been sent''' + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + while True: + try: + msg = viewer.object_queue.get_nowait() + except queue.Empty: + return + Map3DFrame.handle(frame, msg) + + +def labels_on_the_view(frame): + frame.labels_check.SetValue(True) + frame.on_labels_toggle(None) + + +def style_on_the_view(frame): + frame.style_choice.SetStringSelection('plain') + frame.on_style_choice(None) + + +def follow_on_the_view(frame): + frame.follow_button.SetValue(False) + frame.on_follow_toggle(None) + + +def render_on_the_view(frame): + # OK in the settings dialog + frame.apply_render_settings((0.5, False, True, 60.0), notify=True) + + +def rendered(frame): + return (frame.terrain_brightness, frame.terrain_shading, + frame.terrain_wireframe, frame.fpv_fov) + + +# for each control on the view: what the user does there, the command which +# says otherwise, how the module and the view each hold the result, and what +# the command sets +CONTROLS = { + 'labels': (labels_on_the_view, ['set', 'showlabels', 'false'], + lambda module: module.map3d_settings.showlabels, + lambda frame: [frame.mission_labelled, + frame.labels_check.GetValue(), + frame.elements.mission_labelled], + lambda held: held is False), + 'style': (style_on_the_view, ['set', 'missionpath', 'geometry'], + lambda module: module.map3d_settings.missionpath, + lambda frame: [frame.mission_style, + frame.style_choice.GetStringSelection(), + frame.elements.mission_style], + lambda held: held == 'geometry'), + 'follow': (follow_on_the_view, ['follow'], + lambda module: module.follow, + lambda frame: [frame.follow, frame.follow_button.GetValue()], + lambda held: held is True), + 'render': (render_on_the_view, ['set', 'terrainbrightness', '1.75'], + lambda module: (module.map3d_settings.terrainbrightness, + module.map3d_settings.terrainshading, + module.map3d_settings.terrainwireframe, + module.map3d_settings.fpvfov), + lambda frame: [rendered(frame), + frame.terrain.render_settings + (frame.fpv_fov,)], + # the rest are the dialog's, which the command leaves be + lambda held: held == (1.75, False, True, 60.0)), +} + + +class TestSettingsSync(object): + + @pytest.mark.parametrize('control', sorted(CONTROLS)) + def test_a_command_given_while_the_view_is_changed(self, control, + map3d_frame, + map3d_module): + '''the user changes a control on the view, and before the module + has heard of it, a command says otherwise''' + (on_the_view, command, in_the_module, in_the_view, _) = CONTROLS[control] + (viewer, frame) = connect(map3d_frame) + module = map3d_module(viewer) + on_the_view(frame) + module.cmd_map3d(command) + module.idle_task() + deliver(viewer, frame) + # nothing more is on its way either side + module.idle_task() + assert viewer.event_queue.empty() + deliver(viewer, frame) + for held in in_the_view(frame): + assert held == in_the_module(module) + + @pytest.mark.parametrize('control', sorted(CONTROLS)) + def test_a_command_given_after_the_view_is_changed(self, control, + map3d_frame, + map3d_module): + '''the same, the other way round: the command comes second, and + is what both end up with''' + (on_the_view, command, in_the_module, in_the_view, + commanded) = CONTROLS[control] + (viewer, frame) = connect(map3d_frame) + module = map3d_module(viewer) + on_the_view(frame) + module.idle_task() + deliver(viewer, frame) + module.cmd_map3d(command) + deliver(viewer, frame) + # it is the command which stands, not the control + assert commanded(in_the_module(module)) + for held in in_the_view(frame): + assert held == in_the_module(module) + + def test_a_render_setting_reaches_an_open_view(self, map3d_frame, + map3d_module): + (viewer, frame) = connect(map3d_frame) + module = map3d_module(viewer) + module.cmd_map3d(['set', 'terrainbrightness', '0.75']) + module.cmd_map3d(['set', 'terrainwireframe', 'true']) + module.cmd_map3d(['set', 'fpvfov', '45']) + deliver(viewer, frame) + assert rendered(frame) == (0.75, True, True, 45.0) + assert frame.terrain.render_settings == (0.75, True, True) + + def test_render_settings_wait_for_a_scene(self, map3d_frame, + map3d_module): + (viewer, frame) = connect(map3d_frame) + frame.elements = None + frame.terrain = None + module = map3d_module(viewer) + module.cmd_map3d(['set', 'terrainshading', 'false']) + deliver(viewer, frame) + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + Map3DFrame.handle(frame, ('origin', HOME[0], HOME[1], HOME[2])) + assert frame.terrain.render_settings == (1.25, False, False) + + def test_mavexplorer_and_its_views_agree(self, map3d_frame, mavexplorer, + monkeypatch): + '''MAVExplorer has the same crossing, between a view's checkbox and + its own set command''' + from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting + settings = MPSettings([ + MPSetting('showdirection', bool, True), + MPSetting('showlabels', bool, False), + MPSetting('labelsize', int, 14), + MPSetting('missionpath', str, 'flown', + choice=['flown', 'geometry', 'plain'])]) + monkeypatch.setattr(mavexplorer, 'mestate', + SimpleNamespace(settings=settings), raising=False) + views = [connect(map3d_frame), connect(map3d_frame)] + monkeypatch.setattr(mavexplorer, 'map3d_views', + [viewer for (viewer, _) in views]) + labels_on_the_view(views[0][1]) + style_on_the_view(views[0][1]) + mavexplorer.cmd_set(['showlabels', 'false']) + mavexplorer.poll_map3d_views() + for (viewer, frame) in views: + deliver(viewer, frame) + for (viewer, frame) in views: + assert viewer.event_queue.empty() + assert CONTROLS['labels'][3](frame) == [settings.showlabels] * 3 + assert CONTROLS['style'][3](frame) == [settings.missionpath] * 3 + # where the two crossed, what MAVExplorer heard last stands: the + # view's, which it read after the command + assert settings.showlabels is True + + def test_mavexplorer_gives_a_view_a_setting_changed_anywhere( + self, map3d_frame, mavexplorer, monkeypatch): + '''the settings dialog writes the setting and says nothing, so what + the views are drawn by is taken each time round rather than only + when a set command gives it''' + from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting + settings = MPSettings([ + MPSetting('showdirection', bool, True), + MPSetting('showlabels', bool, False), + MPSetting('labelsize', int, 14), + MPSetting('missionpath', str, 'flown', + choice=['flown', 'geometry', 'plain'])]) + monkeypatch.setattr(mavexplorer, 'mestate', + SimpleNamespace(settings=settings), raising=False) + (viewer, frame) = connect(map3d_frame) + monkeypatch.setattr(mavexplorer, 'map3d_views', [viewer]) + monkeypatch.setattr(mavexplorer, 'map3d_pushed', None, + raising=False) + mavexplorer.poll_map3d_views() + deliver(viewer, frame) + assert frame.mission_style == 'flown' + # the dialog's own doing: the setting changed, with nothing said + settings.missionpath = 'plain' + settings.labelsize = 20 + mavexplorer.poll_map3d_views() + deliver(viewer, frame) + assert frame.mission_style == 'plain' + assert frame.mission_label_size == 20 + # and nothing more is sent while nothing changes + mavexplorer.poll_map3d_views() + assert viewer.object_queue.empty() + + +def display(): + return bool(os.environ.get('DISPLAY') or os.environ.get('WAYLAND_DISPLAY')) + + +@pytest.mark.skipif(not display(), reason='needs a display to build a window') +class TestToolbar(object): + '''the frame as it is really built, which the other tests stand in for''' + + def frame(self): + wx = pytest.importorskip("wx") + pytest.importorskip("vtk") + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + self.app = wx.GetApp() or wx.App(False) + closed = threading.Semaphore() + closed.acquire() + state = SimpleNamespace( + title='test', width=600, height=400, follow=True, fpvfov=90.0, + terrain_brightness=1.25, terrain_shading=True, + terrain_wireframe=False, service=None, zexag=1.0, + object_queue=queue.Queue(), event_queue=queue.Queue(), + close_window=closed) + frame = Map3DFrame(state) + frame.timer.Stop() + return frame + + def click(self, control, kind): + import wx + event = wx.CommandEvent(kind, control.GetId()) + event.SetEventObject(control) + control.GetEventHandler().ProcessEvent(event) + + def events(self, frame): + out = [] + while not frame.state.event_queue.empty(): + out.append(frame.state.event_queue.get_nowait()) + return out + + def test_the_controls_start_as_the_view_draws(self): + from MAVProxy.modules.mavproxy_map3d.map3d import MISSION_STYLES + frame = self.frame() + try: + assert frame.labels_check.GetValue() is False + assert frame.style_choice.GetStringSelection() == MISSION_STYLES[0] + assert (list(frame.style_choice.GetStrings()) == + list(MISSION_STYLES)) + assert frame.follow_button.GetValue() is True + finally: + frame.Destroy() + + def test_the_controls_tell_the_parent(self): + import wx + frame = self.frame() + try: + frame.labels_check.SetValue(True) + self.click(frame.labels_check, wx.wxEVT_CHECKBOX) + frame.style_choice.SetStringSelection('plain') + self.click(frame.style_choice, wx.wxEVT_CHOICE) + assert self.events(frame) == [('mission_labels', True), + ('mission_style', 'plain')] + finally: + frame.Destroy() + + def test_the_controls_show_what_the_view_is_told(self): + frame = self.frame() + try: + frame.handle(('mission_labels', True)) + frame.handle(('mission_style', 'geometry')) + assert frame.labels_check.GetValue() is True + assert frame.style_choice.GetStringSelection() == 'geometry' + # and being told is not the user changing them + assert self.events(frame) == [] + finally: + frame.Destroy() diff --git a/tests/test_mavflightview.py b/tests/test_mavflightview.py new file mode 100644 index 0000000000..2ed42ea2c8 --- /dev/null +++ b/tests/test_mavflightview.py @@ -0,0 +1,421 @@ +'''what mavflightview reads out of a log''' + +import struct + +import pytest + +HERE = (-35.363262, 149.165238) + + +def mavflightview(): + pytest.importorskip("cv2") + pytest.importorskip("wx") + import importlib.util + import os + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'mavflightview.py') + # MAVProxy/tools is not a package, so the tool is loaded by path + spec = importlib.util.spec_from_file_location('mavflightview', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def write_tlog(path, messages): + '''write messages to a telemetry log, a second apart. A message is + sent by vehicle 1's autopilot unless it is given as (component, message) + or (system, component, message)''' + from pymavlink import mavutil + usec = 1700000000 * 1000000 + with open(path, 'wb') as f: + for m in messages: + (system, component) = (1, 1) + if isinstance(m, tuple) and len(m) == 2: + (component, m) = m + elif isinstance(m, tuple): + (system, component, m) = m + mav = mavutil.mavlink.MAVLink(None, srcSystem=system, + srcComponent=component) + usec += 1000000 + f.write(struct.pack('>Q', usec) + m.pack(mav)) + + +def mission_item_int(seq, lat, lon, alt, command=None, params=(0, 0, 0, 0)): + from pymavlink import mavutil + mavlink = mavutil.mavlink + if command is None: + command = mavlink.MAV_CMD_NAV_WAYPOINT + return mavlink.MAVLink_mission_item_int_message( + 255, 0, seq, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + command, 0, 1, params[0], params[1], params[2], params[3], + int(lat * 1e7), int(lon * 1e7), alt) + + +def mission_count(count): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_mission_count_message(255, 0, count) + + +def mission_clear_all(): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_mission_clear_all_message(1, 1) + + +def heartbeat(): + from pymavlink import mavutil + mavlink = mavutil.mavlink + return mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, + 0, 0, 0, 3) + + +def param_value(name, value, count=1, index=0): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_param_value_message( + name.encode(), value, mavutil.mavlink.MAV_PARAM_TYPE_REAL32, + count, index) + + +def global_position_int(lat, lon): + from pymavlink import mavutil + return mavutil.mavlink.MAVLink_global_position_int_message( + 0, int(lat * 1e7), int(lon * 1e7), 100000, 100000, 0, 0, 0, 0) + + +def view(tmp_path, messages, types=None, mission=None): + '''run mavflightview over a telemetry log of messages. Returns what + mavflightview_mav does, and the options it ran with''' + from pymavlink import mavutil + tool = mavflightview() + path = str(tmp_path / 'flight.tlog') + write_tlog(path, messages) + options = tool.mavflightview_options() + options.types = types + options.mission = mission + mlog = mavutil.mavlink_connection(path) + return (tool.mavflightview_mav(mlog, options), options) + + +class Message(object): + '''a stand-in for a message from a log, with only the fields given''' + + def __init__(self, type, **fields): + self._type = type + self.__dict__.update(fields) + + def get_type(self): + return self._type + + +class TestTlogMission(object): + """a telemetry log carries its mission only as the MISSION_ITEM_INTs of + a download, with none of the CMD messages a dataflash log has""" + + def mission(self, tmp_path, messages, types=None, mission=None): + return view(tmp_path, messages, types, mission)[0][1] + + def items(self, n=3, lon=0.0, alt=50): + '''the n items of a mission''' + ret = [mission_item_int(0, HERE[0], HERE[1], 584)] + for seq in range(1, n): + ret.append(mission_item_int( + seq, HERE[0] + seq * 0.001, HERE[1] + lon, alt + seq)) + return ret + + def log(self, mission): + return ([global_position_int(HERE[0], HERE[1])] + mission + + [global_position_int(HERE[0] + 0.001, HERE[1])]) + + def altitudes(self, wp): + return [wp.wp(i).z for i in range(wp.count())] + + def test_mission_items_are_read(self, tmp_path): + wp = self.mission(tmp_path, self.log(self.items())) + assert wp.count() == 3 + assert wp.wp(2).x == pytest.approx(HERE[0] + 0.002) + assert wp.wp(2).y == pytest.approx(HERE[1]) + assert self.altitudes(wp) == [584, 51, 52] + + def test_mission_items_are_read_whatever_types_are_plotted(self, tmp_path): + wp = self.mission(tmp_path, self.log(self.items()), + types='GLOBAL_POSITION_INT') + assert wp.count() == 3 + + def test_a_download_is_read(self, tmp_path): + wp = self.mission(tmp_path, self.log([mission_count(3)] + self.items())) + assert self.altitudes(wp) == [584, 51, 52] + + def test_items_arriving_out_of_order_are_all_kept(self, tmp_path): + # a download where some items had to be asked for again + items = self.items(4) + items = [mission_count(4), items[3], items[1], items[0], items[2]] + wp = self.mission(tmp_path, self.log(items)) + assert [wp.wp(i).seq for i in range(wp.count())] == [0, 1, 2, 3] + assert self.altitudes(wp) == [584, 51, 52, 53] + + def test_nothing_is_made_up_for_items_never_seen(self, tmp_path): + # a download which never finished: item 2 did not arrive + items = self.items(4) + items = [mission_count(4), items[0], items[1], items[3]] + wp = self.mission(tmp_path, self.log(items)) + assert self.altitudes(wp) == [584, 51] + + def test_a_download_which_never_finished_leaves_the_last_whole_one( + self, tmp_path): + items = ([mission_count(3)] + self.items() + + [mission_count(3)] + self.items(alt=100)[:2]) + wp = self.mission(tmp_path, self.log(items)) + assert self.altitudes(wp) == [584, 51, 52] + + def test_a_shorter_mission_later_leaves_nothing_of_the_longer_one( + self, tmp_path): + items = ([mission_count(4)] + self.items(4) + + [mission_count(2)] + self.items(2, alt=100)) + wp = self.mission(tmp_path, self.log(items)) + assert self.altitudes(wp) == [584, 101] + + def test_a_cleared_mission_is_not_drawn(self, tmp_path): + items = [mission_count(3)] + self.items() + [mission_clear_all()] + wp = self.mission(tmp_path, self.log(items)) + assert wp.count() == 0 + + def test_a_mission_given_to_draw_is_left_alone(self, tmp_path): + path = tmp_path / 'mission.txt' + path.write_text( + 'QGC WPL 110\n' + '0\t1\t0\t16\t0\t0\t0\t0\t%f\t%f\t584\t1\n' + '1\t0\t3\t16\t0\t0\t0\t0\t%f\t%f\t77\t1\n' % + (HERE[0], HERE[1], HERE[0] - 0.01, HERE[1])) + items = [mission_count(3)] + self.items() + wp = self.mission(tmp_path, self.log(items), mission=str(path)) + assert self.altitudes(wp) == [584, 77] + + def test_a_jump_to_nowhere_ends_the_mission_drawn(self, tmp_path): + from pymavlink import mavutil + tool = mavflightview() + jump = mavutil.mavlink.MAVLink_mission_item_int_message( + 255, 0, 2, 0, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, + 20, 1, 0, 0, 0, 0, 0) + log = [heartbeat(), global_position_int(HERE[0], HERE[1]), + mission_item_int(0, HERE[0], HERE[1], 584), + mission_item_int(1, HERE[0] + 0.01, HERE[1], 100), jump, + mission_item_int(3, HERE[0] + 0.01, HERE[1] + 0.01, 100), + global_position_int(HERE[0] + 0.01, HERE[1])] + (ret, options) = view(tmp_path, log) + (wp, mav_type) = (ret[1], ret[4]) + assert wp.count() == 4 + objects = tool.mission_objects(wp, options, mav_type, 'T') + lines = [o for o in objects if type(o).__name__ == 'SlipPolygon'] + # the line runs to the jump and stops; the item after it, which + # the vehicle never reaches, is drawn apart from it + assert [len(line.points) for line in lines] == [2, 1] + # and the live map is given the same mission to draw + added = [] + + class Map(object): + def add_object(self, obj): + added.append(obj) + tool.display_waypoints(wp, Map()) + assert [o for o in added if type(o).__name__ == 'SlipPolygon'] + # the mission itself is left as it was + assert wp.wp(2).param1 == 20 + + +class TestLogMission(object): + """the messages a log's mission is read from, one at a time: those which + need MAVLink2 fields or a dataflash log to make""" + + def mission(self, messages): + from pymavlink import mavwp + mission = mavflightview().LogMission() + for m in messages: + mission.read(m) + wp = mavwp.MAVWPLoader() + mission.fill(wp) + return [wp.wp(i).z for i in range(wp.count())] + + def item(self, seq, alt, mission_type=0): + return Message('MISSION_ITEM', seq=seq, x=HERE[0], y=HERE[1], z=alt, + mission_type=mission_type) + + def count(self, count, mission_type=0): + return Message('MISSION_COUNT', count=count, + mission_type=mission_type) + + def cmd(self, num, alt, total=None): + fields = dict(CNum=num, CId=16, Prm1=0, Prm2=0, Prm3=0, Prm4=0, + Lat=HERE[0], Lng=HERE[1], Alt=alt) + if total is not None: + fields['CTot'] = total + return Message('CMD', **fields) + + def test_fence_and_rally_points_are_not_the_mission(self): + from pymavlink import mavutil + mission = [self.count(2), self.item(0, 584), self.item(1, 50)] + for mission_type in (mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY): + mission += [self.count(3, mission_type), + self.item(0, 1000, mission_type), + self.item(1, 1001, mission_type), + self.item(2, 1002, mission_type)] + assert self.mission(mission) == [584, 50] + + def test_clearing_the_fence_leaves_the_mission(self): + from pymavlink import mavutil + fence = mavutil.mavlink.MAV_MISSION_TYPE_FENCE + mission = [self.count(2), self.item(0, 584), self.item(1, 50), + Message('MISSION_CLEAR_ALL', mission_type=fence)] + assert self.mission(mission) == [584, 50] + everything = mavutil.mavlink.MAV_MISSION_TYPE_ALL + mission[-1] = Message('MISSION_CLEAR_ALL', mission_type=everything) + assert self.mission(mission) == [] + + def test_a_mavlink1_item_is_a_mission_item(self): + item = Message('MISSION_ITEM', seq=0, x=HERE[0], y=HERE[1], z=584) + assert self.mission([item]) == [584] + + def test_the_last_mission_a_dataflash_log_writes_out(self): + dumps = ([self.cmd(n, 50 + n, 4) for n in range(4)] + + [self.cmd(n, 100 + n, 2) for n in range(2)]) + assert self.mission(dumps) == [100, 101] + + def test_dataflash_mission_with_no_count_of_its_items(self): + # older logs have no CTot: each dump ends where the next starts + dumps = ([self.cmd(n, 50 + n) for n in range(4)] + + [self.cmd(n, 100 + n) for n in range(2)]) + assert self.mission(dumps) == [100, 101] + assert self.mission(dumps[:4]) == [50, 51, 52, 53] + + def test_a_cleared_dataflash_mission_is_not_drawn(self): + # the logger writes "New mission" before each mission it writes + # out, and nothing after it for one which has been cleared + new_mission = Message('MSG', Message='New mission') + dumps = ([new_mission] + [self.cmd(n, 50 + n, 4) for n in range(4)] + + [new_mission]) + assert self.mission(dumps) == [] + # other messages leave it alone, and the next mission is drawn + dumps += [Message('MSG', Message='Mission: 1 WP')] + dumps += [new_mission] + [self.cmd(n, 100 + n, 2) for n in range(2)] + assert self.mission(dumps) == [100, 101] + + def test_a_cleared_dataflash_mission_is_not_drawn_from_a_log(self, tmp_path): + # a dataflash log through mavflightview itself, which has to ask the + # log for the logger's messages as well as the mission + tool = mavflightview() + messages = ( + [Message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), + Message('MSG', Message='New mission')] + + [self.cmd(n, 50 + n, 3) for n in range(3)] + + [Message('MSG', Message='New mission'), + Message('POS', Lat=HERE[0] + 0.01, Lng=HERE[1], Alt=600.0)]) + + class Log(object): + flightmode = 'AUTO' + messages = {} + + def __init__(self): + self.queue = [] + + def rewind(self): + self.queue = list(messages) + + def recv_match(self, type=None): + while self.queue: + m = self.queue.pop(0) + m._timestamp = 1.0 + if type is None or m.get_type() in type: + return m + return None + + def check_condition(self, condition): + return True + options = tool.mavflightview_options() + options.colour_source = None + ret = tool.mavflightview_mav(Log(), options) + assert ret[1].count() == 0 + + +class TestTlogParameters(object): + """a loiter with no radius of its own is drawn at WP_LOITER_RAD, which a + telemetry log only carries in its PARAM_VALUEs""" + + def view(self, tmp_path, messages): + return view(tmp_path, messages) + + def radius(self, tmp_path, messages): + return self.view(tmp_path, messages)[1].default_circle_radius + + def log(self, params, with_heartbeat=True): + from pymavlink import mavutil + loiter = mission_item_int( + 1, HERE[0] + 0.01, HERE[1], 100, + command=mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, + params=(3, 0, 0, 0)) + start = [heartbeat()] if with_heartbeat else [] + return (start + [global_position_int(HERE[0], HERE[1])] + params + + [mission_item_int(0, HERE[0], HERE[1], 584), loiter, + global_position_int(HERE[0] + 0.01, HERE[1])]) + + def test_parameters_sent_by_the_autopilot(self, tmp_path): + # opening a log reads the first message of each type in it, so + # WP_LOITER_RAD is not the first parameter, as in any real download + log = self.log([param_value('AHRS_EKF_TYPE', 3, 2, 0), + param_value('WP_LOITER_RAD', 150, 2, 1)]) + assert self.radius(tmp_path, log) == 150 + + def test_the_loiter_is_drawn_at_that_radius(self, tmp_path): + log = self.log([param_value('AHRS_EKF_TYPE', 3, 2, 0), + param_value('WP_LOITER_RAD', 150, 2, 1)]) + (ret, options) = self.view(tmp_path, log) + (wp, mav_type) = (ret[1], ret[4]) + objects = mavflightview().mission_objects(wp, options, mav_type, 'T') + circles = [o for o in objects if type(o).__name__ == 'SlipCircle'] + assert [c.radius for c in circles] == [150] + + def test_parameters_mavproxy_fetched_over_ftp(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + # the autopilot sends a few of its own too, as they change + log = self.log([(ftp, param_value('WP_RADIUS', 90, 2, 0)), + (ftp, param_value('WP_LOITER_RAD', 150, 2, 1)), + param_value('STAT_RUNTIME', 1000)]) + assert self.radius(tmp_path, log) == 150 + + def test_the_autopilots_own_value_wins(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(ftp, param_value('WP_LOITER_RAD', 150)), + param_value('WP_LOITER_RAD', 80)]) + assert self.radius(tmp_path, log) == 80 + + def test_a_log_with_no_heartbeat(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(ftp, param_value('WP_RADIUS', 90, 2, 0)), + (ftp, param_value('WP_LOITER_RAD', 150, 2, 1)), + param_value('STAT_RUNTIME', 1000)], + with_heartbeat=False) + assert self.radius(tmp_path, log) == 150 + log = self.log([param_value('AHRS_EKF_TYPE', 3, 2, 0), + param_value('WP_LOITER_RAD', 120, 2, 1)], + with_heartbeat=False) + assert self.radius(tmp_path, log) == 120 + + def test_no_guessing_between_vehicles_with_no_heartbeat(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(1, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (1, ftp, param_value('WP_LOITER_RAD', 150, 2, 1)), + (2, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (2, ftp, param_value('WP_LOITER_RAD', 60, 2, 1))], + with_heartbeat=False) + assert self.radius(tmp_path, log) is None + + def test_the_vehicle_the_log_follows(self, tmp_path): + from pymavlink import mavutil + ftp = mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER + log = self.log([(2, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (2, ftp, param_value('WP_LOITER_RAD', 60, 2, 1)), + (1, ftp, param_value('WP_RADIUS', 90, 2, 0)), + (1, ftp, param_value('WP_LOITER_RAD', 150, 2, 1))]) + assert self.radius(tmp_path, log) == 150 diff --git a/tests/test_mission_geometry.py b/tests/test_mission_geometry.py index 21c6a41fe8..3a819bda50 100644 --- a/tests/test_mission_geometry.py +++ b/tests/test_mission_geometry.py @@ -24,6 +24,268 @@ def swept_angle(centre, points): for i in range(1, len(bearings))) +def waypoint(seq, command, x, y, z, frame=3, params=(0, 0, 0, 0)): + '''a mission item the way the wp module's loader holds one''' + from types import SimpleNamespace + return SimpleNamespace(seq=seq, command=command, x=x, y=y, z=z, + frame=frame, param1=params[0], param2=params[1], + param3=params[2], param4=params[3]) + + +def live_mission_items(wpoints, home_amsl=584.0, params=None, + vehicle='plane', default_radius=60.0): + '''the MissionItems the live map3d module sends for a mission. + + The module is built without the MAVProxy around it: only what + send_mission() reads is supplied, and terrain is never available + ''' + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import Map3DModule + module = Map3DModule.__new__(Map3DModule) + loader = SimpleNamespace(wpoints=wpoints, wp=lambda i: wpoints[i]) + module.mpstate = SimpleNamespace( + mav_param=params or {}, vehicle_type=vehicle, + module=lambda name: (SimpleNamespace(wploader=loader) + if name == 'wp' else None)) + sent = [] + module.map = SimpleNamespace( + set_mission=lambda items, track=None: sent.extend(items)) + module.reset_flown_track() + module.ground_heading = None + # only the items are looked at, so there is no path to fly for them + module.map3d_settings = SimpleNamespace(missionpath='geometry') + module.home_amsl = home_amsl + module.home_position = None + module.default_circle_radius = lambda: default_radius + module.terrain_alt = lambda lat, lon: None + module.send_mission() + return sent + + +class TestProjection(object): + """the local frame everything in the 3D map is drawn in""" + + def test_the_antimeridian_is_crossed_the_short_way(self): + pytest.importorskip("vtk") + from MAVProxy.modules.mavproxy_map3d.terrain import enu + lat = -16.5 + (east, _, _) = enu(lat, -179.999, 0.0, lat, 179.998) + (west, _, _) = enu(lat, 179.998, 0.0, lat, -179.999) + metres = math.radians(0.003) * 6378137.0 * math.cos(math.radians(lat)) + assert east == pytest.approx(metres, rel=0.01) + assert west == pytest.approx(-metres, rel=0.01) + # and away from it, as it always was + (e, n, u) = enu(HERE[0] + 0.01, HERE[1] + 0.01, 5.0, HERE[0], HERE[1]) + assert e > 0 and n > 0 and u == 5.0 + + # an origin just west of the antimeridian, and a point just east of it + SEAM = (-16.5, 179.998) + + def terrain(self): + pytest.importorskip("vtk") + from quantized_mesh_tile.global_geodetic import GlobalGeodetic + from MAVProxy.modules.mavproxy_map3d.terrain import TerrainManager + manager = TerrainManager.__new__(TerrainManager) + (manager.lat0, manager.lon0) = self.SEAM + manager.g = GlobalGeodetic(True) + (manager.zoom_fine, manager.lod_min) = (12, 8) + (manager.ring, manager.fine_radius) = (1, 2) + manager.tiles = {} + return manager + + def test_the_camera_looks_across_the_antimeridian(self): + from types import SimpleNamespace + manager = self.terrain() + east = math.radians(0.003) * 6378137.0 * math.cos(math.radians(self.SEAM[0])) + for (focal_east, lon) in ((east, -179.999), (-east, 179.995)): + camera = SimpleNamespace(focal=(focal_east, 0.0, 0.0)) + (lat, focal_lon) = manager.focal_latlon(camera) + assert (lat, focal_lon) == pytest.approx((self.SEAM[0], lon), abs=1e-6) + # and the tiles wanted about a point just east of it are those either + # side of it, not the ones at the western edge of the western tile row + (lat, lon) = manager.focal_latlon(SimpleNamespace(focal=(east, 0.0, 0.0))) + fine = sorted(x for (z, x, y) in manager.desired_set(lat, lon) + if z == manager.zoom_fine) + columns = manager.g.GetNumberOfXTilesAtZoom(manager.zoom_fine) + assert set(fine) == {0, 1, 2, columns - 2, columns - 1} + + def test_terrain_nearest_the_camera_is_fetched_first_across_the_antimeridian(self): + from types import SimpleNamespace + manager = self.terrain() + queued = [] + manager.jobs = SimpleNamespace(put=queued.append) + (manager.inflight, manager.mesh_revision) = (set(), 0) + east = math.radians(0.003) * 6378137.0 * math.cos(math.radians(self.SEAM[0])) + manager.update(SimpleNamespace( + focal=(east, 0.0, 0.0), pos=(0.0, 0.0, 0.0), + cam=SimpleNamespace(GetViewAngle=lambda: 30.0))) + (x, row) = manager.g.LonLatToTile(-179.999, self.SEAM[0], manager.zoom_fine) + columns = manager.g.GetNumberOfXTilesAtZoom(manager.zoom_fine) + order = [x for (_, _, (z, x, y)) in queued + if (z, y) == (manager.zoom_fine, row)] + # either side of the antimeridian in turn, not all of the east first + assert order == [0, columns - 1, 1, columns - 2, 2] + + def test_terrain_heights_are_found_either_side_of_the_antimeridian(self): + from types import SimpleNamespace + manager = self.terrain() + west_tile = SimpleNamespace(bbox=(170.0, -20.0, 180.0, -10.0), + height_at=lambda e, n: 10.0) + east_tile = SimpleNamespace(bbox=(-180.0, -20.0, -170.0, -10.0), + height_at=lambda e, n: 20.0) + manager.tiles = {(12, 1, 0): west_tile, (12, 0, 0): east_tile} + # a ring about a point near it runs on past 180 rather than wrapping + assert manager.height_at(-16.5, 180.001) == 20.0 + assert manager.height_at(-16.5, -180.0) == 10.0 + assert manager.height_at(-16.5, 179.999) == 10.0 + assert manager.height_at(-16.5, -179.999) == 20.0 + + def test_terrain_is_sampled_either_side_of_the_antimeridian(self, monkeypatch): + pytest.importorskip("vtk") + import numpy as np + from MAVProxy.modules.mavproxy_map3d import terrain + fetched = [] + + def decode(z, x, y, timeout=30): + fetched.append((z, x, y, timeout)) + return {"bbox": (-180.0, -20.0, -170.0, -10.0), + "verts": np.array([(-180.0, -10.0, 1.0), (-170.0, -10.0, 1.0), + (-180.0, -20.0, 1.0), (-170.0, -20.0, 1.0)])} + monkeypatch.setattr(terrain, 'decode_terrain', decode) + monkeypatch.setattr(terrain, '_sample_cache', {}) + assert terrain.sample_terrain(-16.5, 180.001) == pytest.approx(1.0) + tile = terrain.GlobalGeodetic(True).LonLatToTile(-179.999, -16.5, 12) + assert fetched == [(12,) + tile + (30,)] + # and a caller which will not wait that long says so + monkeypatch.setattr(terrain, '_sample_cache', {}) + assert terrain.sample_terrain(-16.5, 180.001, + timeout=5.0) == pytest.approx(1.0) + assert fetched[-1] == (12,) + tile + (5.0,) + + def test_a_terrain_tile_is_fetched_with_the_timeout_asked_for( + self, monkeypatch, tmp_path): + pytest.importorskip("vtk") + import time + import urllib.request + from MAVProxy.modules.mavproxy_map3d import terrain + waited = [] + + class Response(object): + def __init__(self): + self.left = [b'terrain ', b'tile'] + + def read(self, size=None): + return self.left.pop(0) if self.left else b'' + + def urlopen(request, timeout=None): + waited.append(timeout) + return Response() + monkeypatch.setattr(urllib.request, 'urlopen', urlopen) + monkeypatch.setattr(terrain, 'CACHE_DIR', str(tmp_path)) + terrain.fetch_terrain_tile(12, 1, 2, timeout=5.0) + assert waited == [5.0] + # and the whole fetch, where nobody says how long to wait for it + monkeypatch.setattr(terrain, 'CACHE_DIR', str(tmp_path / 'again')) + terrain.fetch_terrain_tile(12, 1, 2) + assert waited[-1] == 30 + # decoding a tile waits as long as it is told to as well + + class Stop(Exception): + pass + + def fetch(z, x, y, timeout=30): + waited.append(timeout) + raise Stop() + # a tile which drips in for ever is given up on at the deadline + monkeypatch.setattr(terrain, 'CACHE_DIR', str(tmp_path / 'slow')) + + class Dripping(object): + def __init__(self): + self.left = 100 + + def read(self, size=None): + if self.left <= 0: + return b'' + self.left -= 1 + time.sleep(0.01) + return b'.' + monkeypatch.setattr(urllib.request, 'urlopen', + lambda request, timeout=None: Dripping()) + with pytest.raises(Exception): + terrain.fetch_terrain_tile(12, 1, 2, timeout=0.05) + + # as a real response does, whose read() waits to fill its buffer + # however long the bytes take, and whose read1() does not + class Buffering(Dripping): + def read(self, size=None): + time.sleep(0.01 * self.left) + (data, self.left) = (b'.' * self.left, 0) + return data + + def read1(self, size=None): + return Dripping.read(self, size) + monkeypatch.setattr(urllib.request, 'urlopen', + lambda request, timeout=None: Buffering()) + started = time.time() + with pytest.raises(TimeoutError, match='longer than 0.05s'): + terrain.fetch_terrain_tile(12, 1, 2, timeout=0.05) + assert time.time() - started < 0.5 + monkeypatch.setattr(terrain, 'fetch_terrain_tile', fetch) + for asked in (5.0, None): + with pytest.raises(Stop): + if asked is None: + terrain.decode_terrain(12, 1, 2) + else: + terrain.decode_terrain(12, 1, 2, asked) + assert waited[-1] == (30 if asked is None else asked) + + def test_the_view_is_turned_across_the_antimeridian(self): + pytest.importorskip("vtk") + pytest.importorskip("wx") + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + looked = [] + frame = SimpleNamespace( + terrain=SimpleNamespace(lat0=self.SEAM[0], lon0=self.SEAM[1]), + tc=SimpleNamespace(look_at=lambda focal, dist=None: looked.append(focal)), + state=SimpleNamespace(zexag=1.0), on_camera_change=lambda: None) + Map3DFrame.look_at_latlon(frame, self.SEAM[0], -179.999, 0.0) + east = math.radians(0.003) * 6378137.0 * math.cos(math.radians(self.SEAM[0])) + assert looked[0] == pytest.approx((east, 0.0, 0.0), abs=1.0) + + def test_draped_lines_are_sampled_the_short_way_round(self): + pytest.importorskip("vtk") + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + em = ElementManager.__new__(ElementManager) + (em.lat0, em.lon0) = self.SEAM + points = [(self.SEAM[0], 179.998), (self.SEAM[0], -179.998)] + samples = list(em._terrain_samples(points, closed=False)) + assert len(samples) > 2 + for (lat, lon) in samples: + assert 0.0 <= mp_util.wrap_180(lon - 179.998) <= 0.004 + 1e-9 + + def test_mavexplorer_looks_at_a_flight_across_the_antimeridian(self): + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + import os + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + # starting just east of it, and going further west than east + flight = [(-16.5, -179.9995, 30.0), (-16.5, 179.99, 20.0), + (-16.49, -179.995, 25.0)] + (lat, lon, ground, span) = module.path_view(flight) + assert lat == pytest.approx(-16.49667, abs=1e-4) + assert lon == pytest.approx(179.9985, abs=1e-4) + assert ground == 20.0 + east_west = math.radians(0.015) * mp_util.radius_of_earth * math.cos( + math.radians(-16.49)) + assert span == pytest.approx(east_west, rel=0.01) + + class TestRhumbHelpers(object): def test_distance_takes_the_short_way_around(self): @@ -132,9 +394,10 @@ def test_radius_parameter_per_command(self): cases = [ (m.MAV_CMD_NAV_LOITER_UNLIM, (0, 0, 70, 0), 70), (m.MAV_CMD_NAV_LOITER_TURNS, (3, 0, -55, 0), -55), - (m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0), 90), + # LOITER_TIME has no radius of its own: see below + (m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0), None), (m.MAV_CMD_NAV_LOITER_TO_ALT, (1, -65, 0, 0), -65), - (getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, 0), 80), + (mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, 0), 80), ] for (command, params, expected) in cases: assert mp_util.mission_circle_radius(command, params) == expected @@ -142,7 +405,7 @@ def test_radius_parameter_per_command(self): def test_items_which_do_not_circle(self): m = self.mavlink for command in (m.MAV_CMD_NAV_WAYPOINT, - 36, # MAV_CMD_NAV_ARC_WAYPOINT (absent in older pymavlink) + mp_util.MAV_CMD_NAV_ARC_WAYPOINT, m.MAV_CMD_NAV_TAKEOFF, m.MAV_CMD_DO_JUMP): assert mp_util.mission_circle_radius(command, (1, 2, 3, 4)) is None @@ -156,19 +419,412 @@ def test_unset_radius_uses_the_vehicle_default(self): assert mp_util.mission_circle_radius( m.MAV_CMD_NAV_LOITER_UNLIM, (0, 0, 0, 0)) is None + def test_a_loiter_time_radius_of_one_is_a_direction(self): + m = self.mavlink + # ArduPilot cannot store a radius for LOITER_TIME and hands back +-1 + # to say which way round it flies, so the vehicle's own radius is the + # size and param3 only picks the direction + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 1, 0), 60) == 60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -1, 0), 60) == -60 + # and one uploaded with a real radius is flown at the vehicle's all + # the same: ArduPlane's verify_loiter_time() calls update_loiter(0) + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -90, 0), 60) == -60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0), 60) == 60 + + def test_a_radius_of_a_metre_or_less_is_the_vehicles(self): + m = self.mavlink + # update_loiter() takes a radius of a metre or less as unset, for + # every loiter item, keeping the direction the item asked for + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TURNS): + assert mp_util.mission_circle_radius( + command, (1, 0, 1, 0), 80) == 80 + assert mp_util.mission_circle_radius( + command, (1, 0, -1, 0), 80) == -80 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TO_ALT, (0, -0.5, 0, 0), 80) == -80 + # and a radius which is really a radius is left alone + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TURNS, (1, 0, -90, 0), 80) == -90 + + def test_a_vehicle_radius_of_a_metre_or_less_is_arduplanes_default(self): + m = self.mavlink + # with WP_LOITER_RAD a metre or less, update_loiter() flies + # LOITER_RADIUS_DEFAULT, 60m, the way WP_LOITER_RAD's sign says + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TIME): + assert mp_util.mission_circle_radius( + command, (30, 0, 0, 0), 0) == 60 + assert mp_util.mission_circle_radius( + command, (30, 0, 0, 0), -1) == -60 + assert mp_util.mission_circle_radius( + command, (30, 0, -1, 0), 1) == -60 + + def test_a_loiter_time_goes_the_vehicles_way_unless_told_otherwise(self): + m = self.mavlink + # ArduPlane's update_loiter() flies counter-clockwise when the item + # asked for it, and otherwise the way WP_LOITER_RAD's sign says: so + # a vehicle whose own radius is negative circles counter-clockwise + # for an item handing back +1, or one uploaded with no radius at all + for param3 in (0, 1): + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, param3, 0), -60) == -60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -1, 0), -60) == -60 + # and a clockwise vehicle is only turned round by the item asking + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 0, 0), 60) == 60 + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, -1, 0), 60) == -60 + # and with no vehicle radius to take, nothing is drawn rather than a + # circle a metre across + assert mp_util.mission_circle_radius( + m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 1, 0)) is None + def test_turn_counts(self): m = self.mavlink assert mp_util.mission_circle_turns( m.MAV_CMD_NAV_LOITER_TURNS, (3, 0, 60, 0)) == 3 # DO_ORBIT counts in radians assert mp_util.mission_circle_turns( - getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, math.radians(270))) == pytest.approx(0.75) + mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, math.radians(270))) == pytest.approx(0.75) # circling forever, and items which do not count turns assert mp_util.mission_circle_turns( - getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, 0)) is None + mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, 0)) is None assert mp_util.mission_circle_turns( m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0)) is None + def test_which_items_crosstrack_from_their_centre(self): + m = self.mavlink + # param4 of these picks the track the next leg is flown against: 0 + # asks for one out of the loiter centre, 1 for the exit location + for command in (m.MAV_CMD_NAV_LOITER_TURNS, + m.MAV_CMD_NAV_LOITER_TIME, + m.MAV_CMD_NAV_LOITER_TO_ALT): + assert mp_util.mission_crosstracks_from_centre( + command, (1, 0, 60, 0)) + assert not mp_util.mission_crosstracks_from_centre( + command, (1, 0, 60, 1)) + # an item which does not say gets what the vehicle does by default + assert mp_util.mission_crosstracks_from_centre( + command, (1, 0, 60, float('nan'))) + + def test_items_whose_param4_is_not_a_crosstrack_choice(self): + m = self.mavlink + # LOITER_UNLIM is never left, and its param4 is a yaw angle; DO_ORBIT + # counts its turns there. Neither is asking for a track out + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, mp_util.MAV_CMD_DO_ORBIT): + for param4 in (0, 1, 90, math.radians(270)): + assert not mp_util.mission_crosstracks_from_centre( + command, (80, 5, 60, param4)) + # nor is an item which does not circle at all + assert not mp_util.mission_crosstracks_from_centre( + m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)) + + +class TestLogMissionItems(object): + """what MAVExplorer hands the 3D map for the mission items in a log. + + The drawing tests below build their MissionItems by hand, so they say + nothing about whether anything fills them in: this covers the other end + """ + + def module(self): + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + import os + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + # MAVProxy/tools is not a package, so the tool is loaded by path + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + def resolve(self, mission, params=None): + return self.module().resolve_mission_amsl(mission, 584.0, params or {}) + + def item(self, seq, command, params, alt=100.0): + return (HERE[0], HERE[1] + seq * 0.01, alt, 3, command, seq, params) + + def test_a_takeoff_with_no_position_climbs_from_home(self): + from pymavlink import mavutil + m = mavutil.mavlink.MAV_CMD_NAV_TAKEOFF + home = (HERE[0], HERE[1], 584.0, 0, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)) + takeoff = (0.0, 0.0, 30.0, 3, m, 1, (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds({0: home, 1: takeoff}) + assert len(out) == 2 + # the takeoff is drawn as the climb from home it is + assert out[1][0] == HERE[0] + assert out[1][1] == HERE[1] + assert out[1][2] == 30.0 + assert out[1][4] == m + + def test_an_item_with_nowhere_to_go_is_left_out(self): + from pymavlink import mavutil + home = (HERE[0], HERE[1], 584.0, 0, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)) + # a jump carries no position and is not a takeoff, so it is not drawn + jump = (0.0, 0.0, 0.0, 3, mavutil.mavlink.MAV_CMD_DO_JUMP, 1, + (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds({0: home, 1: jump}) + assert [i[5] for i in out] == [0] + # and with nowhere to climb from, nor is a takeoff + takeoff = (0.0, 0.0, 30.0, 3, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 1, + (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds({1: takeoff}) + assert out == [] + + def test_a_mission_uploaded_before_home_was_known(self): + from pymavlink import mavutil + # the autopilot keeps its own home in item 0, so a mission uploaded + # before it had one leaves that item empty as well + home = (0.0, 0.0, 0.0, 0, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, + (0, 0, 0, 0)) + takeoff = (0.0, 0.0, 30.0, 3, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 1, + (0, 0, 0, 0)) + out = self.module().mission_items_from_cmds( + {0: home, 1: takeoff}, started_at=HERE) + # the empty home is not drawn, but the takeoff climbs from where the + # flight started + assert [i[5] for i in out] == [1] + assert (out[0][0], out[0][1]) == HERE + + def test_a_takeoff_climbs_from_where_it_was_flown(self): + from pymavlink import mavutil + # a vehicle need not take off from the home a mission was uploaded + # with; where the log says the takeoff began wins over home + home = (HERE[0], HERE[1], 584.0, 0, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)) + takeoff = (0.0, 0.0, 30.0, 3, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 1, + (0, 0, 0, 0)) + elsewhere = mp_util.gps_newpos(HERE[0], HERE[1], 45, 2000) + out = self.module().mission_items_from_cmds( + {0: home, 1: takeoff}, flown_from={1: elsewhere}) + assert (out[1][0], out[1][1]) == elsewhere + + def test_the_turns_an_item_asks_for_are_carried_through(self): + from pymavlink import mavutil + m = mavutil.mavlink + out = self.resolve([ + self.item(0, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)), + self.item(1, m.MAV_CMD_NAV_LOITER_TURNS, (3, 0, 60, 0)), + self.item(2, mp_util.MAV_CMD_DO_ORBIT, (60, 5, 0, math.radians(270))), + self.item(3, m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 60, 0)), + ]) + assert out[1].circle_turns == 3 + assert out[2].circle_turns == pytest.approx(0.75) + # circling until its time is up is not a number of turns + assert out[3].circle_turns is None + + def test_a_loiter_to_alt_works_its_own_turns_out(self): + from pymavlink import mavutil + m = mavutil.mavlink + params = {'AIRSPEED_CRUISE': 20.0, 'TECS_CLMB_MAX': 5.0} + out = self.resolve([ + self.item(0, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0), alt=100.0), + self.item(1, m.MAV_CMD_NAV_LOITER_TO_ALT, (0, 60, 0, 0), + alt=700.0), + ], params) + # this one has no turn count of its own: what is left to climb on + # arrival decides it. 600m at 5m/s is 120s of climbing, of which + # the 900m approach does 45s, leaving four turns of a 60m circle + assert out[1].circle_turns == pytest.approx(4.0, abs=0.2) + + def log(self, *messages): + '''a log which hands back these messages in order''' + class Log(object): + def __init__(self, messages): + self.messages = list(messages) + + def recv_match(self, type=None, condition=None): + while self.messages: + m = self.messages.pop(0) + if type is None or m.get_type() in type: + return m + return None + return Log(messages) + + def message(self, kind, **fields): + from types import SimpleNamespace + m = SimpleNamespace(_timestamp=0, **fields) + m.get_type = lambda: kind + return m + + def dump(self, where, count, command=16): + '''what the logger writes for a mission of count items at where''' + out = [self.message('MSG', Message='New mission')] + for seq in range(count): + p = mp_util.gps_newpos(where[0], where[1], 90, 100 * seq) + out.append(self.message('CMD', CNum=seq, CId=command, Lat=p[0], + Lng=p[1], Alt=100.0, Frame=3, Prm1=0, + Prm2=0, Prm3=0, Prm4=0)) + return out + + def test_a_log_draws_the_last_mission_it_holds(self): + first = HERE + second = mp_util.gps_newpos(HERE[0], HERE[1], 0, 5000) + (path, mission, _, _, _, _, _) = self.module().mission_from_log( + self.log(*(self.dump(first, 4) + self.dump(second, 2)))) + # just the second mission, even though it is the shorter of the two + assert len(mission) == 2 + assert mission[0][0] == pytest.approx(second[0]) + + def test_a_cleared_mission_draws_nothing(self): + # clearing the mission writes the message and then no items at all + (path, mission, _, _, _, _, _) = self.module().mission_from_log( + self.log(*(self.dump(HERE, 4) + + [self.message('MSG', Message='New mission')]))) + assert mission == [] + + def test_the_log_says_where_a_takeoff_began(self): + from pymavlink import mavutil + m = mavutil.mavlink + moved = mp_util.gps_newpos(HERE[0], HERE[1], 45, 2000) + + def cmd(seq, command, lat, lng, alt): + return self.message('CMD', CNum=seq, CId=command, Lat=lat, + Lng=lng, Alt=alt, Frame=3, Prm1=0, Prm2=0, + Prm3=0, Prm4=0) + (path, mission, _, _, _, _, _) = self.module().mission_from_log(self.log( + self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), + self.message('MSG', Message='New mission'), + cmd(0, m.MAV_CMD_NAV_WAYPOINT, HERE[0], HERE[1], 584.0), + cmd(1, m.MAV_CMD_NAV_TAKEOFF, 0.0, 0.0, 30.0), + # the vehicle is carried somewhere else before it flies + self.message('POS', Lat=moved[0], Lng=moved[1], Alt=600.0), + self.message('MSG', Message='Mission: 1 Takeoff'), + self.message('POS', Lat=moved[0], Lng=moved[1], Alt=630.0), + )) + assert (mission[1][0], mission[1][1]) == moved + + def test_an_empty_home_is_the_one_the_log_recorded(self): + # a mission uploaded before the vehicle had a home logs home as + # nothing at all; the vehicle logs its home when it gets one. The + # first positions are logged before the origin is, at nothing too + from pymavlink import mavutil + m = mavutil.mavlink + up = mp_util.gps_newpos(HERE[0], HERE[1], 0, 1000) + + def cmd(seq, command, lat, lng, alt, frame): + return self.message('CMD', CNum=seq, CId=command, Lat=lat, + Lng=lng, Alt=alt, Frame=frame, Prm1=0, + Prm2=0, Prm3=0, Prm4=0) + (path, mission, cmds, _, _, _, _) = self.module().mission_from_log(self.log( + self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=0.1), + self.message('MSG', Message='New mission'), + cmd(0, m.MAV_CMD_NAV_WAYPOINT, 0.0, 0.0, 0.0, 0), + cmd(1, m.MAV_CMD_NAV_WAYPOINT, up[0], up[1], 70.0, 3), + self.message('ORGN', Type=0, Lat=HERE[0], Lng=HERE[1], Alt=500.0), + self.message('ORGN', Type=1, Lat=HERE[0], Lng=HERE[1], Alt=584.0), + self.message('POS', Lat=HERE[0], Lng=HERE[1], Alt=584.0), + )) + assert (mission[0][0], mission[0][1], mission[0][2]) == HERE + (584.0,) + assert cmds[0][:3] == HERE + (584.0,) + ground0 = min(p[2] for p in path) + out = self.module().resolve_mission_amsl(mission, ground0, {}) + # measured from the home the vehicle had, not the lowest position + assert out[1].alt == pytest.approx(654.0) + + def test_a_log_from_before_the_logger_said_new_mission(self): + # without the message, a mission's first item still starts it again + first = [m for m in self.dump(HERE, 4) if m.get_type() == 'CMD'] + second = [m for m in self.dump( + mp_util.gps_newpos(HERE[0], HERE[1], 0, 5000), 2) + if m.get_type() == 'CMD'] + (path, mission, _, _, _, _, _) = self.module().mission_from_log( + self.log(*(first + second))) + assert len(mission) == 2 + + +class TestLiveMissionTurns(object): + """the turns the live map3d module hands the viewer""" + + def test_the_turns_an_item_asks_for_are_carried_through(self): + from pymavlink import mavutil + m = mavutil.mavlink + at = [mp_util.gps_newpos(HERE[0], HERE[1], 90, 400 * i) + for i in range(3)] + items = live_mission_items([ + waypoint(0, m.MAV_CMD_NAV_WAYPOINT, at[0][0], at[0][1], 100.0), + waypoint(1, m.MAV_CMD_NAV_LOITER_TURNS, at[1][0], at[1][1], + 300.0, params=(3, 0, 60, 0)), + waypoint(2, m.MAV_CMD_NAV_LOITER_TIME, at[2][0], at[2][1], + 300.0, params=(30, 0, 60, 0)), + ]) + assert items[1].circle_turns == 3 + # circling until its time is up is not a number of turns + assert items[2].circle_turns is None + + +class TestLiveMissionAltitudes(object): + """the map3d module measures the climb into a loiter itself, to work out + how many turns it takes, so it has to resolve the frames first""" + + def module(self, home_amsl): + from MAVProxy.modules.mavproxy_map3d import Map3DModule + # the module talks to a live MAVProxy, which is not what is under + # test here: only the altitude it hands the turn count + module = Map3DModule.__new__(Map3DModule) + module.home_amsl = home_amsl + return module + + def test_frames_resolve_to_amsl(self): + module = self.module(584.0) + for frame in (0, 5): + assert module.item_amsl(700.0, frame) == 700.0 + for frame in (3, 6): + assert module.item_amsl(100.0, frame) == 684.0 + + def test_a_terrain_altitude_stands_above_home_until_it_is_known(self): + # send_mission() turns a terrain-frame item into AMSL when it has + # the terrain height; until then home's height stands in for the + # ground's, so there is a path to draw, and the mission is drawn + # again once the terrain arrives + module = self.module(584.0) + for frame in (10, 11): + assert module.item_amsl(100.0, frame) == 684.0 + # unless home is not known either + module = self.module(None) + assert module.item_amsl(100.0, 10) is None + + def test_a_climb_across_two_frames(self): + module = self.module(584.0) + # 600m AMSL to 100m above a home at 584m is a climb of 84m, not the + # 500m descent the raw item altitudes look like + first = module.item_amsl(600.0, 0) + second = module.item_amsl(100.0, 3) + assert second - first == pytest.approx(84.0) + + def test_send_mission_measures_the_climb_in_one_frame(self): + from pymavlink import mavutil + m = mavutil.mavlink + params = {'AIRSPEED_CRUISE': 20.0, 'TECS_CLMB_MAX': 5.0, + 'TECS_SINK_MIN': 2.0} + loiter_at = mp_util.gps_newpos(HERE[0], HERE[1], 90, 300) + items = live_mission_items([ + waypoint(0, m.MAV_CMD_NAV_WAYPOINT, HERE[0], HERE[1], 600.0, + frame=0), + waypoint(1, m.MAV_CMD_NAV_LOITER_TO_ALT, loiter_at[0], + loiter_at[1], 400.0, frame=3, params=(0, 60, 0, 0)), + ], home_amsl=584.0, params=params) + # 600m AMSL up to 400m above a 584m home is a climb of 384m; the raw + # altitudes would have it a 200m descent, which takes more turns + climb = mp_util.loiter_to_alt_turns(60, 384.0, params, 300.0) + descent = mp_util.loiter_to_alt_turns(60, -200.0, params, 300.0) + assert abs(climb - descent) > 0.5 + assert items[1].circle_turns == pytest.approx(climb, rel=0.01) + + def test_without_a_home_a_relative_altitude_is_unknown(self): + module = self.module(None) + assert module.item_amsl(100.0, 3) is None + # an AMSL item still stands on its own + assert module.item_amsl(700.0, 0) == 700.0 + class TestHoveringVehicles(object): @@ -192,7 +848,7 @@ def test_a_hovering_vehicle_only_circles_for_some_items(self): m = self.mavlink # it flies these as circles ... for (command, params) in ((m.MAV_CMD_NAV_LOITER_TURNS, (2, 0, 60, 0)), - (getattr(m, 'MAV_CMD_DO_ORBIT', 34), (80, 5, 0, 0))): + (mp_util.MAV_CMD_DO_ORBIT, (80, 5, 0, 0))): assert mp_util.mission_circle_radius( command, params, vehicle=m.MAV_TYPE_QUADROTOR) is not None # ... and holds position for these, climbing straight up rather than @@ -201,10 +857,10 @@ def test_a_hovering_vehicle_only_circles_for_some_items(self): (m.MAV_CMD_NAV_LOITER_TIME, (30, 0, 90, 0)), (m.MAV_CMD_NAV_LOITER_TO_ALT, (1, -65, 0, 0))): assert mp_util.mission_circle_radius( - command, params, vehicle=m.MAV_TYPE_QUADROTOR) is None + command, params, 60, vehicle=m.MAV_TYPE_QUADROTOR) is None # a forward-flight vehicle circles for all of them assert mp_util.mission_circle_radius( - command, params, vehicle=m.MAV_TYPE_FIXED_WING) is not None + command, params, 60, vehicle=m.MAV_TYPE_FIXED_WING) is not None def test_unknown_vehicle_keeps_the_old_behaviour(self): m = self.mavlink @@ -675,6 +1331,51 @@ def test_crosstracking_from_the_exit_flies_straight_there(self): assert len(errors) == 1 +class TestProducersCrosstrack(object): + """what each producer of MissionItems makes of param4. Only the loiters + ArduPlane lets go of carry a crosstrack choice there; LOITER_UNLIM + never leaves, and DO_ORBIT counts its turns in param4, so either + producer reading param4 as a crosstrack choice for those would draw a + pull-back after a loiter the vehicle never leaves""" + + PARAMS = {'NAVL1_PERIOD': 17.0, 'AIRSPEED_CRUISE': 20.0} + + def cases(self): + from pymavlink import mavutil + m = mavutil.mavlink + # (command, params, whether the leg out is pulled back to the centre) + return [ + (m.MAV_CMD_NAV_LOITER_TURNS, (1, 0, 60, 0), True), + (m.MAV_CMD_NAV_LOITER_TURNS, (1, 0, 60, 1), False), + (m.MAV_CMD_NAV_LOITER_UNLIM, (0, 0, 60, 0), False), + ] + + def test_the_live_module(self): + from pymavlink import mavutil + m = mavutil.mavlink + at = [mp_util.gps_newpos(HERE[0], HERE[1], 90, 400 * i) + for i in range(3)] + for (command, params, pulled_back) in self.cases(): + items = live_mission_items([ + waypoint(0, m.MAV_CMD_NAV_WAYPOINT, at[0][0], at[0][1], 100.0), + waypoint(1, command, at[1][0], at[1][1], 100.0, params=params), + waypoint(2, m.MAV_CMD_NAV_WAYPOINT, at[2][0], at[2][1], 100.0), + ], params=self.PARAMS) + assert (items[1].exit_converge is not None) == pulled_back, command + + def test_mavexplorer(self): + from pymavlink import mavutil + m = mavutil.mavlink + log = TestLogMissionItems() + for (command, params, pulled_back) in self.cases(): + items = log.resolve([ + log.item(0, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)), + log.item(1, command, params), + log.item(2, m.MAV_CMD_NAV_WAYPOINT, (0, 0, 0, 0)), + ], self.PARAMS) + assert (items[1].exit_converge is not None) == pulled_back, command + + class TestPolygonBounds(object): def test_bounds_cover_the_arc_and_not_just_the_chord(self): @@ -699,3 +1400,74 @@ def test_orbit_geometry_without_enum(monkeypatch): monkeypatch.delattr(mavutil.mavlink, 'MAV_CMD_DO_ORBIT', raising=False) assert mp_util.mission_circle_radius(34, (80, 5, 0, 0)) == 80 assert mp_util.mission_circle_turns(34, (80, 5, 0, math.pi)) == pytest.approx(.5) + + +class TestMissionFiles(object): + """the missions under tests/missions are flown by hand in SITL, so check + here that they still load and are still the missions their README says""" + + def load(self, name): + '''(seq, frame, command, params, lat, lon, alt) for each item''' + import os + path = os.path.join(os.path.dirname(__file__), 'missions', name) + with open(path) as f: + lines = f.read().splitlines() + assert lines[0] == 'QGC WPL 110' + items = [] + for line in lines[1:]: + if not line.strip() or line.startswith('#'): + continue + fields = line.split() + assert len(fields) == 12, line + items.append((int(fields[0]), int(fields[2]), int(fields[3]), + tuple(float(v) for v in fields[4:8]), + float(fields[8]), float(fields[9]), + float(fields[10]))) + assert [i[0] for i in items] == list(range(len(items))) + return items + + def test_the_plane_mission(self): + from pymavlink import mavutil + m = mavutil.mavlink + items = self.load('plane-mission-geometry.txt') + assert len(items) == 16 + commands = set(i[2] for i in items) + # ArduPlane flies neither of these, so a flyable mission has neither + assert mp_util.MAV_CMD_NAV_ARC_WAYPOINT not in commands + assert mp_util.MAV_CMD_DO_ORBIT not in commands + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TURNS, + m.MAV_CMD_NAV_LOITER_TIME, m.MAV_CMD_NAV_LOITER_TO_ALT): + assert command in commands + # both ways round, and param4 both ways + radii = [mp_util.mission_circle_radius(i[2], i[3], 60) for i in items] + assert any(r is not None and r > 0 for r in radii) + assert any(r is not None and r < 0 for r in radii) + xtrack = [mp_util.mission_crosstracks_from_centre(i[2], i[3]) + for i in items if i[2] in (m.MAV_CMD_NAV_LOITER_TURNS, + m.MAV_CMD_NAV_LOITER_TO_ALT)] + assert True in xtrack and False in xtrack + # AMSL, home-relative and terrain altitudes + assert set(i[1] for i in items) >= {0, 3, 10} + + def test_the_copter_mission(self): + from pymavlink import mavutil + m = mavutil.mavlink + items = self.load('copter-mission-geometry.txt') + assert len(items) == 13 + commands = [i[2] for i in items] + # ArduCopter flies arc waypoints, and no vehicle flies DO_ORBIT + sweeps = [i[3][0] for i in items if i[2] == mp_util.MAV_CMD_NAV_ARC_WAYPOINT] + assert any(s > 0 for s in sweeps) + assert any(s < -180 for s in sweeps) + assert mp_util.MAV_CMD_DO_ORBIT not in commands + # a takeoff with no position of its own + takeoff = items[commands.index(m.MAV_CMD_NAV_TAKEOFF)] + assert (takeoff[4], takeoff[5]) == (0.0, 0.0) + # circles only where a multirotor flies them, and only with a radius + drawn = [i[0] for i in items + if mp_util.mission_circle_radius(i[2], i[3], None, + 'copter') is not None] + assert [commands[s] for s in drawn] == [m.MAV_CMD_NAV_LOITER_TURNS] * 2 + for command in (m.MAV_CMD_NAV_LOITER_UNLIM, m.MAV_CMD_NAV_LOITER_TIME, + m.MAV_CMD_NAV_LOITER_TO_ALT): + assert command in commands diff --git a/tests/test_plane_track.py b/tests/test_plane_track.py new file mode 100644 index 0000000000..9be3c63479 --- /dev/null +++ b/tests/test_plane_track.py @@ -0,0 +1,4013 @@ +'''the path plane_track flies a mission along, against ArduPlane's own +navigation and a flight flown through it''' + +import json +import math +import os +import struct +import types + +import pytest + +from pymavlink import mavutil + +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import plane_track + +mavlink = mavutil.mavlink +HOME = (-35.363262, 149.165238, 584.0) +PARAMS = { + 'AIRSPEED_CRUISE': 22.0, 'AIRSPEED_MIN': 10.0, 'AIRSPEED_MAX': 40.0, + 'NAVL1_PERIOD': 15.0, 'NAVL1_DAMPING': 0.75, + 'ROLL_LIMIT_DEG': 50.0, 'WP_RADIUS': 60.0, 'WP_LOITER_RAD': 80.0, + 'TECS_CLMB_MAX': 5.0, 'TECS_SINK_MAX': 5.0, +} + + +# PARAMS for an aircraft whose TECS flies by an airspeed sensor +AIRSPEED_SENSOR = dict(PARAMS, ARSPD_TYPE=1, ARSPD_USE=1) + + +def offset(north, east, alt=100.0, base=HOME): + '''(lat, lon, amsl) north and east metres of base, alt above it''' + (lat, lon) = mp_util.gps_offset(base[0], base[1], east, north) + return (lat, lon, base[2] + alt) + + +def waypoint(north, east, alt=100.0): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, amsl, (0, 0, 0, 0)) + + +def loiter_to_alt(north, east, alt, radius=0.0, xtrack=0.0): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_LOITER_TO_ALT, lat, lon, amsl, + (0, radius, 0, xtrack)) + + +def loiter_turns(north, east, turns, radius, alt=100.0, xtrack=0.0): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_LOITER_TURNS, lat, lon, amsl, + (turns, 0, radius, xtrack)) + + +def change_speed(speed): + return (mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 0, None, (0, speed, -1, 0)) + + +def local(point): + '''metres (north, east) of HOME''' + return (mp_util.gps_distance(HOME[0], HOME[1], point[0], HOME[1]) * + (1 if point[0] >= HOME[0] else -1), + mp_util.gps_distance(HOME[0], HOME[1], HOME[0], point[1]) * + (1 if point[1] >= HOME[1] else -1)) + + +def fly(items, params=PARAMS, home=HOME): + track = plane_track.mission_track(home, items, params) + assert track is not None + return track + + +def up_to(track, north, east): + '''the track as far as where it comes closest to the point north and + east metres of HOME: the mission, without the return to launch which + follows it once it is complete''' + (lat, lon, _) = offset(north, east) + distances = [mp_util.gps_distance(p[0], p[1], lat, lon) for p in track] + return track[:distances.index(min(distances)) + 1] + + +def nearest(track, lat, lon): + '''the point on track nearest lat, lon''' + return min(track, key=lambda p: mp_util.gps_distance(p[0], p[1], lat, lon)) + + +def arrival(track, lat, lon, within=100.0): + '''the first point on track within so many metres of lat, lon: where it + gets there, before it turns for whatever comes next''' + return [p for p in track + if mp_util.gps_distance(p[0], p[1], lat, lon) < within][0] + + +def distance_to_line(point, start, end): + '''signed metres of point to the right of the line start->end''' + (n, e) = (point[0] - start[0], point[1] - start[1]) + (dn, de) = (end[0] - start[0], end[1] - start[1]) + length = math.hypot(dn, de) + return (n * de - e * dn) / length + + +class TestL1Control(object): + '''the controller port, one step at a time''' + + def control(self): + return plane_track.L1Control(15.0, 0.75, 0.0, 0.0) + + def test_on_the_track_it_holds_straight(self): + l1 = self.control() + l1.update_waypoint((0, 0), (20, 0), 0.0, (-500, 0), (500, 0), 0.1) + assert l1.lateral_acceleration == pytest.approx(0.0, abs=1e-9) + + def test_off_the_track_it_turns_back_towards_it(self): + l1 = self.control() + # east of a track running north, it turns left + l1.update_waypoint((0, 50), (20, 0), 0.0, (-500, 0), (500, 0), 0.1) + assert l1.lateral_acceleration < 0 + l1.update_waypoint((0, -50), (20, 0), 0.0, (-500, 0), (500, 0), 0.1) + assert l1.lateral_acceleration > 0 + + def test_far_from_a_loiter_it_heads_for_the_centre(self): + l1 = self.control() + l1.update_loiter((-1000, 0), (20, 0), 0.0, (0, 0), 100.0, 1) + assert not l1.circling + assert l1.lateral_acceleration == pytest.approx(0.0, abs=1e-6) + + def test_on_the_circle_it_turns_at_the_circles_rate(self): + l1 = self.control() + # due west of the centre, flying north: clockwise round it + l1.update_loiter((0, -100), (20, 0), 0.0, (0, 0), 100.0, 1) + assert l1.circling + assert l1.lateral_acceleration == pytest.approx(20.0 ** 2 / 100.0) + + def test_the_circle_flown_grows_with_altitude(self): + l1 = self.control() + assert l1.loiter_radius(100.0, 0.0, 20.0) == pytest.approx(100.0) + assert l1.loiter_radius(100.0, 3000.0, 20.0) > 130.0 + + +class TestWaypointMaxRadius(object): + """WP_MAX_RADIUS: a waypoint is not reached until the aircraft is that + close to it, however far past it has flown (Plane::verify_nav_wp)""" + + def waypoint(self, north, east, passby=0): + (lat, lon, amsl) = offset(north, east) + return (mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, amsl, (0, 0, passby, 0)) + + def corner(self, passby=0): + '''a right-angle turn at 1000 north, 600 east''' + return [self.waypoint(1000, 0), self.waypoint(1000, 600, passby), + self.waypoint(0, 600)] + + def closest(self, track, north, east): + (lat, lon, _) = offset(north, east) + distances = [mp_util.gps_distance(p[0], p[1], lat, lon) for p in track] + return (min(distances), distances.index(min(distances))) + + def test_the_corner_is_not_cut(self): + # turning early, the aircraft passes wide of the corner + (wide, _) = self.closest(fly(self.corner()), 1000, 600) + assert wide > 20.0 + track = fly(self.corner(), dict(PARAMS, WP_MAX_RADIUS=20)) + (near, _) = self.closest(track, 1000, 600) + assert near <= 20.0 + # and the mission goes on from there + assert self.closest(track, 0, 600)[0] <= PARAMS['WP_RADIUS'] + + def test_one_it_cannot_turn_tightly_enough_for_is_not_drawn(self): + # a waypoint 100m to the side of one the aircraft arrives at going + # the other way, with a turn some 85m across: held within 60m it + # is reached, and within 30m it is circled for ever, as ArduPlane + # warns, which leaves no path to draw + params = dict(PARAMS, ROLL_LIMIT_DEG=30.0) + items = [self.waypoint(1000, 0), self.waypoint(1000, 100), + self.waypoint(2000, 100)] + track = fly(items, dict(params, WP_MAX_RADIUS=60)) + assert self.closest(track, 1000, 100)[0] <= 60.0 + assert plane_track.mission_track( + HOME, items, dict(params, WP_MAX_RADIUS=30)) is None + + def test_a_pass_by_waypoint_is_overflown(self): + # the finish line is past the pass-by point, so the aircraft is + # never taken back to the waypoint: "it will overfly badly", as + # ArduPlane has it + params = dict(PARAMS, WP_MAX_RADIUS=30) + items = [self.waypoint(1000, 0), self.waypoint(1000, 600, passby=100), + self.waypoint(1000, 0)] + assert plane_track.mission_track(HOME, items, params) is None + assert plane_track.mission_track(HOME, items, PARAMS) is not None + + def test_only_waypoints_are_held_to_it(self): + # a landing passed wide of the radius is still done with + (lat, lon, amsl) = offset(1200, 0, 0) + items = [loiter_turns(1000, 0, 1, 80), + (mavlink.MAV_CMD_NAV_LAND, lat, lon, amsl, (0, 0, 0, 0))] + track = fly(items) + assert self.closest(track, 1200, 0)[0] > 1.0 + assert fly(items, dict(PARAMS, WP_MAX_RADIUS=1)) == track + # as are loiters + items = [loiter_turns(1000, 0, 1, 80), loiter_turns(1000, 600, 1, 80)] + assert fly(items, dict(PARAMS, WP_MAX_RADIUS=1)) == fly(items) + + +class TestVtolApproach(object): + """a QuadPlane's VTOL landing flown in on a fixed-wing approach: + Plane::verify_landing_vtol_approach""" + + QUADPLANE = dict(PARAMS, Q_ENABLE=1) + + def vtol_land(self, north, east, alt=60.0, option=1): + (lat, lon, amsl) = offset(north, east, alt) + return (mavlink.MAV_CMD_NAV_VTOL_LAND, lat, lon, amsl, + (option, 0, 0, 0)) + + def items(self, **kwargs): + # out east and back to land at home + return [waypoint(0, 2000, 150), self.vtol_land(0, 0, **kwargs)] + + def landed(self, track): + '''where the fixed-wing flight hands over to the VTOL landing, from + the landing, where the path ends; the course it was on there; and + the path before, north and east of home''' + points = [local(p) for p in track] + end = points[-1] + pad = [i for (i, p) in enumerate(points) + if i > 10 and math.hypot(p[0] - end[0], p[1] - end[1]) < 0.5][0] + (a, b) = (points[pad - 2], points[pad - 1]) + course = math.degrees(math.atan2(b[1] - a[1], b[0] - a[0])) + handover = (b[0] - end[0], b[1] - end[1]) + bearing = math.degrees(math.atan2(handover[1], handover[0])) + return (handover, bearing, course, points[:pad]) + + def swept(self, points): + '''degrees swept clockwise about the pad''' + total = 0.0 + for (a, b) in zip(points, points[1:]): + total += mp_util.wrap_180(math.degrees(math.atan2(b[1], b[0])) - + math.degrees(math.atan2(a[1], a[0]))) + return total + + def circling(self, before, radius): + '''what was flown from coming back near the landing until the + approach''' + back = [i for (i, p) in enumerate(before) + if i > len(before) // 3 and math.hypot(*p) < radius * 1.3] + return before[back[0]:] + + def test_the_landing_is_circled_to_and_flown_in_to(self): + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=250) + track = plane_track.mission_track(HOME, self.items(), params, + approach=0.0) + (handover, bearing, course, before) = self.landed(track) + # the landing's altitude, come down to on the way in as ArduPlane + # does, and the circle round it at Q_FW_LND_APR_RAD, clockwise: + # from the east, a quarter of the way round to fly north + assert min(p[2] for p in track[len(track) // 2:]) >= HOME[2] + 55.0 + circling = self.circling(before, 250) + assert self.swept(circling) == pytest.approx(90.0, abs=30.0) + assert max(math.hypot(*p) for p in circling) > 250 + # into the wind: north, from south of the landing, a stopping + # distance or so out, and within the 30 degrees ArduPlane takes as + # lined up + assert mp_util.wrap_180(bearing - 180.0) == pytest.approx(0, abs=15) + assert course == pytest.approx(0.0, abs=30.0) + assert 100.0 < math.hypot(*handover) < 250.0 + # and it lands where the item is, at the altitude approached at + assert math.hypot(*local(track[-1])) < 0.5 + assert track[-1][2] == pytest.approx(HOME[2] + 60.0, abs=1.0) + + def test_it_is_flown_into_the_wind(self): + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=250) + for approach in (0.0, 90.0, -135.0): + track = plane_track.mission_track(HOME, self.items(), params, + approach=approach) + (_, bearing, course, _) = self.landed(track) + assert mp_util.wrap_180(course - approach) == pytest.approx( + 0.0, abs=30.0) + assert mp_util.wrap_180(bearing - (approach + 180)) == \ + pytest.approx(0.0, abs=20.0) + # with no wind to go on, ArduPlane's sum comes to due south + track = plane_track.mission_track(HOME, self.items(), params) + (_, bearing, course, _) = self.landed(track) + assert mp_util.wrap_180(course - 180.0) == pytest.approx(0, abs=30) + assert mp_util.wrap_180(bearing) == pytest.approx(0, abs=20) + + def test_the_circle_is_flown_the_way_its_radius_says(self): + # from the east to fly east: half way round, either way + for (radius, clockwise) in ((250, True), (-250, False)): + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=radius) + track = plane_track.mission_track(HOME, self.items(), params, + approach=90.0) + swept = self.swept(self.circling(self.landed(track)[3], 250)) + assert swept > 150 if clockwise else swept < -150 + # and with no radius of its own, WP_LOITER_RAD's + track = plane_track.mission_track(HOME, self.items(), + self.QUADPLANE, approach=90.0) + radius = PARAMS['WP_LOITER_RAD'] + circling = self.circling(self.landed(track)[3], radius) + assert min(math.hypot(*p) for p in circling) < radius * 1.3 + assert self.swept(circling) > 150 + + def test_what_asks_for_it(self): + def circles(items, params): + # coming from the east, only circling takes it west of the pad + track = plane_track.mission_track(HOME, items, params, + approach=0.0) + return any(local(p)[1] < -40 for p in track) + # param1, or Q_OPTIONS for every VTOL landing + assert circles(self.items(option=1), self.QUADPLANE) + assert circles(self.items(option=0), + dict(self.QUADPLANE, Q_OPTIONS=1 << 4)) + # but not a VTOL landing asked for neither way, which is flown + # straight in, nor anything but a QuadPlane + assert not circles(self.items(option=0), self.QUADPLANE) + assert not circles(self.items(option=1), PARAMS) + items = [waypoint(0, 2000, 150), self.vtol_land(0, 0, option=1)] + assert plane_track.uses_vtol_approach(items, self.QUADPLANE) + assert not plane_track.uses_vtol_approach(items, PARAMS) + items[-1] = self.vtol_land(0, 0, option=0) + assert not plane_track.uses_vtol_approach(items, self.QUADPLANE) + assert plane_track.uses_vtol_approach( + items, dict(self.QUADPLANE, Q_OPTIONS=1 << 4)) + + def test_a_landing_where_the_aircraft_is_goes_out_to_the_circle(self): + # a VTOL landing with no position of its own is where the aircraft + # is when it starts, so the circle is started from its middle, and + # the aircraft goes out to it before breaking out onto the approach + # -- even arriving, as here, on the course it breaks out on: east, + # for a clockwise circle and an approach south + here = (mavlink.MAV_CMD_NAV_VTOL_LAND, 0.0, 0.0, HOME[2] + 60, + (1, 0, 0, 0)) + params = dict(self.QUADPLANE, Q_FW_LND_APR_RAD=250) + track = plane_track.mission_track( + HOME, [waypoint(0, 2000, 60), here], params, approach=180.0) + landing = local(track[-1]) + assert landing[1] > 1800 + points = [local(p) for p in track] + arrived = [i for (i, p) in enumerate(points) + if math.hypot(p[0] - landing[0], p[1] - landing[1]) < 30] + out = [math.hypot(p[0] - landing[0], p[1] - landing[1]) + for p in points[arrived[0]:]] + assert max(out) > 245.0 + # and comes in from the north, into the wind + last = points[-3] + assert last[0] > landing[0] + 50 + + def test_a_mission_which_runs_out_lands_on_the_approach_too(self): + """the return a mission which runs out flies is landed as + Q_RTL_MODE says, so such a mission is flown into the wind as + much as one which returns by an item of its own""" + params = dict(self.QUADPLANE, Q_RTL_MODE=2, Q_FW_LND_APR_RAD=200) + items = [waypoint(0, 2000, 150), waypoint(500, 2000, 150)] + assert plane_track.uses_vtol_approach(items, params) + # and the course it is drawn on is the wind's + into = [plane_track.mission_track(HOME, items, params, + approach=course)[-3] + for course in (45.0, 225.0)] + assert mp_util.gps_distance(into[0][0], into[0][1], + into[1][0], into[1][1]) > 100.0 + # where a mission the flight ends in does not return at all + for ending in (self.vtol_land(0, 0, option=0), + (mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, None, + (0, 0, 0, 0)), + (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (1, -1, 0, 0))): + assert not plane_track.uses_vtol_approach(items + [ending], params) + # and a landing the flight never reaches, a jump going past it, does + # not end anything: the mission still runs out + landing = (mavlink.MAV_CMD_NAV_LAND, HOME[0], HOME[1], HOME[2], + (0, 0, 0, 0)) + jumped = [items[0], (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, + (4, 1, 0, 0)), landing, items[1]] + assert plane_track.uses_vtol_approach(jumped, params) + # nor is an approach landing after one the flight ends at flown + assert not plane_track.uses_vtol_approach( + [items[0], landing, self.vtol_land(0, 0, option=1)], params) + # and a mission nothing can make sense of has no path at all, so + # asking it about the wind is answered rather than raising + nonsense = [items[0], (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, + (float('nan'), 1, 0, 0))] + assert not plane_track.uses_vtol_approach(nonsense, params) + assert plane_track.mission_track(HOME, nonsense, params) is None + + def test_a_return_to_launch_can_land_so(self): + """Q_RTL_MODE 2: circle home at RTL_ALTITUDE, go down to Q_RTL_ALT, + and come in on the approach""" + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + items = [waypoint(0, 2000, 150), rtl] + params = dict(self.QUADPLANE, Q_RTL_MODE=2, Q_FW_LND_APR_RAD=200) + assert plane_track.uses_vtol_approach(items, params) + assert not plane_track.uses_vtol_approach(items, self.QUADPLANE) + track = plane_track.mission_track(HOME, items, params, approach=45.0) + (_, bearing, course, before) = self.landed(track) + assert course == pytest.approx(45.0, abs=30.0) + assert mp_util.wrap_180(bearing - 225.0) == pytest.approx(0, abs=20) + # it was round home at RTL_ALTITUDE before coming down + assert any(150 < math.hypot(*local(p)) < 300 and + p[2] > HOME[2] + 90.0 for p in track[len(track) // 2:]) + assert track[-1][2] == pytest.approx(HOME[2] + 15.0, abs=1.0) + assert math.hypot(*local(track[-1])) < 0.5 + + +class TestMissionFlight(object): + '''ArduPlane's mission logic, flown''' + + def test_it_flies_the_legs_between_waypoints(self): + track = fly([waypoint(3000, 0), waypoint(3000, 3000)]) + points = [local(p) for p in up_to(track, 3000, 3000)] + # tracking the first leg north, well away from its ends + on_leg = [p for p in points if 800 < p[0] < 2200 and p[1] < 1500] + assert on_leg + assert max(abs(p[1]) for p in on_leg) < 3.0 + # and it ends at the last one, which is done with once it is within + # the distance it would start turning for whatever came next + assert math.hypot(points[-1][0] - 3000, points[-1][1] - 3000) < 100.0 + + def test_a_turn_is_cut_short_of_the_waypoint(self): + track = fly([waypoint(3000, 0), waypoint(3000, 3000)]) + points = [local(p) for p in track] + nearest = min(math.hypot(p[0] - 3000, p[1]) for p in points) + # a 90 degree turn is started WP_RADIUS or the L1 distance out + assert nearest > 20.0 + + def test_a_leg_into_a_loiter_tracks_towards_its_centre(self): + # out of one loiter and into another, the track is the line between + # their centres rather than a tangent from one circle to the other, + # until the aircraft is within three radii of the second centre: it + # leaves the first circle a radius to one side of that line, and + # pulls onto it + first = (0.0, 2000.0) + second = (6000.0, 2000.0) + track = fly([loiter_turns(first[0], first[1], 1, 300), + loiter_turns(second[0], second[1], 1, 150), + waypoint(9000, 2000)]) + points = [local(p) for p in track] + radius = plane_track.L1Control(15, 0.75, 0, 0).loiter_radius(150, 684, 22) + approach = [p for p in points + if 2500 < p[0] < second[0] - 3 * radius - 300 and + abs(p[1] - 2000) < 500] + assert approach + assert max(abs(distance_to_line(p, first, second)) for p in approach) < 5.0 + + def test_a_loiter_is_left_lined_up_on_what_comes_next(self): + centre = (0.0, 3000.0) + after = (3000.0, 3000.0) + track = fly([loiter_turns(centre[0], centre[1], 1, 200), + waypoint(*after)]) + points = [local(p) for p in up_to(track, *after)] + radius = plane_track.L1Control(15, 0.75, 0, 0).loiter_radius(200, 684, 22) + leaving = [p for p in points + if p[0] > 400 and math.hypot(p[0] - centre[0], p[1] - centre[1]) > radius + 50] + assert leaving + # the leg is flown against the track out of the loiter's centre, so it + # ends up on the line from the centre to the waypoint + late = [p for p in leaving if 1500 < p[0] < 2500] + assert max(abs(distance_to_line(p, centre, after)) for p in late) < 5.0 + + def test_a_loiter_lines_up_on_an_item_with_no_position_as_stored(self): + # Plane::verify_loiter_heading heads for the next item's location as + # the mission holds it, which is 0, 0 for one with no position: only + # starting that item puts it where the aircraft is. From here that + # is west-north-west, as Location::get_distance_NE has it + here = (mavlink.MAV_CMD_NAV_LOITER_TURNS, 0.0, 0.0, HOME[2] + 100, + (1, 0, 80, 0)) + (clat, clon) = offset(1000, 0)[:2] + scale = math.cos(math.radians(clat / 2.0)) + towards = math.degrees(math.atan2(-clon * scale, -clat)) + for radius in (100, -100): + flight = plane_track.MissionFlight( + (HOME[0], HOME[1]), HOME, + [loiter_turns(1000, 0, 1, radius, xtrack=1), here], PARAMS) + courses = [] + real = flight.fly_loiter + + def recording(index): + if index == 1: + courses.append(math.degrees(flight.yaw)) + return real(index) + flight.fly_loiter = recording + assert flight.run() is not None + (course,) = courses + # within ArduPlane's 10 degrees, and the aircraft's turn since + assert mp_util.wrap_180(course - towards) == pytest.approx( + 0, abs=15) + + def test_param4_crosstracks_from_where_the_loiter_is_left(self): + centre = (0.0, 3000.0) + after = (3000.0, 3000.0) + from_centre = [local(p) for p in up_to(fly( + [loiter_turns(centre[0], centre[1], 1, 200), waypoint(*after)]), + *after)] + from_exit = [local(p) for p in up_to(fly( + [loiter_turns(centre[0], centre[1], 1, 200, xtrack=1), waypoint(*after)]), + *after)] + # from the exit the aircraft does not pull back onto the centre line, + # so halfway along the leg it is still well to one side of it + halfway = [p for p in from_exit if 1400 < p[0] < 1600] + assert halfway + assert min(abs(distance_to_line(p, centre, after)) for p in halfway) > 50.0 + halfway = [p for p in from_centre if 1400 < p[0] < 1600] + assert max(abs(distance_to_line(p, centre, after)) for p in halfway) < 5.0 + + def test_loiters_turn_the_way_their_radius_says(self): + for (radius, clockwise) in ((200, True), (-200, False)): + points = [local(p) for p in fly( + [loiter_turns(0, 3000, 2, radius), waypoint(3000, 3000)])] + circling = [p for p in points + if math.hypot(p[0], p[1] - 3000) < 400] + swept = 0.0 + for (a, b) in zip(circling, circling[1:]): + swept += mp_util.wrap_180( + math.degrees(math.atan2(b[1] - 3000, b[0])) - + math.degrees(math.atan2(a[1] - 3000, a[0]))) + # a bearing from the centre measured east of north grows clockwise + if clockwise: + assert swept > 600 + else: + assert swept < -600 + + def test_a_loiter_to_altitude_circles_until_it_gets_there(self): + climb = 1000.0 + points = fly([loiter_to_alt(0, 3000, 100 + climb, 150), + waypoint(3000, 3000, 100 + climb)]) + centre = offset(0, 3000) + on_circle = [p for p in points + if mp_util.gps_distance(p[0], p[1], centre[0], centre[1]) < 260] + # climbing at 5 m/s it spends at least climb / 5 seconds up there + # (less what it climbs on the way), a circle every 2 pi r / v seconds + laps = len(on_circle) * plane_track.POINT_SPACING / (2 * math.pi * 170) + assert laps > 2.0 + assert max(p[2] for p in points) == pytest.approx(HOME[2] + 100 + climb, abs=6.0) + + def test_a_faster_airspeed_takes_a_wider_line_through_a_turn(self): + def overshoot(speed): + items = [waypoint(3000, 0), waypoint(3000, 3000)] + if speed is not None: + items.insert(0, change_speed(speed)) + points = [local(p) for p in fly(items)] + return max(p[0] for p in points) + assert overshoot(35.0) > overshoot(None) + 10.0 + + def test_only_an_airspeed_within_its_limits_changes_the_speed(self): + def overshoot(params, change=None): + items = [waypoint(3000, 0), waypoint(3000, 3000)] + if change is not None: + items.insert(0, (mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 0, None, + change)) + return max(local(p)[0] for p in fly(items, params)) + params = PARAMS + plain = overshoot(params) + assert overshoot(params, (0, 35, -1, 0)) > plain + 10.0 + # a groundspeed is only a minimum ground speed to ArduPlane + assert overshoot(params, (1, 35, -1, 0)) == pytest.approx(plain) + # and it will not take an airspeed outside AIRSPEED_MIN/MAX + assert overshoot(dict(params, AIRSPEED_MAX=30), + (0, 35, -1, 0)) == pytest.approx(plain) + assert overshoot(params, (0, 5, -1, 0)) == pytest.approx(plain) + # -2 goes back to AIRSPEED_CRUISE + assert overshoot(params, (0, -2, -1, 0)) == pytest.approx(plain) + + def test_a_loiter_unlimited_ends_the_flight_on_its_circle(self): + (lat, lon, amsl) = offset(0, 3000) + track = fly([(mavlink.MAV_CMD_NAV_LOITER_UNLIM, lat, lon, amsl, + (0, 0, 150, 0)), waypoint(3000, 3000)]) + last = track[-1] + radius = mp_util.gps_distance(last[0], last[1], lat, lon) + assert 120 < radius < 220 + + def test_what_it_cannot_fly_is_not_drawn(self): + delay = (mavlink.MAV_CMD_NAV_DELAY, 0, 0, None, (10, 0, 0, 0)) + assert plane_track.mission_track( + HOME, [waypoint(3000, 0), delay, waypoint(3000, 3000)], PARAMS) is None + assert plane_track.mission_track( + (0, 0, 0), [waypoint(3000, 0)], PARAMS) is None + + def test_a_waypoint_too_close_to_turn_for_is_passed(self): + # ArduPlane counts a waypoint done once the aircraft is past it along + # the leg, so one right beside the last does not leave it circling + track = fly([waypoint(3000, 0), waypoint(3000, 20), waypoint(3020, 20)], + dict(PARAMS, ROLL_LIMIT_DEG=10.0)) + assert len(track) > 10 + + def jump(self, target, repeats, command=mavlink.MAV_CMD_DO_JUMP): + return (command, 0, 0, None, (target, repeats, 0, 0)) + + def visits(self, track, north, east, within=80.0): + '''how many separate times the track passes near a point''' + (lat, lon, _) = offset(north, east) + count = 0 + near = False + for p in track: + close = mp_util.gps_distance(p[0], p[1], lat, lon) < within + if close and not near: + count += 1 + near = close + return count + + def test_a_jump_is_followed(self): + # 1 and 2 are a circuit's first two corners; 3 jumps past a + # waypoint which the mission only reaches by the jump back at 6 + items = [waypoint(2000, 0), waypoint(2000, 2000), + self.jump(5, -1), waypoint(-3000, -3000), + waypoint(0, 2000), self.jump(4, 1), waypoint(0, 4000)] + track = up_to(fly(items), 0, 4000) + # never flown straight from 2 to 4: 4 is reached once, by the jump + # back at 6, and 5 is flown before and after it + assert self.visits(track, -3000, -3000) == 1 + assert self.visits(track, 0, 2000) == 2 + assert self.visits(track, 0, 4000) == 1 + + def test_a_jump_is_drawn_once_however_often_it_repeats(self): + items = [waypoint(2000, 0), waypoint(2000, 2000), waypoint(0, 2000), + self.jump(1, 50), waypoint(0, 4000)] + track = fly(items) + assert self.visits(track, 2000, 2000) == 2 + assert self.visits(track, 0, 4000) == 1 + + def test_a_jump_repeated_for_ever_ends_the_flight(self): + # the aircraft never gets past it: the circuit is drawn going round + # the once, and nothing after the jump is drawn + items = [waypoint(2000, 0), waypoint(2000, 2000), waypoint(0, 2000), + self.jump(1, -1), waypoint(0, 4000)] + track = fly(items) + assert self.visits(track, 2000, 2000) == 2 + assert self.visits(track, 0, 4000) == 0 + + def test_a_jump_with_no_repeats_is_not_followed(self): + # followed, it would skip the waypoint straight after it + items = [waypoint(2000, 0), self.jump(4, 0), waypoint(2000, 2000), + waypoint(0, 2000)] + assert self.visits(fly(items), 2000, 2000) == 1 + + def test_a_jump_to_a_tag(self): + tag = (600, 0, 0, None, (7, 0, 0, 0)) + items = [waypoint(2000, 0), self.jump(7, 1, command=601), + waypoint(-3000, -3000), tag, waypoint(0, 2000)] + track = fly(items) + assert self.visits(track, -3000, -3000) == 0 + assert self.visits(track, 0, 2000) == 1 + + def test_a_jump_nowhere_ends_the_mission(self): + # AP_Mission finds no next command, and the mission is over: the + # aircraft returns to launch + track = fly([waypoint(2000, 0), self.jump(9, 1), waypoint(0, 2000)]) + assert self.visits(track, 2000, 0) == 1 + assert self.visits(track, 0, 2000) == 0 + assert math.hypot(*local(track[-1])) < PARAMS['WP_LOITER_RAD'] * 1.8 + + def test_a_completed_mission_returns_to_launch(self): + # Plane::exit_mission_callback changes mode to RTL, which circles home + track = fly([waypoint(3000, 0), waypoint(3000, 3000)]) + assert self.visits(track, 3000, 3000) == 1 + circling = [local(p) for p in track[-20:]] + for p in circling: + assert math.hypot(*p) <= PARAMS['WP_LOITER_RAD'] * 1.8 + + def test_a_speed_change_before_a_mission_runs_out_is_flown_home_at(self): + """the items passed on the way to a mission's end are done before + the return which follows it: a DO_CHANGE_SPEED among them is the + speed the return is flown at, so its turn for home is wider or + tighter than it would have been""" + def turn(speed): + items = [waypoint(1500, 0), waypoint(1500, 900)] + if speed is not None: + items.append(change_speed(speed)) + track = fly(items, dict(PARAMS, RTL_RADIUS=200)) + return max(local(p)[1] for p in track) + unchanged = turn(None) + assert turn(30.0) > unchanged + 25.0 + assert turn(12.0) < unchanged - 20.0 + + def test_waypoints_closer_than_a_turn_are_passed_on_the_way_home(self): + # a box smaller than the distance a turn is started from: each + # waypoint is done with as soon as it is started, and the mission is + # over in moments, so the path is the return to launch's circle + box = [waypoint(50, -5), waypoint(50, 45), waypoint(0, 45), + waypoint(0, -5)] + track = fly(box, dict(PARAMS, WP_RADIUS=90.0)) + assert len(track) > 20 + far = max(math.hypot(*local(p)) for p in track) + assert far > PARAMS['WP_LOITER_RAD'] * 0.9 + + def test_a_loop_with_nothing_to_navigate_to_is_flown_past(self): + """AP_Mission gives up on meeting a jump twice over only while it is + looking ahead; advancing, it runs the jump's repeats out and carries + on past it, so a loop with no item to navigate to in it does not end + the mission""" + after = (3000, 0) + for loop in ([self.jump(2, 3)], + [change_speed(25), self.jump(2, 3)]): + items = [waypoint(1000, 0)] + loop + [waypoint(*after)] + assert self.visits(fly(items), *after) == 1 + + def test_a_jump_asking_for_fewer_than_for_ever_is_not_followed(self): + """AP_Mission repeats for ever on -1 alone, and never follows a jump + asking for fewer than that""" + items = [waypoint(1000, 0), self.jump(1, -2), waypoint(3000, 0)] + assert self.visits(fly(items), 3000, 0) == 1 + # where -1 is flown round the loop the once and ends there + items = [waypoint(1000, 0), self.jump(1, -1), waypoint(3000, 0)] + assert self.visits(fly(items), 3000, 0) == 0 + + def test_an_item_with_no_altitude_at_all_holds_the_one_flown(self): + """Plane::set_next_WP takes a zero altitude, in whatever frame, as + the altitude the aircraft is at""" + for frame in (0, 3, 10): + assert plane_track.positionless_amsl(0.0, frame, HOME[2]) is None + # where an altitude of its own is flown at + assert plane_track.positionless_amsl(700.0, 0, HOME[2]) == 700.0 + assert plane_track.positionless_amsl(100.0, 3, HOME[2]) == HOME[2] + 100 + + def test_a_mission_which_loops_for_ever_does_not_return(self): + items = [waypoint(2000, 0), waypoint(2000, 2000), waypoint(0, 2000), + self.jump(1, -1)] + track = fly(items) + # it ends going round the loop, not circling home + assert math.hypot(*local(track[-1])) > 1000 + + def takeoff(self, alt=50.0, command=mavlink.MAV_CMD_NAV_TAKEOFF): + # a takeoff item placed at home, as a ground station puts it + return (command, HOME[0], HOME[1], HOME[2] + alt, (0, 0, 0, 0)) + + def climbed_out(self, track, alt=50.0): + '''metres from home where the track first reaches the takeoff + altitude''' + for p in track: + if p[2] >= HOME[2] + alt - 0.5: + return mp_util.gps_distance(HOME[0], HOME[1], p[0], p[1]) + return None + + def takeoff_at(self, pitch, alt=50.0): + return (mavlink.MAV_CMD_NAV_TAKEOFF, HOME[0], HOME[1], HOME[2] + alt, + (pitch, 0, 0, 0)) + + def test_a_fixed_wing_takeoff_climbs_at_its_pitch(self): + # Plane::takeoff_calc_pitch: with no airspeed sensor the aircraft + # holds the item's pitch, 10 degrees here, and 4 where it gives none + for (pitch, flown) in ((10, 10), (0, 4)): + track = fly([self.takeoff_at(pitch), waypoint(3000, 0)]) + expected = 50.0 / math.tan(math.radians(flown)) + assert self.climbed_out(track) == pytest.approx(expected, rel=0.1) + (lat, lon, _) = up_to(track, 3000, 0)[-1] + assert mp_util.gps_bearing(HOME[0], HOME[1], lat, lon) == pytest.approx(0.0, abs=5.0) + + def test_a_sensor_the_throttle_turns_off_is_not_flown_by(self): + # ARSPD_USE 2 is a glider's sensor behind the propeller, which is + # not used while the throttle runs: the climb out is its pitch's + track = fly([self.takeoff_at(10), waypoint(3000, 0)], + dict(AIRSPEED_SENSOR, ARSPD_USE=2)) + assert self.climbed_out(track) == pytest.approx( + 50.0 / math.tan(math.radians(10)), rel=0.1) + + def test_a_takeoff_with_an_airspeed_sensor_climbs_out_at_full_rate(self): + # TECS flies it no lower than its pitch: at TECS_CLMB_MAX, 50m at + # 5m/s is ten seconds at 22m/s, not a slow approach to the altitude + track = fly([self.takeoff(), waypoint(3000, 0)], AIRSPEED_SENSOR) + assert self.climbed_out(track) == pytest.approx(220.0, abs=40.0) + # but a steeper pitch than that is still flown + track = fly([self.takeoff_at(20), waypoint(3000, 0)], AIRSPEED_SENSOR) + assert self.climbed_out(track) == pytest.approx( + 50.0 / math.tan(math.radians(20)), rel=0.1) + + def test_a_fixed_wing_takeoff_heads_for_the_next_waypoint(self): + # it holds whatever course it was launched or rolled down the + # runway on, which the way it pointed beforehand says little about: + # north, for the waypoint, whatever heading it is given + for heading in (None, 90.0): + track = plane_track.mission_track( + HOME, [self.takeoff(), waypoint(3000, 0)], AIRSPEED_SENSOR, + heading=heading) + climbed = [local(p) for p in track if p[2] >= HOME[2] + 49.5][0] + assert climbed[0] == pytest.approx(220.0, abs=40.0) + assert abs(climbed[1]) < 5.0 + # where a VTOL takeoff transitions the way it points + track = plane_track.mission_track( + HOME, [self.takeoff(command=mavlink.MAV_CMD_NAV_VTOL_TAKEOFF), + waypoint(3000, 0)], dict(PARAMS, Q_ENABLE=1), heading=90.0) + assert local(track[2])[1] > 10.0 + + def test_a_fixed_wing_takeoff_flies_the_course_it_was_flown_on(self): + # a caller which has a flight of the takeoff to measure knows the + # course it held, which the mission does not say + for course in (90.0, 270.0): + track = plane_track.mission_track( + HOME, [self.takeoff(), waypoint(3000, 0)], AIRSPEED_SENSOR, + heading=0.0, takeoff_course=course) + climbed = [local(p) for p in track if p[2] >= HOME[2] + 49.5][0] + east = 220.0 if course == 90.0 else -220.0 + assert climbed[1] == pytest.approx(east, abs=40.0) + assert abs(climbed[0]) < 5.0 + + def test_a_quadplane_takes_off_straight_up(self): + params = dict(PARAMS, Q_ENABLE=1, Q_OPTIONS=0) + for command in (mavlink.MAV_CMD_NAV_TAKEOFF, + mavlink.MAV_CMD_NAV_VTOL_TAKEOFF): + # and sets off from overhead home towards the waypoint behind it + track = up_to(fly([self.takeoff(command=command), + waypoint(-3000, 0)], params), -3000, 0) + assert self.climbed_out(track) == pytest.approx(0.0, abs=1.0) + assert max(local(p)[0] for p in track) < 5.0 + # unless Q_OPTIONS lets NAV_TAKEOFF be a fixed-wing one + track = fly([self.takeoff(), waypoint(3000, 0)], + dict(params, Q_OPTIONS=plane_track.Q_OPTION_ALLOW_FW_TAKEOFF)) + assert self.climbed_out(track) > 100.0 + + def test_a_mission_started_away_from_home(self): + # ArduPlane starts where the aircraft is when AUTO starts; home is + # only where it came from + start = offset(-1000, 500, 20) + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), waypoint(3000, 3000)], PARAMS, + start=start) + assert track[0] == pytest.approx(start) + # home still stands for home: a return to launch goes back there + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), rtl], PARAMS, start=start) + home_ward = [p for p in track + if mp_util.gps_distance(p[0], p[1], + HOME[0], HOME[1]) < 200.0] + assert home_ward + + def test_a_return_to_launch_circles_where_it_returns_to(self): + """ModeRTL::update circles at RTL_RADIUS, or WP_LOITER_RAD""" + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + for (rtl_radius, radius, clockwise) in ((0.0, 60.0, True), + (250.0, 250.0, True), + (-250.0, 250.0, False)): + params = dict(PARAMS, RTL_RADIUS=rtl_radius) + points = [local(p) for p in + fly([waypoint(3000, 0), rtl], params=params)] + # once it is home for the last time, it is circling there + home_ward = [i for (i, p) in enumerate(points) + if math.hypot(p[0], p[1]) > 600][-1] + circling = points[home_ward:] + circling = circling[len(circling) // 2:] + # at the radius asked for, allowing the room an aircraft which + # cannot bank that tightly takes + for p in circling: + assert radius <= math.hypot(p[0], p[1]) <= radius * 1.8 + swept = 0.0 + for (a, b) in zip(circling, circling[1:]): + swept += mp_util.wrap_180( + math.degrees(math.atan2(b[1], b[0])) - + math.degrees(math.atan2(a[1], a[0]))) + # a bearing from the centre grows clockwise + assert swept > 120 if clockwise else swept < -120 + + def test_a_return_to_launch_joins_its_circle(self): + # ArduPlane circles from the start of an RTL, so the aircraft turns + # onto the circle rather than flying over its middle first + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + + def closest_on_return(params): + distances = [math.hypot(*local(p)) + for p in fly([waypoint(3000, 0), rtl], params=params)] + return min(distances[distances.index(max(distances)):]) + for (params, radius) in ((dict(PARAMS, RTL_RADIUS=250), 250.0), + (PARAMS, PARAMS['WP_LOITER_RAD'])): + assert closest_on_return(params) > radius * 0.9 + # a plane given the parameter which only a QuadPlane takes is still + # one of those + assert closest_on_return( + dict(PARAMS, RTL_RADIUS=250, Q_RTL_MODE=1)) > 225.0 + + def test_a_quadplane_returns_to_land(self): + '''QRTL, which Q_RTL_MODE 1 switches to on the way home and 3 goes + to at once, lands where it returns to rather than circling''' + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + for (mode, alt) in ((1, 100.0), (3, 15.0)): + params = dict(PARAMS, RTL_RADIUS=250, Q_ENABLE=1, Q_RTL_MODE=mode) + track = fly([waypoint(3000, 0), rtl], params=params) + assert math.hypot(*local(track[-1])) < 5.0 + # QRTL comes home at Q_RTL_ALT, and RTL at RTL_ALTITUDE + assert track[-1][2] == pytest.approx(HOME[2] + alt, abs=10.0) + # and it never went round + assert all(local(p)[0] > -20.0 for p in track) + + def test_qrtl_comes_home_on_its_own_altitude_profile(self): + '''QRTL, which Q_RTL_MODE 3 goes to at once, comes in at + RTL_ALTITUDE, not down a slope to Q_RTL_ALT, and drops to that only + over the stretch its sink rate needs (ModeQRTL:: + update_target_altitude)''' + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + params = dict(PARAMS, RTL_RADIUS=250, Q_ENABLE=1, Q_RTL_MODE=3, + RTL_ALTITUDE=100, Q_RTL_ALT=15) + + def heights(start_alt): + track = fly([waypoint(3000, 0, start_alt), rtl], params=params) + turned = next(i for (i, p) in enumerate(track) + if local(p)[0] > 2900) + return [(math.hypot(*local(p)), p[2] - HOME[2]) + for p in track[turned:]] + + def at(heights, distance): + return next(h for (d, h) in heights if d < distance) + # from RTL_ALTITUDE: 2 * RTL_RADIUS, plus 85m at 3m/s (0.6 of + # TECS_SINK_MAX) and AIRSPEED_CRUISE, out: 500m + 22m/s * 28.3s + level = heights(100) + assert all(h == pytest.approx(100, abs=1) for (d, h) in level + if 1150 < d < 2500) + assert 30 < at(level, 700) < 90 + assert at(level, 20) == pytest.approx(15, abs=5) + # from higher, down a slope to RTL_ALTITUDE by then + high = heights(300) + assert 150 < at(high, 2000) < 270 + assert at(high, 1150) == pytest.approx(100, abs=20) + assert at(high, 20) == pytest.approx(15, abs=5) + + def test_qrtl_does_not_climb_back_up_to_its_slope(self): + '''an aircraft which has got below QRTL's slope down to + RTL_ALTITUDE is followed down, not sent back up to it''' + params = dict(PARAMS, RTL_RADIUS=250, Q_ENABLE=1, Q_RTL_MODE=3, + RTL_ALTITUDE=100, Q_RTL_ALT=15) + flight = plane_track.MissionFlight((HOME[0], HOME[1]), HOME, [], + params) + flight.qrtl = True + flight.next_wp = (flight.home, HOME[2] + 15) + # the approach began 3km out, 300m up, and the slope from there + # is at 200m 2km out, where the aircraft is only 150m up + flight.qrtl_start = (285.0, 3000.0) + flight.position = (flight.home[0] + 2000.0, flight.home[1]) + flight.amsl = HOME[2] + 150 + flight.update_target_altitude() + assert flight.target_amsl == pytest.approx(HOME[2] + 150) + # and never below RTL_ALTITUDE while out there + flight.amsl = HOME[2] + 50 + flight.update_target_altitude() + assert flight.target_amsl == pytest.approx(HOME[2] + 100) + + def test_the_first_item_navigated_to(self): + items = [change_speed(30), self.jump(4, 1), waypoint(2000, 0), + waypoint(3000, 0)] + assert plane_track.first_navigation_item(items) == 4 + assert plane_track.first_navigation_item([change_speed(30)]) is None + # a mission nothing can make sense of has no first item to navigate + # to either, as it has no path: the caller is told so rather than + # having the item raised at it + nonsense = [self.jump(float('nan'), 1), waypoint(2000, 0)] + assert plane_track.first_navigation_item(nonsense) is None + assert plane_track.mission_track(HOME, nonsense, PARAMS) is None + + def test_a_mission_across_the_antimeridian(self): + import time + home = (10.0, 179.998, 0.0) + east = mp_util.gps_newpos(home[0], home[1], 90, 300) + assert east[1] < 0 + start = time.time() + track = fly([(mavlink.MAV_CMD_NAV_WAYPOINT, east[0], east[1], 100.0, + (0, 0, 0, 0))], home=home) + assert time.time() - start < 2.0 + # reaching the waypoint, having flown the short way round + distances = [mp_util.gps_distance(p[0], p[1], east[0], east[1]) + for p in track] + assert min(distances) < 100.0 + assert distances.index(min(distances)) < 100 + + def test_a_long_loiter_is_flown_through(self): + # its turns are not taken for being stuck + for (turns, radius) in ((100, 80), (40, 200)): + track = fly([loiter_turns(3000, 0, turns, radius), + waypoint(3000, 3000)]) + (lat, lon, _) = offset(3000, 3000) + end = up_to(track, 3000, 3000)[-1] + assert mp_util.gps_distance(end[0], end[1], lat, lon) < 100.0 + circled = sum(mp_util.gps_distance(a[0], a[1], b[0], b[1]) + for (a, b) in zip(track, track[1:])) + assert circled > turns * 2 * math.pi * radius + # nor is a long climb, longer than the leg to it and twenty laps + track = fly([loiter_to_alt(500, 0, 6000, radius=80), + waypoint(3000, 3000, 6000)]) + assert max(p[2] for p in track) >= HOME[2] + 5990 + + def test_a_jump_which_cannot_be_counted_is_not_flown(self): + items = [waypoint(2000, 0), + (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, + (float('nan'), 1, 0, 0)), + waypoint(0, 2000)] + assert plane_track.mission_track(HOME, items, PARAMS) is None + + def test_a_mission_too_long_to_fly_is_not(self): + import time + far = offset(2000 * 1000, 0) + start = time.time() + assert plane_track.mission_track( + HOME, [waypoint(3000, 0), + (mavlink.MAV_CMD_NAV_WAYPOINT, far[0], far[1], far[2], + (0, 0, 0, 0))], PARAMS) is None + assert time.time() - start < 0.5 + + def test_a_mission_too_long_to_fly_is_not_flown_at_all(self, monkeypatch): + # the check is there to save flying it: it must not be flown first + flights = [] + monkeypatch.setattr(plane_track.MissionFlight, 'run', + lambda self: flights.append(self)) + far = offset(2000 * 1000, 0) + assert plane_track.mission_track( + HOME, [waypoint(3000, 0), + (mavlink.MAV_CMD_NAV_WAYPOINT, far[0], far[1], far[2], + (0, 0, 0, 0))], PARAMS) is None + assert flights == [] + + def test_a_mission_is_measured_from_where_it_starts(self): + # the aircraft carried a long way from home before the mission + # starts flies from there, so it is no longer than it looks + start = offset(2000 * 1000, 0) + items = [(mavlink.MAV_CMD_NAV_WAYPOINT, start[0], start[1], start[2], + (0, 0, 0, 0))] + assert plane_track.least_flight_time( + HOME, items, PARAMS, start=start) < 60.0 + assert plane_track.least_flight_time(HOME, items, PARAMS) > 4 * 3600 + assert plane_track.mission_track(HOME, items, PARAMS, + start=start) is not None + assert plane_track.mission_track(HOME, items, PARAMS) is None + + def test_a_jump_to_the_last_item_is_followed(self): + # items are numbered from home, which is not among them, so the + # last item is numbered as many as there are + items = [waypoint(2000, 0), waypoint(2000, 2000), + (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (3, 1, 0, 0)), + waypoint(0, 2000)] + flight = plane_track.MissionFlight((HOME[0], HOME[1]), HOME, + list(items), PARAMS) + assert flight.jump_target(2) == 2 + # and one past it goes nowhere + beyond = list(items) + beyond[2] = (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (5, 1, 0, 0)) + flight = plane_track.MissionFlight((HOME[0], HOME[1]), HOME, + list(beyond), PARAMS) + assert flight.jump_target(2) is None + + def test_the_flight_time_limit_holds_inside_an_item(self, monkeypatch): + # a mission short enough in straight lines, but which takes longer + # than the limit to fly, stops at the limit rather than flying on to + # the end of the item it is in + longest = [] + real = plane_track.MissionFlight.fly + + def timed(flight, *args, **kwargs): + real(flight, *args, **kwargs) + longest.append(flight.time) + monkeypatch.setattr(plane_track.MissionFlight, 'fly', timed) + monkeypatch.setattr(plane_track, 'MAX_FLIGHT_TIME', 250.0) + items = [waypoint(8000, 0)] + assert plane_track.mission_track(HOME, items, PARAMS) is None + assert longest and max(longest) < 250.0 + plane_track.NAV_PERIOD * 2 + monkeypatch.setattr(plane_track, 'MAX_FLIGHT_TIME', 3600.0) + assert plane_track.mission_track(HOME, items, PARAMS) is not None + + def test_a_return_to_launch_climbs_to_rtl_altitude(self): + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + items = [waypoint(3000, 0, alt=40), rtl] + + def arrives(params=None): + return fly(items, dict(PARAMS, **(params or {})))[-1][2] - HOME[2] + # ArduPlane's default is 100m above home; the height follows the + # glide slope home a few seconds behind + assert arrives() == pytest.approx(100, abs=10.0) + assert arrives({'RTL_ALTITUDE': 250}) == pytest.approx(250, abs=15.0) + # and a negative one comes home at the altitude it is at + assert arrives({'RTL_ALTITUDE': -1}) == pytest.approx(40, abs=3.0) + + def climbed_by(self, track, north): + '''metres above home the track has climbed to by the time it is so + far north''' + return [p for p in track if local(p)[0] >= north][0][2] - HOME[2] + + def test_a_return_to_launch_climbs_at_once(self): + # Plane::setup_alt_slope: RTL slopes only down to where it goes, and + # climbs to it as fast as it can + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + track = fly([waypoint(-3000, 0, alt=40), rtl], + dict(PARAMS, RTL_ALTITUDE=200)) + home_ward = track[track.index(up_to(track, -3000, 0)[-1]):] + # 160m at 5m/s is 32s, or 700m at 22m/s, not the whole way home + assert self.climbed_by(home_ward, -2000) > 190 + + def test_a_climb_in_auto_is_sloped_unless_asked_otherwise(self): + items = [waypoint(1000, 0, alt=30), waypoint(4000, 0, alt=200)] + # CLIMB_SLOPE_HGT and above: along the leg, halfway up it halfway + sloped = fly(items) + assert self.climbed_by(sloped, 2500) == pytest.approx(115, abs=15) + # FLIGHT_OPTIONS' IMMEDIATE_CLIMB_IN_AUTO climbs as fast as it can + at_once = fly(items, dict( + PARAMS, FLIGHT_OPTIONS=plane_track.FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO)) + assert self.climbed_by(at_once, 2500) > 190 + # and is still sloped on the way down + down = [waypoint(1000, 0, alt=200), waypoint(4000, 0, alt=30)] + for params in (PARAMS, dict( + PARAMS, FLIGHT_OPTIONS=plane_track.FLIGHT_OPTION_IMMEDIATE_CLIMB_IN_AUTO)): + assert self.climbed_by(fly(down, params), 2500) == pytest.approx( + 115, abs=15) + + def test_a_loiter_time_holds_to_the_flight_time_limit(self, monkeypatch): + longest = [] + real = plane_track.MissionFlight.fly + + def timed(flight, *args, **kwargs): + real(flight, *args, **kwargs) + longest.append(flight.time) + monkeypatch.setattr(plane_track.MissionFlight, 'fly', timed) + monkeypatch.setattr(plane_track, 'MAX_FLIGHT_TIME', 250.0) + (lat, lon, amsl) = offset(2200, 0) + items = [(mavlink.MAV_CMD_NAV_LOITER_TIME, lat, lon, amsl, + (100, 0, 0, 0)), waypoint(2200, 300)] + params = dict(PARAMS, AIRSPEED_CRUISE=12.0, AIRSPEED_MAX=22.0) + # plainly short enough to try, in straight lines + assert plane_track.least_flight_time(HOME, items, params) < 250.0 + assert plane_track.mission_track(HOME, items, params) is None + assert longest and max(longest) < 250.0 + plane_track.NAV_PERIOD * 2 + + def test_the_least_flight_time_follows_jumps(self): + # a jump straight over a waypoint a long way off: AP_Mission never + # goes there, so neither is it counted + far = offset(1000 * 1000, 0) + jump = (mavlink.MAV_CMD_DO_JUMP, 0, 0, None, (3, 1, 0, 0)) + items = [jump, (mavlink.MAV_CMD_NAV_WAYPOINT, far[0], far[1], far[2], + (0, 0, 0, 0)), waypoint(1000, 0)] + assert plane_track.least_flight_time(HOME, items, PARAMS) < 60.0 + assert plane_track.mission_track(HOME, items, PARAMS) is not None + # and without the jump, it is far too far + assert plane_track.least_flight_time(HOME, items[1:], PARAMS) > 20000.0 + + def rtl_end(self, rally, params=None): + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), rtl], dict(PARAMS, **(params or {})), + rally=rally) + return track[-1] + + def test_a_return_to_launch_goes_to_the_nearest_rally_point(self): + near = offset(2500, 500, 150) + further = offset(-2000, 0, 150) + end = self.rtl_end([further, near]) + assert mp_util.gps_distance(end[0], end[1], near[0], near[1]) < 150.0 + assert end[2] == pytest.approx(near[2], abs=15.0) + # unless it is beyond RALLY_LIMIT_KM, when it goes home + end = self.rtl_end([near], {'RALLY_LIMIT_KM': 0.3}) + assert mp_util.gps_distance(end[0], end[1], HOME[0], HOME[1]) < 150.0 + # or RALLY_INCL_HOME has it take home, being nearer + end = self.rtl_end([further], {'RALLY_INCL_HOME': 1}) + assert mp_util.gps_distance(end[0], end[1], HOME[0], HOME[1]) < 150.0 + end = self.rtl_end([further]) + assert mp_util.gps_distance(end[0], end[1], further[0], + further[1]) < 150.0 + + def test_turns_are_flown_as_ap_mission_keeps_them(self): + def turns(n): + return fly([loiter_turns(2000, 0, n, 200), waypoint(2000, 3000)]) + # whole turns, the fraction dropped, as AP_Mission stores them + assert turns(2.5) == turns(2.0) + assert turns(2.5) != turns(3.0) + + def test_a_loiter_time_flies_the_vehicles_radius_whatever_it_asks(self): + def loiter(param3): + (lat, lon, amsl) = offset(2000, 0) + return fly([(mavlink.MAV_CMD_NAV_LOITER_TIME, lat, lon, amsl, + (60, 0, param3, 0)), waypoint(2000, 3000)]) + assert loiter(300) == loiter(0) + assert loiter(-300) != loiter(0) + + def test_no_glide_slope_with_alt_slope_min_zero(self): + # a climb along a leg follows the slope to the waypoint, unless + # ALT_SLOPE_MIN is zero or less, when it climbs as fast as it can + items = [waypoint(0, 50, alt=100), waypoint(6000, 50, alt=400)] + + def halfway(params): + track = fly(items, dict(PARAMS, CLIMB_SLOPE_HGT=0, **params)) + return min((abs(local(p)[0] - 3000), p[2]) for p in track)[1] + sloped = halfway({}) + assert sloped == pytest.approx(HOME[2] + 250, abs=25.0) + flat_out = halfway({'ALT_SLOPE_MIN': 0}) + assert flat_out == pytest.approx(HOME[2] + 400, abs=5.0) + + def test_the_altitude_of_a_rally_point(self): + # above home unless the flags say otherwise + assert plane_track.rally_amsl(100, 0, 584.0) == 684.0 + frame_valid = 1 << 2 + assert plane_track.rally_amsl(700, frame_valid | (0 << 3), 584.0) == 700.0 + assert plane_track.rally_amsl(100, frame_valid | (1 << 3), 584.0) == 684.0 + # above the EKF origin, and above the terrain under the point, + # which the caller looks up + assert plane_track.rally_amsl(100, frame_valid | (2 << 3), 584.0, + origin_amsl=500.0) == 600.0 + assert plane_track.rally_amsl(100, frame_valid | (2 << 3), 584.0) is None + assert plane_track.rally_amsl(100, frame_valid | (3 << 3), 584.0, + terrain_amsl=300.0) == 400.0 + assert plane_track.rally_amsl(100, frame_valid | (3 << 3), 584.0) is None + assert plane_track.rally_amsl(100, 0, None) is None + # only a point above home moves when home does + assert plane_track.rally_alt_is_fixed(0) is False + assert plane_track.rally_alt_is_fixed(frame_valid | (1 << 3)) is False + for frame in (0, 2, 3): + assert plane_track.rally_alt_is_fixed( + frame_valid | (frame << 3)) is True + + def test_a_rally_point_at_an_unknown_altitude_is_not_flown_to(self): + # terrain nobody has, most likely: how high the aircraft flies the + # return is anyone's guess, so the mission is not drawn flown + rtl = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, None, (0, 0, 0, 0)) + items = [waypoint(2000, 0), rtl] + (lat, lon, amsl) = offset(2200, 200) + assert plane_track.mission_track(HOME, items, PARAMS, + rally=[(lat, lon, amsl)]) is not None + assert plane_track.mission_track(HOME, items, PARAMS, + rally=[(lat, lon, None)]) is None + # one it does not go to does not matter + far = offset(2000 * 100, 0) + assert plane_track.mission_track( + HOME, items, PARAMS, rally=[(far[0], far[1], None)]) is not None + + def test_the_altitude_an_item_with_no_position_is_flown_at(self): + # Location::sanitize(): its own altitude, unless that is a relative 0 + assert plane_track.positionless_amsl(700.0, 0, 584.0) == 700.0 + assert plane_track.positionless_amsl(100.0, 3, 584.0) == 684.0 + assert plane_track.positionless_amsl(0.0, 3, 584.0) is None + assert plane_track.positionless_amsl(100.0, 3, None) is None + # the terrain under wherever the aircraft is, is not known here + assert plane_track.positionless_amsl(100.0, 10, 584.0) is None + assert plane_track.positionless_amsl(None, 0, 584.0) is None + + def test_without_parameters_arduplanes_defaults_fly(self): + track = plane_track.mission_track( + HOME, [waypoint(3000, 0), loiter_turns(3000, 3000, 1, 0)], {}) + assert track is not None + + +# SITL flights of ArduPilot autotest missions -- added by ArduPilot PR 34424, +# https://github.com/ArduPilot/ardupilot/pull/34424 -- recorded by +# missions/record_flight.py. All were flown by ArduPlane V4.8.0-dev, which +# each recording's source says: the drawing follows what a version of +# ArduPlane does, so one which changes how it navigates -- as the QRTL +# approach did in 4.8 -- may want the drawing changed and these flown +# again. Then how near the path drawn for each has to +# keep to it: the median and 90th percentile of the distance across from +# each point flown to the path drawn, the worst of them, and the worst after +# the first 30s, which takes in a QuadPlane's transition and a plane's +# takeoff; and the 90th percentile and worst of the height of each point +# flown above or below the nearest point of the path drawn +FLIGHTS = { + # QuadPlane.KalaupapaCanyonRun: loiters up and down over the sea, a + # winding gorge inland, a spiral up out of the head of the canyon, and + # down with the ground over a ridge and along the valley beyond + 'kalaupapa-canyon-run.json': (2.0, 8.0, 80.0, 30.0, 25.0, 45.0), + # Plane.MissionItemTypes: every kind of item a plane flies on the way, + # and a landing + 'sitl-plane-mission-items.json': (3.0, 16.0, 35.0, 35.0, 7.0, 22.0), + # a return to launch, which a rally point is nearer than home for + 'sitl-plane-rtl-rally.json': (10.0, 40.0, 55.0, 55.0, 11.0, 18.0), + # a loiter for ever + 'sitl-plane-loiter-unlimited.json': (2.0, 16.0, 30.0, 30.0, 12.0, 18.0), + # QuadPlane.VTOLMissionItemTypes: a VTOL landing on a fixed-wing + # approach, asked for by param1 + 'sitl-quadplane-vtol-land-approach.json': (7.0, 28.0, 45.0, 45.0, 7.0, 25.0), + # a VTOL landing straight in + 'sitl-quadplane-vtol-land.json': (2.0, 20.0, 40.0, 40.0, 6.0, 11.0), + # a VTOL landing on an approach, asked for by Q_OPTIONS. The flight + # met its circle already within the 5 degrees of the course it breaks + # out on, and broke out at once, where the path drawn went round once + # more: the approach is not modelled that finely + 'sitl-quadplane-vtol-land-q-options.json': (15.0, 42.0, 90.0, 90.0, 13.0, 16.0), + # a return to launch with each Q_RTL_MODE: switching to QRTL near home, + # landing on an approach, whose circle is flown a little wider than + # drawn, and as QRTL + 'sitl-quadplane-rtl-q-rtl-mode-1.json': (2.0, 15.0, 40.0, 40.0, 7.0, 12.0), + 'sitl-quadplane-rtl-q-rtl-mode-2.json': (13.0, 35.0, 70.0, 70.0, 7.0, 22.0), + 'sitl-quadplane-rtl-q-rtl-mode-3.json': (4.0, 30.0, 45.0, 45.0, 6.0, 11.0), +} + + +class TestFlownMission(object): + """the path worked out for a mission against a flight of it""" + + @pytest.fixture(params=sorted(FLIGHTS)) + def flight(self, request): + path = os.path.join(os.path.dirname(__file__), 'missions', + request.param) + with open(path) as f: + return (json.load(f), FLIGHTS[request.param]) + + @staticmethod + def recorder(): + import importlib.util + path = os.path.join(os.path.dirname(__file__), 'missions', + 'record_flight.py') + spec = importlib.util.spec_from_file_location('record_flight', path) + recorder = importlib.util.module_from_spec(spec) + spec.loader.exec_module(recorder) + return recorder + + def test_the_firmware_which_flew_a_recording_is_kept(self): + '''the drawing follows what a version of ArduPlane does, so a + recording of a flight says which flew it''' + recorder = self.recorder() + + class Log(object): + def __init__(self, *messages): + self.messages = list(messages) + + def rewind(self): + self.left = list(self.messages) + + def recv_match(self, type=None): + while self.left: + m = self.left.pop(0) + if type is None or m.get_type() in type: + return m + return None + + def message(kind, **fields): + m = types.SimpleNamespace(**fields) + m.get_type = lambda: kind + return m + started = message('MSG', Message='ArduPlane V4.8.0-dev (48c91a4c9e)') + other = message('MSG', Message='Frame: QUADPLANE') + version = message('VER', FWS='ArduPlane V4.8.0-dev', GH=0x48c91a4c) + # VER says which commit it was built from, wherever it comes: after + # the message the firmware starts with, as well as before it + for log in (Log(other, version, started), + Log(other, started, version)): + assert recorder.firmware(log) == 'ArduPlane V4.8.0-dev (48c91a4c)' + # an older log has only what the firmware said as it started + assert recorder.firmware(Log(other, started)) == \ + 'ArduPlane V4.8.0-dev (48c91a4c9e)' + # and a VER which does not name a commit is still the version + assert recorder.firmware(Log(message('VER', FWS='ArduPlane V4.8.0'))) \ + == 'ArduPlane V4.8.0' + # and one which says neither says nothing + assert recorder.firmware(Log(other)) is None + + def test_a_course_of_north_is_recorded(self): + recorder = self.recorder() + assert recorder.extras(0.0, None, [], 0.0) == { + 'heading': 0.0, 'approach': 0.0} + assert recorder.extras(None, (1, 2, 3), [(4, 5, 6)], None) == { + 'start': (1, 2, 3), 'rally': [(4, 5, 6)]} + + def test_a_loiter_to_altitude_with_no_radius_is_reached_at_the_default( + self): + # WP_LOITER_RAD 80: a point 100m from the centre is on its circle, + # which with 50 it is not, and one 60m off is + (lat, lon, amsl) = offset(1000, 0, 100) + items = [(mavlink.MAV_CMD_NAV_LOITER_TO_ALT, lat, lon, amsl, + (0, 0, 0, 0))] + path = [offset(0, 0, 100), offset(500, 0, 100), + offset(900, 0, 100), offset(1000, 60, 100)] + assert self.arrivals(path, items, dict(PARAMS, WP_LOITER_RAD=80)) == \ + [(1, 2)] + assert self.arrivals(path, items, dict(PARAMS, WP_LOITER_RAD=-50)) == \ + [(1, 3)] + # and one of no radius at all is ArduPlane's 60m + for none in (0, 1, -1): + assert self.arrivals(path, items, + dict(PARAMS, WP_LOITER_RAD=none)) == [(1, 3)] + + @staticmethod + def drawn(data): + '''the path drawn for a recorded flight's mission, given what + MAVExplorer gave it''' + items = [(c, la, lo, a, tuple(p)) for (c, la, lo, a, p) in data['items']] + rally = [tuple(point) for point in data.get('rally', [])] + start = data.get('start') + return (items, plane_track.mission_track( + tuple(data['home']), items, data['params'], + heading=data.get('heading'), + start=None if start is None else tuple(start), + rally=rally or None, approach=data.get('approach'), + takeoff_course=data.get('takeoff_course'))) + + def distances(self, track, flown): + '''metres from each flown point to the nearest leg of track, in + order of distance''' + return sorted(self.unsorted_distances(track, flown)) + + def unsorted_distances(self, track, flown): + '''metres from each flown point to the nearest leg of track''' + (lat0, lon0) = (track[0][0], track[0][1]) + scale = math.cos(math.radians(lat0)) + + def xy(p): + return ((p[0] - lat0) * 111319.5, (p[1] - lon0) * 111319.5 * scale) + segments = [(xy(a), xy(b)) for (a, b) in zip(track, track[1:])] + out = [] + for p in flown: + (pn, pe) = xy(p) + best = None + for ((an, ae), (bn, be)) in segments: + (dn, de) = (bn - an, be - ae) + squared = dn * dn + de * de + t = 0.0 + if squared > 0: + t = min(max(((pn - an) * dn + (pe - ae) * de) / squared, 0.0), 1.0) + d = math.hypot(pn - (an + t * dn), pe - (ae + t * de)) + if best is None or d < best: + best = d + out.append(best) + return out + + def heights(self, track, flown): + '''metres each flown point is above the nearest point of track, + nearest counting height as well, so a spiral is measured against + the turn of it at the height flown; below is negative''' + (lat0, lon0) = (track[0][0], track[0][1]) + scale = math.cos(math.radians(lat0)) + + def xyz(p): + return ((p[0] - lat0) * 111319.5, + (p[1] - lon0) * 111319.5 * scale, p[2]) + segments = [(xyz(a), xyz(b)) for (a, b) in zip(track, track[1:])] + out = [] + for p in flown: + q = xyz(p) + best = None + for (a, b) in segments: + d = [b[i] - a[i] for i in range(3)] + squared = sum(x * x for x in d) + t = 0.0 + if squared > 0: + t = sum((q[i] - a[i]) * d[i] for i in range(3)) / squared + t = min(max(t, 0.0), 1.0) + nearest = [a[i] + t * d[i] for i in range(3)] + distance = math.dist(q, nearest) + if best is None or distance < best[0]: + best = (distance, q[2] - nearest[2]) + out.append(best[1]) + return out + + # items which end a mission's flight: nothing after one is flown to + ENDINGS = (mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavlink.MAV_CMD_NAV_LAND, + mavlink.MAV_CMD_NAV_VTOL_LAND) + + def arrivals(self, path, items, flight_params): + '''for each item flown to, in mission order, the index into path of + the first point near it after the item before was reached: a + waypoint's is within the distance a turn is started out, a loiter's + within its circle, which is ArduPlane's default where it gives + none. An item reached out of turn leaves the ones after it + unreached, as None; so does a flight which ends short. + Items with no position, those ArduPlane skips, takeoffs, which the + flight starts from, VTOL landings, and anything after the item which + ends the flight, are left out''' + out = [] + after = 0 + for (seq, (command, lat, lon, amsl, params)) in enumerate(items, 1): + if seq > 1 and items[seq - 2][0] in self.ENDINGS: + break + if ((lat == 0 and lon == 0) or + command in plane_track.SKIPPED_COMMANDS or + command in plane_track.TAKEOFF_COMMANDS or + command == mavlink.MAV_CMD_NAV_VTOL_LAND): + continue + near = 120.0 + if command == mavlink.MAV_CMD_NAV_LOITER_TO_ALT: + radius = abs(params[1] or 0.0) + if radius <= 1: + radius = plane_track.default_loiter_radius( + plane_track.parameter(flight_params, 'WP_LOITER_RAD')) + near = radius * 1.4 + index = None + if after is not None: + for i in range(after, len(path)): + if mp_util.gps_distance(path[i][0], path[i][1], + lat, lon) < near: + index = i + break + out.append((seq, index)) + after = index + return out + + # a fixed-wing launch goes whichever way the aircraft is thrown or the + # runway points. Where the recording says which course it was flown on, + # the launch is drawn on that, and is held to as near as the rest of the + # flight; where it does not, it is drawn towards the first waypoint, + # anywhere within 45 degrees or so of the way flown, so until the + # aircraft gets there it can be that far across from the line drawn + LAUNCH_LIMIT = 10.0 + LAUNCH_SPREAD = math.sin(math.radians(45)) + 0.05 + LAUNCH_SLACK = 30.0 + + def launch(self, data, items, flown): + '''the flight split into its fixed-wing launch, up to where it gets + to the first waypoint after it, and the rest. A VTOL takeoff goes + up where it is, and is left with the rest''' + first = items[0][0] if items else None + if first not in plane_track.TAKEOFF_COMMANDS: + return ([], flown) + flight = plane_track.MissionFlight( + (0.0, 0.0), (0.0, 0.0, 0.0), list(items), data['params']) + if flight.vtol_takeoff(first): + return ([], flown) + arrived = [i for (seq, i) in self.arrivals(flown, items, data['params']) + if i is not None] + if not arrived: + return (flown, []) + return (flown[:arrived[0]], flown[arrived[0]:]) + + def length(self, path, start, end): + return sum(mp_util.gps_distance(a[0], a[1], b[0], b[1]) + for (a, b) in zip(path[start:end], path[start + 1:end + 1])) + + def test_the_path_is_flown_in_the_order_flown(self, flight): + # the distances alone would not notice legs flown in the wrong order, + # backwards or with extra loops, so long as the lines were there + (data, _) = flight + (items, track) = self.drawn(data) + flown = data['flown'] + drawn = self.arrivals(track, items, data['params']) + seen = self.arrivals(flown, items, data['params']) + # the flight kept ends short of the last waypoint + seen = [(seq, i) for (seq, i) in seen if i is not None] + order = [seq for (seq, i) in sorted(seen, key=lambda x: x[1])] + assert order == [seq for (seq, i) in seen] + assert None not in [i for (seq, i) in drawn] + assert ([seq for (seq, i) in sorted(drawn, key=lambda x: x[1])] == + [seq for (seq, i) in drawn]) + # and about as far between each of them as was flown: an extra lap + # of a loiter, or a leg flown twice, would not be + drawn = dict(drawn) + stages = [seq for (seq, i) in seen] + total_drawn = self.length(track, drawn[stages[0]], drawn[stages[-1]]) + total_flown = self.length(flown, dict(seen)[stages[0]], + dict(seen)[stages[-1]]) + assert total_drawn == pytest.approx(total_flown, rel=0.1) + seen = dict(seen) + for (a, b) in zip(stages, stages[1:]): + flown_stage = self.length(flown, seen[a], seen[b]) + if flown_stage < 1000: + continue + assert self.length(track, drawn[a], drawn[b]) == pytest.approx( + flown_stage, rel=0.2), (a, b) + + def test_the_path_is_the_one_flown(self, flight): + (data, (median_limit, p90_limit, worst_limit, later_limit, _, + _)) = flight + (items, track) = self.drawn(data) + assert track is not None + (launch, flown) = self.launch(data, items, data['flown']) + # the launch, drawn on the course flown where that is known, and + # otherwise only going the right general way + start = launch[0] if launch else None + known = data.get('takeoff_course') is not None + for (p, d) in zip(launch, self.unsorted_distances(track, launch)): + if known: + assert d < self.LAUNCH_LIMIT + continue + out = mp_util.gps_distance(start[0], start[1], p[0], p[1]) + assert d < self.LAUNCH_SPREAD * out + self.LAUNCH_SLACK + distances = self.distances(track, flown) + median = distances[len(distances) // 2] + p90 = distances[int(len(distances) * 0.9)] + # the tangent-and-circle drawing this replaced was 6m out at the + # median and 22m at the 90th percentile over the first flight of the + # canyon run + assert median < median_limit + assert p90 < p90_limit + # a QuadPlane climbs out of its VTOL takeoff and transitions on the + # way to the first item, and a plane rolls down the runway, neither + # of which is modelled; after that the path stays closer + assert distances[-1] < worst_limit + later = self.distances(track, flown[30:]) + assert later[-1] < later_limit + + def test_the_path_is_flown_at_the_height_flown(self, flight): + # the distances above are across the ground only + (data, (_, _, _, _, p90_limit, worst_limit)) = flight + (items, track) = self.drawn(data) + heights = sorted(abs(h) for h in self.heights(track, data['flown'])) + assert heights[int(len(heights) * 0.9)] < p90_limit + assert heights[-1] < worst_limit + + +class TestDrawnTrack(object): + """where the maps get the path from, and what they do with it""" + + def items(self): + '''a mission in the form the wp module's loader holds it: home, a + speed change with no position, and two waypoints''' + from types import SimpleNamespace + wpoints = [] + rows = [(mavlink.MAV_CMD_NAV_WAYPOINT, offset(0, 0, 0)), + (mavlink.MAV_CMD_DO_CHANGE_SPEED, (0, 0, 0)), + (mavlink.MAV_CMD_NAV_WAYPOINT, offset(3000, 0)), + (mavlink.MAV_CMD_NAV_WAYPOINT, offset(3000, 3000))] + for (seq, (command, (lat, lon, amsl))) in enumerate(rows): + params = (0, 30, -1, 0) if command == mavlink.MAV_CMD_DO_CHANGE_SPEED else (0, 0, 0, 0) + wpoints.append(SimpleNamespace( + seq=seq, command=command, x=lat, y=lon, + z=0 if seq == 0 else amsl - HOME[2], frame=0 if seq == 0 else 3, + param1=params[0], param2=params[1], param3=params[2], + param4=params[3])) + wpoints[0].z = HOME[2] + return wpoints + + def live_module(self, vehicle, params=PARAMS, settle=True): + '''the live map3d module, with only what send_mission() reads''' + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import Map3DModule + module = Map3DModule.__new__(Map3DModule) + wpoints = self.items() + loader = SimpleNamespace(wpoints=wpoints, wp=lambda i: wpoints[i]) + module.mpstate = SimpleNamespace( + mav_param=dict(params), vehicle_type=vehicle, + module=lambda name: (SimpleNamespace(wploader=loader) + if name == 'wp' else None)) + module.sent = [] + module.map = SimpleNamespace( + set_mission=lambda items, track=None: module.sent.append(track)) + module.home_amsl = HOME[2] + module.home_position = None + module.reset_flown_track() + module.map3d_settings = SimpleNamespace(missionpath='flown') + module.armed = False + module.ground_heading = None + module.ground_heading_changed = False + module.ground_heading_redrawn = 0 + module.default_circle_radius = lambda: 80.0 + module.terrain_alt = lambda lat, lon: None + if settle: + # the path is flown on a thread of its own, and drawn from the + # idle task: wait for it and draw it, wherever the module sends + # the mission, so these tests can look at what is drawn + send_mission = module.send_mission + + def send_and_draw(): + send_mission() + self.settle(module) + module.send_mission = send_and_draw + return module + + @staticmethod + def settle(module, timeout=10.0): + '''wait for the module's thread to fly the mission, and draw it''' + import time + deadline = time.time() + timeout + while module.track_thread is not None: + assert time.time() < deadline, 'the mission was never flown' + time.sleep(0.001) + module.draw_flown_track() + + def test_the_live_map_waits_for_the_whole_of_a_mission(self): + """the items of a download still coming in are drawn, but flying + them would end the mission where it has got to, with a return to + launch or a landing the rest of it may say nothing of""" + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + loader.count = lambda: len(loader.wpoints) + loader.expected_count = len(loader.wpoints) + 1 + module.send_mission() + assert module.sent[-1] is None + # and once the last of it arrives, it is flown + loader.expected_count = len(loader.wpoints) + module.send_mission() + assert module.sent[-1] is not None + + def test_the_live_map_flies_a_planes_mission(self): + module = self.live_module('plane') + module.send_mission() + track = module.sent[-1] + assert track is not None + assert track[0][:2] == pytest.approx(HOME[:2]) + # the speed change with no position of its own was flown too: at 30 + # m/s rather than 22 the turn at the first waypoint swings wider + slow = self.live_module('plane', dict(PARAMS, AIRSPEED_CRUISE=22.0)) + slow.mpstate.module('wp').wploader.wpoints[1].param2 = -1 + slow.send_mission() + assert (max(local(p)[0] for p in track) > + max(local(p)[0] for p in slow.sent[-1]) + 5.0) + + def test_the_live_map_does_not_fly_the_mission_again_as_home_wanders( + self, monkeypatch): + # ArduPlane keeps setting home from the GPS every few seconds before + # it arms, so home wanders by a metre or two each time + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + module = self.live_module('plane') + # a takeoff with no position of its own is flown from home, so it + # wanders with it + loader = module.mpstate.module('wp').wploader + loader.wpoints[1].command = mavlink.MAV_CMD_NAV_TAKEOFF + loader.wpoints[1].z = 30.0 + module.home_position = HOME[:2] + module.send_mission() + assert len(calls) == 1 + flown = module.sent[-1] + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, 3) + module.home_position = (lat, lon) + module.home_amsl = HOME[2] + 2 + module.send_mission() + assert len(calls) == 1 + # the path it was flown with is moved to start at the new home, and + # up with it, but still meets the waypoints, which have not moved + moved = module.sent[-1] + assert moved[0][0] == pytest.approx(lat, abs=1e-7) + assert moved[0][1] == pytest.approx(lon, abs=1e-7) + assert moved[0][2] == pytest.approx(flown[0][2] + 2) + far = [i for (i, p) in enumerate(flown) if local(p)[1] > 1000][0] + assert moved[far][:2] == pytest.approx(flown[far][:2], abs=1e-7) + assert moved[far][2] == pytest.approx(flown[far][2] + 2) + # but it moving further is a different flight + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, 50) + module.home_position = (lat, lon) + module.send_mission() + assert len(calls) == 2 + module.home_amsl = HOME[2] + 30 + module.send_mission() + assert len(calls) == 3 + + def test_an_amsl_mission_is_not_flown_again_as_homes_altitude_wanders( + self, monkeypatch): + # an item at an altitude of its own does not move when home does, + # so the mission is the same mission as home's altitude wanders + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + for w in loader.wpoints[1:]: + if w.frame == 3: + w.z += HOME[2] + w.frame = 0 + module.home_position = HOME[:2] + module.send_mission() + assert len(calls) == 1 + for altitude in (0.3, -0.4, 0.2): + module.home_amsl += altitude + module.send_mission() + assert len(calls) == 1 + # and the path it was drawn with stays where it was flown + flown = [track for track in module.sent if track is not None][0] + assert module.sent[-1][0][2] == pytest.approx(flown[0][2]) + # while a mission above home moves up with it + relative = self.live_module('plane') + relative.home_position = HOME[:2] + relative.send_mission() + moved_from = relative.sent[-1][0][2] + relative.home_amsl += 0.3 + relative.send_mission() + assert relative.sent[-1][0][2] == pytest.approx(moved_from + 0.3) + assert len(calls) == 2 + + def counted_flights(self, monkeypatch): + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + return calls + + def test_a_mission_at_both_kinds_of_altitude(self, monkeypatch): + # a path with some altitudes moving with home and some not cannot be + # moved to fit a home which has: it is moved all the same while home + # stays near, and flown again once home has gone further + calls = self.counted_flights(monkeypatch) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + # the last waypoint at an altitude of its own, the rest above home + loader.wpoints[-1].z += HOME[2] + loader.wpoints[-1].frame = 0 + module.home_position = HOME[:2] + module.send_mission() + assert len(calls) == 1 + last = loader.wpoints[-1] + flown = arrival(module.sent[-1], last.x, last.y) + # ArduPlane sends home every few seconds before it arms, a few + # centimetres different each time + for altitude in (0.03, -0.02, 0.04): + module.home_amsl += altitude + module.send_mission() + assert len(calls) == 1 + assert arrival(module.sent[-1], last.x, last.y)[2] == pytest.approx( + flown[2], abs=0.1) + # further, and it is flown again, with the item with an altitude of + # its own still drawn at it, rather than moved up with home + module.home_amsl += 30.0 + module.send_mission() + assert len(calls) == 2 + assert arrival(module.sent[-1], last.x, last.y)[2] == pytest.approx( + flown[2], abs=5.0) + + def test_an_item_with_no_altitude_to_fly_at_does_not_move_with_home( + self, monkeypatch): + # a speed change is at an altitude only because every item carries + # one, and one with no position is flown wherever the aircraft is; + # whatever the frame, neither makes the mission move with home + speed = mavlink.MAV_CMD_DO_CHANGE_SPEED + for (command, frame, z) in ((speed, 3, 0), (speed, 3, 50), + (speed, 10, 0), (speed, 0, 0), + (mavlink.MAV_CMD_NAV_LOITER_UNLIM, 3, 0), + (mavlink.MAV_CMD_NAV_LOITER_UNLIM, 10, 50)): + calls = self.counted_flights(monkeypatch) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + for w in loader.wpoints[1:]: + if w.command == speed: + (w.command, w.frame, w.z) = (command, frame, z) + w.param1 = w.param2 = w.param3 = w.param4 = 0 + else: + (w.frame, w.z) = (0, w.z + HOME[2]) + module.home_position = HOME[:2] + for i in range(4): + module.home_amsl = HOME[2] + 5.0 * (i % 2) + module.send_mission() + assert len(calls) == 1, (command, frame, z) + # nor is the path drawn moved up and down with home + flown = [track for track in module.sent if track is not None][0] + assert module.home_amsl == HOME[2] + 5.0 + assert ([p[2] for p in module.sent[-1]] == + pytest.approx([p[2] for p in flown])), (command, frame, z) + monkeypatch.undo() + + def test_an_item_flown_above_home_moves_with_it(self, monkeypatch): + # a loiter with no position of its own, above home, is flown at an + # altitude which moves with home, among waypoints which do not + calls = self.counted_flights(monkeypatch) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + for w in loader.wpoints[1:]: + if w.command == mavlink.MAV_CMD_DO_CHANGE_SPEED: + (w.command, w.frame, w.z) = ( + mavlink.MAV_CMD_NAV_LOITER_TURNS, 3, 50) + (w.param1, w.param2, w.param3, w.param4) = (1, 0, 0, 0) + else: + (w.frame, w.z) = (0, w.z + HOME[2]) + module.home_position = HOME[:2] + module.send_mission() + module.home_amsl += 30.0 + module.send_mission() + assert len(calls) == 2 + + def test_the_live_map_takes_home_for_an_origin_it_has_not_been_told(self): + from types import SimpleNamespace + module = self.live_module('plane') + frame_valid = 1 << 2 + rally = [SimpleNamespace(lat=int(HOME[0] * 1e7), + lng=int(HOME[1] * 1e7), alt=100, + flags=frame_valid | (2 << 3))] + module.mpstate.module = lambda name: SimpleNamespace( + rally_count=lambda: 1, rally_point=lambda i: rally[0]) + # ArduPilot logs the EKF origin and home together, and MAVProxy + # keeps only the last of each type, so the origin is often unknown: + # home stands in for it, and moves the point as home moves + assert module.origin_amsl is None + (point,) = module.rally_points((HOME[0], HOME[1], HOME[2])) + assert point[2] == HOME[2] + 100 + assert point[3] is False + + def test_the_live_map_takes_a_rally_points_own_altitude_frame(self): + from types import SimpleNamespace + module = self.live_module('plane') + frame_valid = 1 << 2 + rally = [SimpleNamespace(lat=int(HOME[0] * 1e7), + lng=int(HOME[1] * 1e7), alt=100, + flags=frame_valid | (frame << 3)) + for frame in (1, 2, 3)] + module.mpstate.module = lambda name: SimpleNamespace( + wploader=module.mpstate.module('wp').wploader + if name == 'wp' else None, + rally_count=lambda: len(rally), rally_point=lambda i: rally[i]) + module.origin_amsl = 500.0 + module.terrain_alt = lambda lat, lon: 300.0 + points = module.rally_points((HOME[0], HOME[1], HOME[2])) + assert [p[2] for p in points] == [HOME[2] + 100, 600.0, 400.0] + # only the one above home moves when home does + assert [p[3] for p in points] == [False, True, True] + + def test_the_live_map_draws_rally_points_where_they_are_returned_to( + self): + pytest.importorskip("vtk") + import vtk + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + module = self.live_module('plane') + frame_valid = 1 << 2 + # absolute, above home, above the EKF origin, above terrain + rally = [SimpleNamespace(lat=int(HOME[0] * 1e7), + lng=int(HOME[1] * 1e7), alt=100, + flags=frame_valid | (frame << 3)) + for frame in (0, 1, 2, 3)] + module.mpstate.module = lambda name: SimpleNamespace( + rally_count=lambda: len(rally), rally_point=lambda i: rally[i]) + drawn = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + drawn.set_home(HOME[2]) + module.map = SimpleNamespace( + is_alive=lambda: True, set_rally=drawn.set_rally, + set_home=drawn.set_home) + module.send_mission = lambda: None + module.send_fence = lambda: None + module.origin_amsl = 500.0 + module.terrain_alt = lambda lat, lon: 300.0 + + def heights(): + source = drawn.actors['rally'][0].GetMapper().GetInput() + return [source.GetPoint(i)[2] + for i in range(source.GetNumberOfPoints())] + module.send_rally() + assert heights() == pytest.approx([100.0, HOME[2] + 100, 600.0, 400.0]) + + def message(kind, **fields): + m = SimpleNamespace(**fields) + m.get_type = lambda: kind + return m + # each drawn again as what it is above moves + module.mavlink_packet(message('HOME_POSITION', altitude=600 * 1000, + latitude=int(HOME[0] * 1e7), + longitude=int(HOME[1] * 1e7))) + assert heights() == pytest.approx([100.0, 700.0, 600.0, 400.0]) + module.mavlink_packet(message('GPS_GLOBAL_ORIGIN', altitude=450 * 1000)) + assert heights() == pytest.approx([100.0, 700.0, 550.0, 400.0]) + # and where the terrain is not known yet, above home for now + module.terrain_alt = lambda lat, lon: None + module.send_rally() + assert heights() == pytest.approx([100.0, 700.0, 550.0, 700.0]) + + def test_an_item_with_no_position_keeps_its_altitude(self, monkeypatch): + flown = [] + real = plane_track.mission_track + + def recording(home, items, *args, **kwargs): + flown.append(items) + return real(home, items, *args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + # a loiter to altitude where the aircraft is, 200m above home + loader.wpoints[1].command = mavlink.MAV_CMD_NAV_LOITER_TO_ALT + loader.wpoints[1].z = 200.0 + loader.wpoints[1].param2 = 0 + module.send_mission() + assert flown[-1][0][3] == pytest.approx(HOME[2] + 200.0) + + def test_the_mission_is_flown_off_the_main_thread(self, monkeypatch): + import threading + release = threading.Event() + flown_on = [] + real = plane_track.mission_track + + def slow(*args, **kwargs): + flown_on.append(threading.current_thread()) + assert release.wait(10) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', slow) + module = self.live_module('plane', settle=False) + module.send_mission() + # the mission is drawn at once, while its path is still being flown + assert module.sent == [None] + module.draw_flown_track() + assert module.sent == [None] + # sent again as it is flown, it is not flown twice over + import time + deadline = time.time() + 10 + while not flown_on: + assert time.time() < deadline + time.sleep(0.001) + module.send_mission() + release.set() + self.settle(module) + assert module.sent[-1] is not None + assert flown_on[0] is not threading.main_thread() + assert len(flown_on) == 1 + # and drawn with it straight away the next time, without flying it + module.send_mission() + assert module.sent[-1] is not None + assert len(flown_on) == 1 + + def flying_slowly(self, monkeypatch): + '''plane_track.mission_track made to wait for a release, and a list + of the calls made to it''' + import threading + release = threading.Event() + calls = [] + real = plane_track.mission_track + + def slow(*args, **kwargs): + calls.append(args) + assert release.wait(10) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', slow) + return (release, calls) + + def wait_for(self, condition): + import time + deadline = time.time() + 10 + while not condition(): + assert time.time() < deadline + time.sleep(0.001) + + def test_home_wandering_as_the_mission_is_flown(self, monkeypatch): + (release, calls) = self.flying_slowly(monkeypatch) + module = self.live_module('plane', settle=False) + module.home_position = HOME[:2] + module.send_mission() + self.wait_for(lambda: calls) + # home wanders a few metres while the path is being flown: the path + # is not flown again, and is drawn from where home is now + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, 3) + module.home_position = (lat, lon) + module.send_mission() + release.set() + self.settle(module) + assert len(calls) == 1 + assert module.sent[-1][0][:2] == pytest.approx((lat, lon), abs=1e-7) + + def test_home_wandering_far_as_the_mission_is_flown(self, monkeypatch): + (release, calls) = self.flying_slowly(monkeypatch) + module = self.live_module('plane', settle=False) + module.home_position = HOME[:2] + module.send_mission() + self.wait_for(lambda: calls) + # a few metres at a time, but in all more than ten from the home it + # is being flown from: that is flown again + for metres in (4, 8, 13): + (lat, lon) = mp_util.gps_newpos(HOME[0], HOME[1], 45, metres) + module.home_position = (lat, lon) + module.send_mission() + release.set() + self.settle(module) + assert len(calls) == 2 + assert module.sent[-1][0][:2] == pytest.approx((lat, lon), abs=1e-7) + + def test_the_idle_task_draws_the_path_flown(self): + import time + module = self.live_module('plane', settle=False) + module.map.is_alive = lambda: True + module.map.check_events = lambda: [] + wp_module = module.mpstate.module('wp') + wp_module.wploader.last_change = 1.0 + module.mpstate.module = lambda name: ( + wp_module if name == 'wp' else None) + module.wp_change_time = 1.0 + module.kml_change_state = module._kml_state(None) + module.fence_change_time = module.rally_change_time = 0 + module.terrain_resolved = False + module.send_mission() + assert module.sent == [None] + deadline = time.time() + 10 + while module.track_thread is not None: + assert time.time() < deadline + time.sleep(0.001) + module.idle_task() + assert module.sent[-1] is not None + + def wp_module(self): + """the wp module, with only what loading a mission into it reads""" + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_wp + from pymavlink import mavwp + module = mavproxy_wp.WPModule.__new__(mavproxy_wp.WPModule) + module.wploader_by_sysid = {1: mavwp.MAVWPLoader()} + module.mpstate = SimpleNamespace( + settings=SimpleNamespace(target_system=1, target_component=1), + status=SimpleNamespace(logdir=None)) + return module + + def test_the_wp_module_expects_the_mission_it_is_given(self): + """whatever fills the loader says how many items to expect, so what + it holds is not taken for part of a download which stopped: that + would have the 3D map wait for the rest of it for ever, and the wp + module ask the vehicle for items it has""" + import struct + module = self.wp_module() + loader = module.wploader + # a download of four items, which three of arrive + loader.expected_count = 4 + for seq in range(3): + (lat, lon, _) = offset(seq * 100, 0) + loader.add(mavlink.MAVLink_mission_item_message( + 1, 1, seq, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + lat, lon, 100)) + assert loader.count() < loader.expected_count + # and then the whole of one fetched over ftp, which is shorter + items = b'' + for seq in range(2): + (lat, lon, _) = offset(seq * 100, 0) + item = mavlink.MAVLink_mission_item_int_message( + 1, 1, seq, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + int(lat * 1e7), int(lon * 1e7), 100) + item.mission_type = 0 + ordered = [None] * len(item.fieldnames) + for (i, order) in enumerate(item.orders): + ordered[order] = getattr(item, item.fieldnames[i]) + items += item.unpacker.pack(*ordered) + # the items are taken as they come: turning one into a waypoint of + # its own reads a field only a MAVLink2 dialect carries, which these + # tests do not load + module.wp_from_mission_item_int = lambda item: item + from io import BytesIO + module.ftp_callback(BytesIO(struct.pack( + " drawn + assert module.sent[-1][0][2] == pytest.approx(HOME[2] + 100, abs=1.0) + # and no further drawing for a home which has not moved at all + drawn = len(module.sent) + home_position(HOME[2] + 100) + assert len(module.sent) == drawn + + def test_an_items_frame_is_part_of_which_mission_it_is(self): + """100m above home and 100m AMSL are not the same item, so a mission + changed from one to the other is flown again rather than drawn with + the path worked out for the other""" + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + last = loader.wpoints[-1] + (last.frame, last.z) = (3, 100.0) + module.send_mission() + above_home = nearest(module.sent[-1], last.x, last.y)[2] + assert above_home == pytest.approx(HOME[2] + 100, abs=30.0) + (last.frame, last.z) = (0, 100.0) + module.send_mission() + assert module.sent[-1] is not None + # 100m AMSL is a long way below a home at 584m, and the path drawn + # for it dives rather than holding the height of the other + assert nearest(module.sent[-1], last.x, last.y)[2] < above_home - 300 + + def test_a_waypoint_above_terrain_nobody_has_yet_stands_above_home(self): + """there is a path to draw before the terrain arrives: home's height + stands in for the ground's, and the mission is drawn again once the + terrain is known""" + module = self.live_module('plane') + module.terrain_alt = lambda lat, lon: None + loader = module.mpstate.module('wp').wploader + last = loader.wpoints[-1] + (last.frame, last.z) = (10, 100.0) + module.send_mission() + assert module.sent[-1] is not None + assert nearest(module.sent[-1], last.x, last.y)[2] == \ + pytest.approx(HOME[2] + 100, abs=30.0) + # and once it is known, at the height above that + module.terrain_alt = lambda lat, lon: HOME[2] + 300 + module.send_mission() + assert nearest(module.sent[-1], last.x, last.y)[2] == \ + pytest.approx(HOME[2] + 400, abs=30.0) + + def test_the_live_map_redraws_for_a_changed_parameter(self): + """how the vehicle flies a mission is its parameters' as much as its + items': a parameter the path is flown by changing, or arriving for + the first time, draws the mission again""" + module = self.live_module('plane') + module.map.is_alive = lambda: True + module.map.check_events = lambda: [] + wp_module = module.mpstate.module('wp') + wp_module.wploader.last_change = 1.0 + module.mpstate.module = lambda name: ( + wp_module if name == 'wp' else None) + module.wp_change_time = 1.0 + module.kml_change_state = module._kml_state(None) + module.fence_change_time = module.rally_change_time = 0 + module.terrain_resolved = False + module.send_mission() + # the parameters arriving for the first time is a change of its own: + # the mission is drawn again with them + module.idle_task() + drawn = len(module.sent) + was = max(p[2] for p in module.sent[-1]) + # with nothing changed since, it is not + module.idle_task() + assert len(module.sent) == drawn + # RTL_ALTITUDE, which a return climbs to, changing is: the return + # comes home higher than it did + module.mpstate.mav_param['RTL_ALTITUDE'] = 300.0 + module.idle_task() + assert len(module.sent) > drawn + assert max(p[2] for p in module.sent[-1]) > was + 100.0 + + def test_a_path_for_a_mission_since_changed_is_not_drawn(self, monkeypatch): + import threading + import time + (first, second) = (threading.Event(), threading.Event()) + releases = [first, second] + calls = [] + real = plane_track.mission_track + + def slow(*args, **kwargs): + calls.append(args) + assert releases.pop(0).wait(10) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', slow) + module = self.live_module('plane', settle=False) + loader = module.mpstate.module('wp').wploader + module.send_mission() + # the mission changes while the first is being flown, rather than + # before the thread has taken it, which it would simply replace + self.wait_for(lambda: calls) + loader.wpoints[3].y += 0.01 + module.send_mission() + assert module.sent == [None, None] + first.set() + deadline = time.time() + 10 + while module.track_result is None: + assert time.time() < deadline + time.sleep(0.001) + # the first mission's path is not drawn over the second mission + module.draw_flown_track() + assert module.sent == [None, None] + second.set() + self.settle(module) + (lat, lon, _) = nearest(module.sent[-1], loader.wpoints[3].x, + loader.wpoints[3].y) + assert mp_util.gps_distance(lat, lon, loader.wpoints[3].x, + loader.wpoints[3].y) < 150.0 + + def test_the_live_map_returns_to_its_rally_points(self, monkeypatch): + from types import SimpleNamespace + module = self.live_module('plane') + loader = module.mpstate.module('wp').wploader + loader.wpoints[3].command = mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH + loader.wpoints[3].x = loader.wpoints[3].y = 0.0 + (lat, lon, _) = offset(2500, 800, 0) + rally = SimpleNamespace(rally_points=[]) + rally.rally_count = lambda: len(rally.rally_points) + rally.rally_point = lambda i: rally.rally_points[i] + modules = SimpleNamespace(wploader=loader, + rally_count=rally.rally_count, + rally_point=rally.rally_point, + last_change=lambda: 0) + module.mpstate.module = lambda name: ( + modules if name in ('wp', 'rally') else None) + module.send_mission() + (end_lat, end_lon, end_amsl) = module.sent[-1][-1] + assert mp_util.gps_distance(end_lat, end_lon, HOME[0], HOME[1]) < 150 + # a rally point 150m above home, nearer than home is + rally.rally_points.append(SimpleNamespace( + lat=int(lat * 1e7), lng=int(lon * 1e7), alt=150, flags=0)) + module.send_mission() + (end_lat, end_lon, end_amsl) = module.sent[-1][-1] + assert mp_util.gps_distance(end_lat, end_lon, lat, lon) < 150 + assert end_amsl == pytest.approx(HOME[2] + 150, abs=15) + + def test_a_mission_which_will_not_fly_does_not_stop_the_next(self, monkeypatch): + real = plane_track.mission_track + failures = [RuntimeError('bad mission')] + + def failing(*args, **kwargs): + if failures: + raise failures.pop() + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', failing) + module = self.live_module('plane') + module.send_mission() + assert module.sent[-1] is None + loader = module.mpstate.module('wp').wploader + loader.wpoints[3].y += 0.01 + module.send_mission() + assert module.sent[-1] is not None + + def test_the_path_across_the_antimeridian_is_drawn_with_its_mission(self): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + home = (10.0, 179.998, 0.0) + east = mp_util.gps_newpos(home[0], home[1], 90, 300) + track = plane_track.mission_track( + home, [(mavlink.MAV_CMD_NAV_WAYPOINT, east[0], east[1], 100.0, + (0, 0, 0, 0))], PARAMS) + items = [MissionItem(home[0], home[1], 0.0, 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 0), + MissionItem(east[0], east[1], 100.0, 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 1)] + for origin in (home, (east[0], east[1])): + em = ElementManager(vtk.vtkRenderer(), origin[0], origin[1], 1.0) + em.set_mission(items, track) + marker = em.mission_markers[-1] + assert min(math.hypot(p[0] - marker[0], p[1] - marker[1]) + for p in em.mission_line) < 100.0 + + def test_the_live_map_leaves_other_vehicles_to_the_items(self): + module = self.live_module('copter') + module.send_mission() + assert module.sent[-1] is None + + def test_an_unchanged_mission_is_not_flown_again(self, monkeypatch): + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + module = self.live_module('plane') + module.send_mission() + module.send_mission() + assert len(calls) == 1 + module.mpstate.mav_param['WP_RADIUS'] = 30.0 + module.send_mission() + assert len(calls) == 2 + + def test_the_live_map_takes_off_the_way_the_vehicle_points(self, monkeypatch): + headings = [] + real = plane_track.mission_track + + def recording(home, items, params, heading=None, **kwargs): + headings.append(heading) + return real(home, items, params, heading, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module('plane') + module.map.is_alive = lambda: True + module.map.set_vehicle_type = lambda name: None + module.icon_type = None + mav = mavutil.mavlink + + def heartbeat(armed): + return mav.MAVLink_heartbeat_message( + mav.MAV_TYPE_FIXED_WING, mav.MAV_AUTOPILOT_ARDUPILOTMEGA, + mav.MAV_MODE_FLAG_SAFETY_ARMED if armed else 0, 0, 0, 3) + + def attitude(yaw): + return mav.MAVLink_attitude_message(0, 0, 0, yaw, 0, 0, 0) + + def points(yaw, now): + module.mavlink_packet(attitude(math.radians(yaw))) + module.redraw_for_ground_heading(now) + # on the ground, pointing east: the best guess there is at the course + # it will take off on, and the mission is flown again from it without + # waiting for anything else to change + module.mavlink_packet(heartbeat(False)) + points(90, 100.0) + assert headings == [90] + # a degree or two of wander is not worth flying it again for + points(92, 200.0) + assert headings == [90] + # turning round is, though not more often than every couple of seconds + points(180, 200.5) + assert headings == [90, 180] + points(270, 201.0) + assert headings == [90, 180] + module.redraw_for_ground_heading(203.0) + assert headings == [90, 180, 270] + # once it is armed and flying, where it points is not the takeoff's, + # and a ground station's heartbeat says nothing about the vehicle + module.mavlink_packet(heartbeat(True)) + module.mavlink_packet(mav.MAVLink_heartbeat_message( + mav.MAV_TYPE_GCS, mav.MAV_AUTOPILOT_INVALID, 0, 0, 0, 3)) + points(0, 300.0) + module.send_mission() + assert headings == [90, 180, 270] + + def test_the_live_map_redraws_for_a_new_heading_on_its_own(self, monkeypatch): + # nothing else has to happen for the module's idle task to do it + headings = [] + real = plane_track.mission_track + + def recording(home, items, params, heading=None, **kwargs): + headings.append(heading) + return real(home, items, params, heading, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module('plane') + module.send_mission() + assert headings == [None] + module.map.is_alive = lambda: True + module.map.check_events = lambda: [] + wp_module = module.mpstate.module('wp') + wp_module.wploader.last_change = 1.0 + module.mpstate.module = lambda name: ( + wp_module if name == 'wp' else None) + module.wp_change_time = 1.0 + module.kml_change_state = module._kml_state(None) + module.fence_change_time = module.rally_change_time = 0 + module.terrain_resolved = False + module.note_ground_heading(math.radians(45)) + module.idle_task() + assert headings == [None, 45] + + def explorer(self): + pytest.importorskip("wx") + pytest.importorskip("lxml") + import importlib.util + path = os.path.join(os.path.dirname(os.path.dirname(__file__)), + 'MAVProxy', 'tools', 'MAVExplorer.py') + spec = importlib.util.spec_from_file_location('mavexplorer', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + def log_mission(self, with_home=True): + '''a log's CMD entries, as mission_from_log() keeps them''' + cmds = {} + for w in self.items(): + if w.seq == 0 and not with_home: + cmds[0] = (0.0, 0.0, 0.0, 0, w.command, 0, (0, 0, 0, 0)) + continue + cmds[w.seq] = (w.x, w.y, w.z, w.frame, w.command, w.seq, + (w.param1, w.param2, w.param3, w.param4)) + return cmds + + def test_mavexplorer_flies_a_planes_mission(self): + mx = self.explorer() + cmds = self.log_mission() + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert track is not None + assert track[0][:2] == pytest.approx(HOME[:2]) + # the speed change is in the CMDs but not among the items drawn + slow = dict(cmds) + slow[1] = slow[1][:6] + ((0, -1, -1, 0),) + slow = mx.plane_mission_track(slow, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert (max(local(p)[0] for p in track) > + max(local(p)[0] for p in slow) + 5.0) + assert mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_QUADROTOR) is None + + def test_mavexplorer_draws_no_path_for_a_mission_with_an_item_missing(self): + # jumps, and where each item began, go by sequence number, which a + # gap would put out by one for every item after it + mx = self.explorer() + cmds = self.log_mission() + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) is not None + del cmds[1] + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) is None + + def test_mavexplorer_keeps_the_altitude_of_an_item_with_no_position( + self, monkeypatch): + flown = [] + real = plane_track.mission_track + + def recording(home, items, *args, **kwargs): + flown.append(items) + return real(home, items, *args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + mx = self.explorer() + cmds = self.log_mission() + cmds[1] = (0.0, 0.0, 200.0, 3, mavlink.MAV_CMD_NAV_LOITER_TO_ALT, 1, + (0, 0, 0, 0)) + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert flown[-1][0][3] == pytest.approx(HOME[2] + 200.0) + + def test_mavexplorer_starts_where_the_flight_did_with_no_home(self): + mx = self.explorer() + cmds = self.log_mission(with_home=False) + started = offset(-500, -500, 0) + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds, started_at=started[:2]), + HOME[2], PARAMS, mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, started, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert track is not None + assert track[0][:2] == pytest.approx(started[:2]) + + def log(self, *messages): + '''a dataflash log handing back these messages in order''' + queue = list(messages) + + class Log(object): + def recv_match(self, type=None, condition=None): + while queue: + m = queue.pop(0) + if type is None or m.get_type() in type: + return m + return None + return Log() + + def message(self, kind, **fields): + from types import SimpleNamespace + m = SimpleNamespace(_timestamp=0, **fields) + m.get_type = lambda: kind + return m + + def cmd(self, seq, command, lat, lon, alt): + return self.message('CMD', CNum=seq, CId=command, Lat=lat, Lng=lon, + Alt=alt, Frame=3, Prm1=0, Prm2=0, Prm3=0, Prm4=0) + + def pos(self, north, east, alt=0.0): + (lat, lon, amsl) = offset(north, east, alt) + return self.message('POS', Lat=lat, Lng=lon, Alt=amsl) + + def mission_dump(self, new_mission=True, + takeoff=mavlink.MAV_CMD_NAV_TAKEOFF): + (lat, lon, _) = offset(3000, 0) + return (([self.message('MSG', Message='New mission')] if new_mission + else []) + + [self.cmd(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2]), + self.cmd(1, takeoff, 0, 0, 50), + self.cmd(2, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100)]) + + def test_mavexplorer_takes_the_takeoff_course_from_the_log(self): + mx = self.explorer() + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + # taxied north a little before the mission started + self.pos(10, 0), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(10, 10), self.pos(10, 25), self.pos(10, 45), + self.pos(10, 80)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + # east, from where the takeoff began, not from where the log did + assert mx.takeoff_course(path, cmds, started) == pytest.approx(90.0, abs=1.0) + assert mx.takeoff_course(path, cmds, {}) is None + + def mavlink(self, message, sysid=1, mission_type=None): + '''a MAVLink message from sysid, as a telemetry log holds it. The + mission protocol's mission_type is a MAVLink 2 extension, which is + set after packing where the dialect loaded is MAVLink 1''' + message.pack(mavlink.MAVLink(None, srcSystem=sysid, + srcComponent=1 if sysid < 200 else 190)) + message._timestamp = 0 + if mission_type is not None: + message.mission_type = mission_type + return message + + def mission_count(self, target, count, mission_type, sysid): + return self.mavlink(mavlink.MAVLink_mission_count_message( + target, 1, count), sysid, mission_type) + + def telemetry(self, vehicle=1): + '''a telemetry log of a vehicle, and a ground station which + uploads it a mission and watches it take off''' + gcs = 255 + out = [self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_GCS, mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0, 3), + gcs)] + out.append(self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, + 0, 0, 0, 3), vehicle)) + + def position(north, east, alt=0.0, sysid=vehicle): + (lat, lon, amsl) = offset(north, east, alt) + return self.mavlink(mavlink.MAVLink_global_position_int_message( + 0, int(lat * 1e7), int(lon * 1e7), int(amsl * 1000), + int(alt * 1000), 0, 0, 0, 0), sysid) + + def item(seq, command, lat, lon, alt, frame, params=(0, 0, 0, 0), + mission_type=0, sysid=gcs, target=vehicle): + return self.mavlink(mavlink.MAVLink_mission_item_int_message( + target, 1, seq, frame, command, 0, 1, *params, + int(lat * 1e7), int(lon * 1e7), alt), sysid, mission_type) + (lat, lon, _) = offset(3000, 0) + (rlat, rlon, _) = offset(-2000, 500) + out += [ + position(0, 0), + # another vehicle, whose position is nothing to do with this one + position(5000, 5000, sysid=vehicle + 1), + self.mavlink(mavlink.MAVLink_home_position_message( + int(HOME[0] * 1e7), int(HOME[1] * 1e7), int(HOME[2] * 1000), + 0, 0, 0, [1, 0, 0, 0], 0, 0, 0), vehicle), + self.mavlink(mavlink.MAVLink_gps_global_origin_message( + int(HOME[0] * 1e7), int(HOME[1] * 1e7), int(500 * 1000)), + vehicle), + # an old mission, then the one uploaded over it + self.mission_count(vehicle, 2, 0, gcs), + item(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2], 0), + item(1, mavlink.MAV_CMD_NAV_WAYPOINT, rlat, rlon, 80, 3), + self.mission_count(vehicle, 3, 0, gcs), + item(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2], 0), + item(1, mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 50, 3, (10, 0, 0, 0)), + item(2, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100, 3), + # and a rally point downloaded from the vehicle + self.mission_count(gcs, 1, 2, vehicle), + item(0, mavlink.MAV_CMD_NAV_RALLY_POINT, rlat, rlon, 120, 3, + mission_type=2, sysid=vehicle, target=gcs), + self.mavlink(mavlink.MAVLink_statustext_message( + 6, b'Mission: 1 Takeoff'), vehicle), + position(10, 10, 5), position(20, 20, 10), + ] + return out + + def test_mavexplorer_reads_a_telemetry_log(self): + mx = self.explorer() + (path, mission, cmds, started, rally, origin, + approach) = mx.mission_from_log(self.log(*self.telemetry())) + # the vehicle's own positions, not another's + assert len(path) == 3 + assert path[0][:3] == pytest.approx(offset(0, 0, 0)) + # the mission last uploaded, which replaced the one before + assert sorted(cmds) == [0, 1, 2] + assert cmds[1][4] == mavlink.MAV_CMD_NAV_TAKEOFF + assert cmds[1][6][0] == 10 + assert cmds[2][:2] == pytest.approx(offset(3000, 0)[:2]) + # the takeoff placed where it began + assert started == {1: 0} + assert (mission[1][0], mission[1][1]) == pytest.approx(offset(0, 0)[:2]) + assert origin == pytest.approx((HOME[0], HOME[1], 500.0)) + (lat, lon, _) = offset(-2000, 500) + assert len(rally) == 1 + assert rally[0][:3] == pytest.approx((lat, lon, 120.0)) + assert plane_track.rally_alt_frame(rally[0][3]) == \ + plane_track.RALLY_ALT_ABOVE_HOME + assert approach is None + # which is enough to fly: up at the takeoff's pitch, towards the + # waypoint after it + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track( + cmds, mission, path[0][:3], PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin, approach) + climbed = [local(p) for p in track if p[2] >= HOME[2] + 49.5][0] + assert climbed[0] == pytest.approx(50 / math.tan(math.radians(10)), + rel=0.1) + assert abs(climbed[1]) < 5.0 + + def test_mavexplorer_keeps_the_frame_a_rally_point_came_in(self): + """a rally point downloaded or uploaded carries the frame its + altitude is in, which AP_Rally keeps as flags of its own""" + mx = self.explorer() + frames = {mavlink.MAV_FRAME_GLOBAL: plane_track.RALLY_ALT_ABSOLUTE, + mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT: + plane_track.RALLY_ALT_ABOVE_HOME, + mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT: + plane_track.RALLY_ALT_ABOVE_TERRAIN} + (lat, lon, _) = offset(-2000, 500) + for (frame, expected) in frames.items(): + log = self.telemetry_mission( + self.mission_count(255, 1, 2, 1), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + 255, 1, 0, frame, mavlink.MAV_CMD_NAV_RALLY_POINT, 0, 1, + 0, 0, 0, 0, int(lat * 1e7), int(lon * 1e7), 120), 1, 2)) + (_, _, _, _, rally, _, _) = mx.mission_from_log(log) + assert len(rally) == 1 + assert plane_track.rally_alt_frame(rally[0][3]) == expected + + def rally_messages(self, count=2, sysid=1, target=255): + """a rally table as it travels: the points 2km south of home""" + out = [self.mission_count(target, count, 2, sysid)] + for seq in range(count): + (lat, lon, _) = offset(-2000, 500 * seq) + out.append(self.mavlink( + mavlink.MAVLink_mission_item_int_message( + target, 1, seq, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavlink.MAV_CMD_NAV_RALLY_POINT, 0, 1, 0, 0, 0, 0, + int(lat * 1e7), int(lon * 1e7), 120), sysid, 2)) + return out + + def test_mavexplorer_keeps_a_rally_table_a_download_stops_through(self): + """as with a mission, a rally download the log ends part way through + says nothing about the table the vehicle holds""" + mx = self.explorer() + table = self.rally_messages() + log = self.telemetry_mission(*(table + self.rally_messages()[:2])) + (_, _, _, _, rally, _, _) = mx.mission_from_log(log) + assert len(rally) == 2 + + def test_mavexplorer_drops_old_rally_points_a_download_says_are_gone( + self): + """a point the old RALLY_POINT protocol set, and then a download of + the table in the mission protocol with nothing in it: the vehicle + holds no rally points, though the mission protocol's table was + empty already""" + mx = self.explorer() + (lat, lon, _) = offset(-2000, 500) + log = self.telemetry_mission( + self.mavlink(mavlink.MAVLink_rally_point_message( + 255, 1, 0, 1, int(lat * 1e7), int(lon * 1e7), 120, 0, 0, 0), + 1), + self.mission_count(255, 0, mavlink.MAV_MISSION_TYPE_RALLY, 1)) + (_, _, _, _, rally, _, _) = mx.mission_from_log(log) + assert rally == [] + + def test_mavexplorer_does_not_fly_a_rally_table_cut_short(self): + """a download of the rally table which the log ends part way + through: what arrived is what is known of the table the vehicle + holds, and the points the old protocol set before it are gone, but + a return may go to one of those which never arrived, so there is no + flying the mission either""" + mx = self.explorer() + + def legacy(seq, count, north, east): + (lat, lon, _) = offset(north, east) + return self.mavlink(mavlink.MAVLink_rally_point_message( + 255, 1, seq, count, int(lat * 1e7), int(lon * 1e7), 120, + 0, 0, 0), 1) + table = self.rally_messages(count=2) + log = self.telemetry_mission(legacy(0, 2, -2000, 0), + legacy(1, 2, -2000, 500), *table[:2]) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + # the one point of the new table which arrived, not the old two + assert len(rally) == 1 + assert rally[0][:2] == pytest.approx(offset(-2000, 0)[:2]) + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is None + # as there is none for a table of the old protocol's which the log + # holds fewer points of than it says the table has + log = self.telemetry_mission(legacy(0, 2, -2000, 0)) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + assert len(rally) == 1 + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is None + # where the whole of the table is flown + log = self.telemetry_mission(legacy(0, 2, -2000, 0), *table) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + assert len(rally) == 2 + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is not None + + def test_mavexplorer_flies_a_mission_once_a_rally_table_is_cleared(self): + """a table the log holds only part of leaves the mission unflown, + but a table cleared away afterwards is the whole of what the vehicle + holds: none of it, and the mission is flown home again""" + mx = self.explorer() + + def legacy(seq, count, north, east): + (lat, lon, _) = offset(north, east) + return self.mavlink(mavlink.MAVLink_rally_point_message( + 255, 1, seq, count, int(lat * 1e7), int(lon * 1e7), 120, + 0, 0, 0), 1) + + def flown(log): + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + return mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) + # one point of a table of two: not a table to work a return out from + assert flown(self.telemetry_mission(legacy(0, 2, -2000, 0))) is None + # and then the logger writing out a table with nothing in it + assert flown(self.telemetry_mission( + legacy(0, 2, -2000, 0), + self.message('MSG', Message='New rally'))) is not None + + def test_mavexplorer_does_not_fly_a_rally_download_which_brought_none( + self): + """a download which says how many points to expect and ends before + any of them arrive leaves nothing known of the table: the mission is + not flown home as though the vehicle held no points""" + mx = self.explorer() + log = self.telemetry_mission( + self.mission_count(255, 2, mavlink.MAV_MISSION_TYPE_RALLY, 1)) + (path, mission, cmds, started, rally, origin, _) = \ + mx.mission_from_log(log) + assert rally == [] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started, rally, origin) is None + + def test_mavexplorer_drops_rally_points_cleared_away(self): + """MISSION_CLEAR_ALL for the rally table, and for everything, which + takes the mission with it""" + mx = self.explorer() + for mission_type in (mavlink.MAV_MISSION_TYPE_RALLY, + mavlink.MAV_MISSION_TYPE_ALL): + log = self.telemetry_mission(*(self.rally_messages() + [ + self.mavlink(mavlink.MAVLink_mission_clear_all_message(1, 1), + 255, mission_type)])) + (_, mission, cmds, _, rally, _, _) = mx.mission_from_log(log) + assert rally == [] + everything = mission_type == mavlink.MAV_MISSION_TYPE_ALL + assert (cmds == {}) is everything + + def test_mavexplorer_takes_home_from_a_telemetry_log_as_home(self): + """HOME_POSITION is where the vehicle says home is, not the EKF + origin, which GPS_GLOBAL_ORIGIN says""" + mx = self.explorer() + vehicle = 1 + # a mission uploaded before the vehicle had a home, whose first item + # is the empty one it carries, which home is put into + log = self.telemetry_mission(self.mavlink( + mavlink.MAVLink_home_position_message( + int(HOME[0] * 1e7), int(HOME[1] * 1e7), int(400 * 1000), + 0, 0, 0, [1, 0, 0, 0], 0, 0, 0), vehicle), + self.mission_count(vehicle, 2, 0, 255), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 0, 0, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, + 0, 0, 0, 0, 0, 0, 0), 255, 0), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 1, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, + 0, 0, int(HOME[0] * 1e7), int(HOME[1] * 1e7), 100), 255, 0)) + (_, _, cmds, _, _, origin, _) = mx.mission_from_log(log) + assert cmds[0][:3] == pytest.approx((HOME[0], HOME[1], 400.0)) + # and nothing said where the origin was + assert origin is None + + def mission_messages(self, sysid=255, target=1): + """a three item mission as it travels: a MISSION_COUNT and its + items, uploaded to the vehicle, or downloaded from it the other way + about""" + (lat, lon, _) = offset(3000, 0) + items = [(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2], 0), + (1, mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 50, 3), + (2, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100, 3)] + out = [self.mission_count(target, len(items), 0, sysid)] + for (seq, command, ilat, ilon, alt, frame) in items: + out.append(self.mavlink( + mavlink.MAVLink_mission_item_int_message( + target, 1, seq, frame, command, 0, 1, 0, 0, 0, 0, + int(ilat * 1e7), int(ilon * 1e7), alt), sysid, 0)) + return out + + def telemetry_mission(self, *messages, upload=None): + """a telemetry log of a mission uploaded -- mission_messages()'s, + or upload -- taken off on 600m east of home, and then whatever + messages follow""" + if upload is None: + upload = self.mission_messages() + vehicle = 1 + + def position(north, east, alt=0.0): + (plat, plon, amsl) = offset(north, east, alt) + return self.mavlink(mavlink.MAVLink_global_position_int_message( + 0, int(plat * 1e7), int(plon * 1e7), int(amsl * 1000), + int(alt * 1000), 0, 0, 0, 0), vehicle) + return self.log(*( + [self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, + mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 3), vehicle), + position(0, 0)] + list(upload) + + [position(0, 600), + self.mavlink(mavlink.MAVLink_statustext_message( + 6, b'Mission: 1 Takeoff'), vehicle)] + list(messages))) + + def test_mavexplorer_keeps_a_mission_a_download_stops_part_way_through( + self): + """a telemetry log carries the missions downloaded from the vehicle + as well as those uploaded to it, and one the log ends part way + through says nothing about the mission the vehicle holds""" + mx = self.explorer() + download = self.mission_messages(sysid=1, target=255) + log = self.telemetry_mission(*download[:3]) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + # the whole mission, not the two items of the download over it + assert sorted(cmds) == [0, 1, 2] + assert cmds[2][:2] == pytest.approx(offset(3000, 0)[:2]) + # and where the flight had got to in it + assert started == {1: 1} + + def test_mavexplorer_draws_a_mission_cut_short_but_does_not_fly_it(self): + """the only transfer a log carries of its mission ends part way + through it: those items are where the vehicle was told to go, and + are drawn, but flying them would end the flight with a return to + launch which whatever was still to come may say nothing of""" + mx = self.explorer() + vehicle = 1 + (lat, lon, _) = offset(3000, 0) + messages = ( + [self.mavlink(mavlink.MAVLink_heartbeat_message( + mavlink.MAV_TYPE_FIXED_WING, + mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 3), vehicle), + self.mavlink(mavlink.MAVLink_global_position_int_message( + 0, int(HOME[0] * 1e7), int(HOME[1] * 1e7), + int(HOME[2] * 1000), 0, 0, 0, 0, 0), vehicle), + self.mission_count(vehicle, 3, 0, 255), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 0, 0, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, + 0, 0, 0, 0, int(HOME[0] * 1e7), int(HOME[1] * 1e7), + HOME[2]), 255, 0), + self.mavlink(mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 1, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, + 0, 0, 0, 0, int(lat * 1e7), int(lon * 1e7), 100), 255, 0)]) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log( + self.log(*messages)) + # the two items which arrived, drawn where they were sent + assert sorted(cmds) == [0, 1] + assert len(mission) == 2 + assert (mission[1][0], mission[1][1]) == pytest.approx((lat, lon)) + # and no path flown for them + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started) is None + # where a download cut short after a whole mission arrived leaves + # that mission, which is flown + after = self.telemetry_mission(*self.downloaded_mission()[:2]) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(after) + assert sorted(cmds) == [0, 1, 2] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started) is not None + # where the whole of it is flown once the rest arrives + whole = self.log(*(messages + [self.mavlink( + mavlink.MAVLink_mission_item_int_message( + vehicle, 1, 2, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, + 0, 0, int(lat * 1e7), int(lon * 1e7), 100), 255, 0)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(whole) + assert sorted(cmds) == [0, 1, 2] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert mx.plane_mission_track( + cmds, mission, None, PARAMS, mavlink.MAV_TYPE_FIXED_WING, + path, started) is not None + + def test_mavexplorer_keeps_where_a_mission_downloaded_again_was_flown( + self): + """a ground station downloads a mission mid-flight to see what the + vehicle holds: the same mission, so the flight goes on""" + mx = self.explorer() + log = self.telemetry_mission(*self.mission_messages(sysid=1, + target=255)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert sorted(cmds) == [0, 1, 2] + assert started == {1: 1} + # so the takeoff is still drawn from where it was flown + assert (mission[1][0], mission[1][1]) == \ + pytest.approx(offset(0, 600)[:2]) + + def downloaded_mission(self, alt=None, lat=None, north=3000): + """the mission of telemetry_mission() as the vehicle sends it back, + with home or the waypoint moved""" + (wlat, wlon, _) = offset(north, 0) + items = [(0, mavlink.MAV_CMD_NAV_WAYPOINT, + HOME[0] if lat is None else lat, HOME[1], + HOME[2] if alt is None else alt, 0), + (1, mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 50, 3), + (2, mavlink.MAV_CMD_NAV_WAYPOINT, wlat, wlon, 100, 3)] + out = [self.mission_count(255, len(items), 0, 1)] + for (seq, command, ilat, ilon, ialt, frame) in items: + out.append(self.mavlink( + mavlink.MAVLink_mission_item_int_message( + 255, 1, seq, frame, command, 0, 1, 0, 0, 0, 0, + int(ilat * 1e7), int(ilon * 1e7), ialt), 1, 0)) + return out + + def test_mavexplorer_knows_a_mission_downloaded_again_by_its_items(self): + """home is the vehicle's own, not the mission's: an uploaded one is + never stored, and a downloaded one is wherever home is now, which + the vehicle keeps setting while it is disarmed. A download which + differs there alone is still the mission being flown""" + mx = self.explorer() + for kwargs in ({}, dict(alt=HOME[2] + 0.4), dict(lat=HOME[0] + 1.0e-7)): + log = self.telemetry_mission(*self.downloaded_mission(**kwargs)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert sorted(cmds) == [0, 1, 2] + assert started == {1: 1} + assert (mission[1][0], mission[1][1]) == \ + pytest.approx(offset(0, 600)[:2]) + # where a mission whose items are somewhere else is another mission + log = self.telemetry_mission(*self.downloaded_mission(north=3050)) + (_, _, _, started, _, _, _) = mx.mission_from_log(log) + assert started == {} + + def transferred(self, items, sysid, target, as_float=False): + """a mission as it travels, from its items as (command, frame, + (param1, param2, param3, param4), lat, lon, alt): as MISSION_ITEM_INT + with lat and lon given in 1e-7 degrees, or as MISSION_ITEM""" + out = [self.mission_count(target, len(items), 0, sysid)] + for (seq, (command, frame, params, lat, lon, alt)) in \ + enumerate(items): + if as_float: + message = mavlink.MAVLink_mission_item_message( + target, 1, seq, frame, command, 0, 1, *params, + lat, lon, alt) + else: + message = mavlink.MAVLink_mission_item_int_message( + target, 1, seq, frame, command, 0, 1, *params, + lat, lon, alt) + out.append(self.mavlink(message, sysid, 0)) + return out + + def uploaded_and_downloaded(self, upload, download, **kwargs): + """where the log says the flight had got to, with the mission + uploaded as upload and downloaded again as download: each a list of + items as transferred() takes them, after home and the takeoff""" + home = [(mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0), + int(HOME[0] * 1e7), int(HOME[1] * 1e7), HOME[2]), + (mavlink.MAV_CMD_NAV_TAKEOFF, 3, (0, 0, 0, 0), 0, 0, 50)] + up_kwargs = dict((k[3:], v) for (k, v) in kwargs.items() + if k.startswith('up_')) + down_kwargs = dict((k[5:], v) for (k, v) in kwargs.items() + if k.startswith('down_')) + up_home = home + if up_kwargs.get('as_float'): + up_home = [item[:3] + (item[3] * 1e-7, item[4] * 1e-7) + item[5:] + for item in home] + log = self.telemetry_mission( + *self.transferred(home + download, 1, 255, **down_kwargs), + upload=self.transferred(up_home + upload, 255, 1, **up_kwargs)) + (_, _, cmds, started, _, _, _) = self.explorer().mission_from_log(log) + assert len(cmds) == 2 + len(upload) + return started + + def test_mavexplorer_knows_a_mission_as_ardupilot_sends_it_back(self): + """AP_Mission keeps an item's position only for a command which has + one, and gives it back in frame 0, 3 or 10 alone; it keeps only + some of an item's parameters, and a plane's waypoint keeps neither + param1 nor param4. A download of the mission as the vehicle sends + it is still the mission being flown""" + (lat, lon, _) = offset(3000, 0) + (lat, lon) = (int(lat * 1e7), int(lon * 1e7)) + speed = mavlink.MAV_CMD_DO_CHANGE_SPEED + waypoint = mavlink.MAV_CMD_NAV_WAYPOINT + loiter = mavlink.MAV_CMD_NAV_LOITER_TURNS + for (upload, download) in ( + # a speed change, which has no position and so no frame + ([(speed, 3, (0, 18, -1, 0), 0, 0, 0)], + [(speed, 0, (0, 18, -1, 0), 0, 0, 0)]), + ([(speed, mavlink.MAV_FRAME_MISSION, (0, 18, -1, 0), + 0, 0, 0)], + [(speed, 0, (0, 18, -1, 0), 0, 0, 0)]), + # a location command uploaded in MISSION, the frame for an + # item with no position, which is kept as an absolute one + ([(mavlink.MAV_CMD_DO_LAND_START, + mavlink.MAV_FRAME_MISSION, (0, 0, 0, 0), lat, lon, 100)], + [(mavlink.MAV_CMD_DO_LAND_START, 0, (0, 0, 0, 0), + lat, lon, 100)]), + # a waypoint's _INT frame + ([(waypoint, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + (0, 0, 0, 0), lat, lon, 100)], + [(waypoint, mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + (0, 0, 0, 0), lat, lon, 100)]), + # a plane's waypoint, uploaded with a delay and a yaw + ([(waypoint, 3, (5, 0, 0, 45), lat, lon, 100)], + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 100)]), + # an altitude, which is kept in centimetres + ([(waypoint, 3, (0, 0, 0, 0), lat, lon, 100.009)], + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 100)]), + # turns kept in 256ths, and a radius in tens of metres + ([(loiter, 3, (0.3, 0, 305, 0), lat, lon, 100)], + [(loiter, 3, (76 / 256.0, 0, 300, 0), lat, lon, 100)])): + assert self.uploaded_and_downloaded(upload, download) == {1: 1} + + def test_mavexplorer_knows_a_mission_changed_by_its_parameters(self): + """where the vehicle does keep a parameter, a mission uploaded again + with it changed is another mission""" + speed = mavlink.MAV_CMD_DO_CHANGE_SPEED + jump = mavlink.MAV_CMD_DO_JUMP + for (upload, download) in ( + ([(speed, 0, (0, 18, -1, 0), 0, 0, 0)], + [(speed, 0, (0, 20, -1, 0), 0, 0, 0)]), + ([(jump, 0, (1, 2, 0, 0), 0, 0, 0)], + [(jump, 0, (1, 3, 0, 0), 0, 0, 0)])): + assert self.uploaded_and_downloaded(upload, download) == {} + + def test_mavexplorer_knows_a_mission_changed_in_altitude_alone(self): + """a waypoint moved up or down, with its position left where it + was, is another mission""" + waypoint = mavlink.MAV_CMD_NAV_WAYPOINT + (lat, lon, _) = offset(3000, 0) + (lat, lon) = (int(lat * 1e7), int(lon * 1e7)) + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 100)], + [(waypoint, 3, (0, 0, 0, 0), lat, lon, 105)]) == {} + + def test_mavexplorer_knows_a_mission_uploaded_as_floats_sent_back(self): + """AP_Mission turns a MISSION_ITEM's position into 1e-7 degrees in + float32, and gives a MISSION_ITEM back by turning those into float32 + and multiplying them by 1e-7 in float32: either way the position + comes back more than a tenth of a metre from where it went, and the + second as much as a metre and more""" + def float32(value): + return struct.unpack(' 5.0e-6 + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), flat, flon, 100)], + [(waypoint, 3, (0, 0, 0, 0), klat, klon, 100)], + up_as_float=True) == {1: 1} + # and uploaded as MISSION_ITEM_INT, downloaded as MISSION_ITEM + (ilat, ilon) = (int(lat * 1e7), int(lon * 1e7)) + (dlat, dlon) = (float32(float32(ilat) * float32(1.0e-7)), + float32(float32(ilon) * float32(1.0e-7))) + assert abs(dlon - ilon * 1e-7) > 1.0e-5 + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), ilat, ilon, 100)], + [(waypoint, 3, (0, 0, 0, 0), dlat, dlon, 100)], + down_as_float=True) == {1: 1} + # where a waypoint 5m away is somewhere else + (lat, lon, _) = offset(3005, 10) + assert self.uploaded_and_downloaded( + [(waypoint, 3, (0, 0, 0, 0), ilat, ilon, 100)], + [(waypoint, 3, (0, 0, 0, 0), int(lat * 1e7), int(lon * 1e7), + 100)]) == {} + + def test_mavexplorer_reads_a_log_from_before_items_carried_params(self): + # CMD carried neither the frame nor the parameters of an item until + # 2014 + mx = self.explorer() + (lat, lon, _) = offset(2000, 0) + entries = [(0, mavlink.MAV_CMD_NAV_WAYPOINT, HOME[0], HOME[1], HOME[2]), + (1, mavlink.MAV_CMD_NAV_WAYPOINT, lat, lon, 100)] + messages = [self.pos(0, 0)] + for (seq, command, clat, clon, alt) in entries: + messages.append(self.message('CMD', CNum=seq, CId=command, + Lat=clat, Lng=clon, Alt=alt)) + (_, _, cmds, _, _, _, _) = mx.mission_from_log(self.log(*messages)) + assert sorted(cmds) == [0, 1] + assert cmds[1][:3] == pytest.approx((lat, lon, 100)) + assert cmds[1][6] == (0.0, 0.0, 0.0, 0.0) + # and takes the altitude as being above home, as it was then + assert cmds[1][3] == mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + + def test_mavexplorer_drops_a_mission_cleared_away(self): + mx = self.explorer() + log = self.telemetry_mission(self.mavlink( + mavlink.MAVLink_mission_clear_all_message(1, 1), 255, 0)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert cmds == {} + assert mission == [] + assert started == {} + + def test_mavexplorer_reads_a_mission_uploaded_as_float_items(self): + # MISSION_ITEM, which a ground station may upload instead of + # MISSION_ITEM_INT + mx = self.explorer() + (lat, lon, _) = offset(2000, 0) + log = self.telemetry_mission( + self.mission_count(1, 2, 0, 255), + self.mavlink(mavlink.MAVLink_mission_item_message( + 1, 1, 0, 0, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + HOME[0], HOME[1], HOME[2]), 255, 0), + self.mavlink(mavlink.MAVLink_mission_item_message( + 1, 1, 1, 3, mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, + lat, lon, 120), 255, 0)) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + assert sorted(cmds) == [0, 1] + assert cmds[1][:3] == pytest.approx((lat, lon, 120)) + # a mission of its own, so where the one before it was flown is not + # where this one is + assert started == {} + + def test_mavexplorer_takes_the_course_a_vtol_takeoff_left_on(self): + # a QuadPlane climbs where it is, then transitions the way it + # points, which is nowhere in the mission either + mx = self.explorer() + log = self.log(*([self.pos(0, 0)] + self.mission_dump( + takeoff=mavlink.MAV_CMD_NAV_VTOL_TAKEOFF) + [ + self.message('MSG', Message='Mission: 1 VTOLTakeoff'), + self.pos(0, 0, 25), self.pos(0, 0, 50), + self.pos(-10, -10, 50), self.pos(-30, -30, 50)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + # south-west, away from the next waypoint, which is north + assert mx.takeoff_course(path, cmds, started) == pytest.approx(225.0, abs=1.0) + + def test_mavexplorer_gives_the_course_as_what_the_takeoff_flies_on( + self, monkeypatch): + """the course measured over a takeoff is the way a VTOL one points + as it transitions, and the course a fixed-wing one holds: each goes + to the drawing as what it is""" + mx = self.explorer() + given = {} + real = plane_track.mission_track + + def recording(*args, **kwargs): + # heading is the fourth of them, passed either way about + names = ('home', 'items', 'params', 'heading', 'start', 'rally', + 'approach', 'takeoff_course') + given.update(dict(zip(names, args))) + given.update(kwargs) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + for (command, params, field) in ( + (mavlink.MAV_CMD_NAV_TAKEOFF, PARAMS, 'takeoff_course'), + (mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, PARAMS, 'heading'), + (mavlink.MAV_CMD_NAV_TAKEOFF, dict(PARAMS, Q_ENABLE=1), + 'heading')): + log = self.log(*([self.pos(0, 0)] + + self.mission_dump(takeoff=command) + [ + self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(0, 20, 10), self.pos(0, 50, 20), self.pos(0, 90, 30)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + mission = mx.resolve_mission_amsl(mission, HOME[2], params, + mavlink.MAV_TYPE_FIXED_WING) + given.clear() + mx.plane_mission_track(cmds, mission, None, params, + mavlink.MAV_TYPE_FIXED_WING, path, started) + # east, as the log shows it flown + assert given[field] == pytest.approx(90.0, abs=2.0) + other = 'heading' if field == 'takeoff_course' else 'takeoff_course' + assert given[other] is None + + def test_mavexplorer_takes_the_takeoff_course_of_the_last_mission(self): + # a log from before the logger wrote "New mission": the first item + # of a mission written out again still starts it afresh, and where + # the items of the mission before began has to go with it + mx = self.explorer() + log = self.log(*( + [self.pos(0, 0)] + self.mission_dump(new_mission=False) + + [self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(0, 20), self.pos(0, 50)] + + self.mission_dump(new_mission=False) + + [self.pos(0, 60), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(40, 60), self.pos(80, 60)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + course = mx.takeoff_course(path, cmds, started) + # north, the second takeoff, not east, the first + assert mp_util.wrap_180(course) == pytest.approx(0.0, abs=1.0) + + def test_mavexplorer_returns_to_the_rally_points_the_log_ends_with(self): + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + (old_lat, old_lon, _) = offset(-2000, 0, 0) + + def raly(seq, total, lat, lon, alt, **fields): + return self.message('RALY', Tot=total, Seq=seq, Lat=lat, Lng=lon, + Alt=alt, **fields) + dump = self.mission_dump() + # home as the logger writes it, in absolute altitude + dump[1].Frame = 0 + dump[-1] = self.cmd(2, mavlink.MAV_CMD_NAV_WAYPOINT, + *offset(3000, 0)[:2], 100) + dump.append(self.cmd(3, mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0)) + log = self.log(*([self.pos(0, 0)] + dump + [ + raly(0, 2, old_lat, old_lon, 80, Flags=0), + raly(1, 2, old_lat, old_lon, 80, Flags=0), + # written out again, as one point + raly(0, 1, lat, lon, 150, Flags=0)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(lat, lon, 150, 0)] + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING, + rally=rally) + (end_lat, end_lon, end_amsl) = track[-1] + assert mp_util.gps_distance(end_lat, end_lon, lat, lon) < 150 + assert end_amsl == pytest.approx(HOME[2] + 150, abs=15) + + def test_mavexplorer_keeps_rally_points_a_new_one_is_appended_to(self): + """AP_Rally::append() logs the new point alone, with the new total""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + (other_lat, other_lon, _) = offset(-2000, 0, 0) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('RALY', Tot=1, Seq=0, Lat=other_lat, Lng=other_lon, + Alt=80, Flags=0), + self.message('RALY', Tot=2, Seq=1, Lat=lat, Lng=lon, Alt=150, + Flags=0)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(other_lat, other_lon, 80, 0), (lat, lon, 150, 0)] + # while a smaller table drops the points it no longer has + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('RALY', Tot=2, Seq=0, Lat=other_lat, Lng=other_lon, + Alt=80, Flags=0), + self.message('RALY', Tot=2, Seq=1, Lat=lat, Lng=lon, Alt=150, + Flags=0), + self.message('RALY', Tot=1, Seq=0, Lat=lat, Lng=lon, Alt=90, + Flags=0)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(lat, lon, 90, 0)] + + def test_mavexplorer_waits_only_so_long_for_rally_terrain( + self, monkeypatch): + pytest.importorskip("vtk") + mx = self.explorer() + from MAVProxy.modules.mavproxy_map3d import terrain + asked = [] + + def sample(lat, lon, **kwargs): + asked.append(kwargs) + return 300.0 + monkeypatch.setattr(terrain, 'sample_terrain', sample) + frame_valid = 1 << 2 + amsl = mx.rally_point_amsl(HOME[0], HOME[1], 100, + frame_valid | (3 << 3), HOME[2]) + assert amsl == 400.0 + assert asked == [{'timeout': mx.RALLY_TERRAIN_TIMEOUT}] + assert 0 < mx.RALLY_TERRAIN_TIMEOUT <= 10.0 + # and an origin the log does not give falls back to home + assert mx.rally_point_amsl(HOME[0], HOME[1], 100, + frame_valid | (2 << 3), + HOME[2]) == HOME[2] + 100 + + def test_mavexplorer_forgets_rally_points_which_were_cleared(self): + """the logger writes "New rally" before the whole table, even an + empty one, which is all a cleared table is logged as""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('RALY', Tot=1, Seq=0, Lat=lat, Lng=lon, Alt=150, + Flags=0), + self.message('MSG', Message='New rally')])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [] + + def test_mavexplorer_measures_a_rally_point_from_the_ekf_origin(self): + """ORGN type 0 is the EKF origin, type 1 home""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + frame_valid = 1 << 2 + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.message('ORGN', Type=0, Lat=HOME[0], Lng=HOME[1], + Alt=HOME[2] - 50), + self.message('ORGN', Type=1, Lat=HOME[0], Lng=HOME[1], + Alt=HOME[2]), + self.message('RALY', Tot=1, Seq=0, Lat=lat, Lng=lon, Alt=120, + Flags=frame_valid | (2 << 3))])) + (path, mission, cmds, started, rally, origin, _) = mx.mission_from_log(log) + assert origin == (HOME[0], HOME[1], HOME[2] - 50) + assert mx.rally_point_amsl(lat, lon, 120, frame_valid | (2 << 3), + HOME[2], origin) == HOME[2] + 70 + + def test_mavexplorer_takes_rally_points_from_an_older_log(self): + """RALY had no Flags before 4.5, and one point set is logged alone""" + mx = self.explorer() + (lat, lon, _) = offset(2500, 800, 0) + (other_lat, other_lon, _) = offset(-2000, 0, 0) + + def raly(seq, total, lat, lon, alt, **fields): + return self.message('RALY', Tot=total, Seq=seq, Lat=lat, Lng=lon, + Alt=alt, **fields) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + raly(0, 2, other_lat, other_lon, 80), + raly(1, 2, lat, lon, 150), + # each point set again on its own, which is logged alone: the + # other points still stand, rather than being forgotten + raly(1, 2, lat, lon, 160), + raly(0, 2, other_lat, other_lon, 90)])) + (path, mission, cmds, started, rally, _, _) = mx.mission_from_log(log) + assert rally == [(other_lat, other_lon, 90, 0), (lat, lon, 160, 0)] + + @staticmethod + def bearing_to(a, b): + return mp_util.gps_bearing(a[0], a[1], b[0], b[1]) + + def takeoff_bearing(self, track): + '''the course from where the track starts to where it has climbed + to the takeoff altitude''' + climbed = [p for p in track if p[2] >= HOME[2] + 49.5][0] + return self.bearing_to(track[0], climbed) + + def test_mavexplorer_starts_where_the_log_says_the_mission_did(self): + mx = self.explorer() + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + # carried off to the east before the mission started + self.pos(0, 600, 5), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(40, 600, 30), self.pos(80, 600, 60)])) + (path, mission, cmds, started, _, _, _) = mx.mission_from_log(log) + mission = mx.resolve_mission_amsl(mission, HOME[2], PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING, + path, started) + assert track[0] == pytest.approx(offset(0, 600, 5)) + # and takes off from there on the course the log shows it flown, + # north, rather than towards the waypoint away to the north-west + assert mp_util.wrap_180(self.takeoff_bearing(track)) == \ + pytest.approx(0.0, abs=2.0) + assert self.bearing_to(track[0], offset(3000, 0)) < 350.0 + # without the log to go on, it starts from home + track = mx.plane_mission_track(cmds, mission, None, PARAMS, + mavlink.MAV_TYPE_FIXED_WING) + assert track[0][:2] == pytest.approx(HOME[:2]) + + def test_mavexplorer_draws_the_path_from_where_the_log_started(self, monkeypatch): + # the 3D map command itself, with a stand-in for the viewer + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + mx = self.explorer() + drawn = [] + + class Viewer(object): + def __init__(self, title=None): + pass + + def set_mission(self, items, track=None): + drawn.append(track) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + log = self.log(*([self.pos(0, 0)] + self.mission_dump() + [ + self.pos(0, 600, 5), self.message('MSG', Message='Mission: 1 Takeoff'), + self.pos(40, 600, 30), self.pos(80, 600, 60)])) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mavlink.MAV_TYPE_FIXED_WING + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=SimpleNamespace( + condition=None, showdirection=True, showlabels=False, + labelsize=14, sync_xmap=False, + missionpath='flown')), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + (track,) = drawn + assert track[0] == pytest.approx(offset(0, 600, 5)) + # on the course the log shows the takeoff flown on + assert mp_util.wrap_180(self.takeoff_bearing(track)) == \ + pytest.approx(0.0, abs=2.0) + + def test_the_3d_map_draws_the_track_it_is_given(self): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + items = [MissionItem(p[0], p[1], p[2], 0, mavlink.MAV_CMD_NAV_WAYPOINT, i) + for (i, p) in enumerate((offset(0, 0), offset(3000, 0)))] + track = [offset(0, 0), offset(1000, 50), offset(2000, 20), offset(3000, 0)] + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + em.set_mission(items, track) + assert em.mission_line == [em._enu(*p) for p in track] + assert em.mission_markers == [em._enu(i.lat, i.lon, i.alt) for i in items] + # and without one, the items are drawn as they always were + em.set_mission(items) + assert em.mission_line == [em._enu(i.lat, i.lon, i.alt) for i in items] + + +class TestMissionStyles(object): + """the 3D map draws a mission as the path flown, as the geometry of its + items, or plain: straight from item to item with a ring at each loiter""" + + RADIUS = 150.0 + + def items(self): + from MAVProxy.modules.mavproxy_map3d.map3d import MissionItem + (a, b, c) = (offset(0, 0), offset(2000, 0), offset(2000, 2000)) + return [ + MissionItem(a[0], a[1], a[2], 0, mavlink.MAV_CMD_NAV_WAYPOINT, 1), + MissionItem(b[0], b[1], b[2], 0, mavlink.MAV_CMD_NAV_LOITER_TURNS, + 2, 1.0, self.RADIUS, 1.0), + MissionItem(c[0], c[1], c[2], 0, mavlink.MAV_CMD_NAV_WAYPOINT, 3), + ] + + def track(self): + return [offset(0, 0), offset(1000, 30), offset(2000, 2000)] + + def elements(self): + pytest.importorskip("vtk") + import vtk + from MAVProxy.modules.mavproxy_map3d.elements import ElementManager + em = ElementManager(vtk.vtkRenderer(), HOME[0], HOME[1], 1.0) + em.set_home(HOME[2]) + return em + + def distance_from_loiter(self, em, point): + centre = em._enu(*offset(2000, 0)) + return math.hypot(point[0] - centre[0], point[1] - centre[1]) + + def test_the_path_flown_is_drawn_by_default(self): + em = self.elements() + em.set_mission(self.items(), self.track()) + assert em.mission_line == [em._enu(*p) for p in self.track()] + assert em.mission_rings == [] + + def test_geometry(self): + em = self.elements() + em.set_mission(self.items(), self.track()) + em.set_mission_style('geometry') + # round the circle rather than through its centre, with no ring + assert em.mission_rings == [] + assert min(self.distance_from_loiter(em, p) + for p in em.mission_line) > self.RADIUS - 5.0 + # and a mission which comes with no path flown is drawn the same way + # in the flown style + geometry = em.mission_line + em.set_mission_style('flown') + em.set_mission(self.items()) + assert em.mission_line == geometry + + def test_plain(self): + em = self.elements() + items = self.items() + em.set_mission(items, self.track()) + em.set_mission_style('plain') + assert em.mission_line == [em._enu(i.lat, i.lon, i.alt) for i in items] + (ring,) = em.mission_rings + for point in ring: + assert self.distance_from_loiter(em, point) == pytest.approx( + self.RADIUS, abs=1.0) + assert point[2] == pytest.approx(items[1].alt) + # the ring is drawn as well as the line and the markers + assert len(em.actors['mission']) == 3 + # a new mission is drawn in the style last asked for + em.set_mission(items, self.track()) + assert len(em.mission_rings) == 1 + # and a style it does not know leaves it as it is + em.set_mission_style('fancy') + assert em.mission_style == 'plain' + + def test_the_viewer_is_told_the_style_and_the_mission(self, map3d_frame): + # from the parent's Map3D, over the queue, to the child's frame and + # its ElementManager: nothing but the queue stands in + pytest.importorskip("wx") + pytest.importorskip("vtk") + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d.map3d import Map3D + from MAVProxy.modules.mavproxy_map3d.map3d_ui import Map3DFrame + sent = [] + viewer = Map3D.__new__(Map3D) + viewer.child = SimpleNamespace(is_alive=lambda: True) + viewer.object_queue = SimpleNamespace(put=sent.append) + em = self.elements() + frame = map3d_frame(em) + items = self.items() + viewer.set_mission_style('plain') + viewer.set_mission(items, self.track()) + for msg in sent: + Map3DFrame.handle(frame, msg) + assert em.mission_style == 'plain' + assert len(em.mission_rings) == 1 + sent[:] = [] + viewer.set_mission_style('flown') + for msg in sent: + Map3DFrame.handle(frame, msg) + assert em.mission_line == [em._enu(*p) for p in self.track()] + # and the map's own control shows the style it is drawing + assert frame.style_choice.GetStringSelection() == 'flown' + + def live_module(self, style): + module = TestDrawnTrack().live_module('plane') + module.map3d_settings.missionpath = style + return module + + def test_the_live_map_only_flies_the_mission_for_the_path_flown(self, monkeypatch): + calls = [] + real = plane_track.mission_track + + def counting(*args, **kwargs): + calls.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', counting) + for style in ('geometry', 'plain'): + module = self.live_module(style) + module.send_mission() + assert module.sent[-1] is None + assert calls == [] + module = self.live_module('flown') + module.send_mission() + assert module.sent[-1] is not None + + def test_the_live_map_setting(self): + from types import SimpleNamespace + from MAVProxy.modules.lib import mp_settings + from MAVProxy.modules.mavproxy_map3d.map3d import MISSION_STYLES + module = TestDrawnTrack().live_module('plane') + styles = [] + sends = [] + # settings of the kind the module makes, to see the choice enforced + module.map3d_settings = mp_settings.MPSettings([ + ('fpvfov', float, 90.0), ('terrainbrightness', float, 1.25), + ('terrainshading', bool, True), ('terrainwireframe', bool, False), + ('showdirection', bool, True), ('showlabels', bool, False), + ('labelsize', int, 14), + mp_settings.MPSetting('missionpath', str, MISSION_STYLES[0], + choice=MISSION_STYLES)]) + module.map = SimpleNamespace( + is_alive=lambda: True, set_fpv_fov=lambda fov: None, + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_render_settings=lambda *args: None, + set_mission_style=styles.append, + set_mission=lambda items, track=None: sends.append(track)) + module.cmd_map3d(['set', 'missionpath', 'plain']) + assert styles == ['plain'] + assert sends == [None] + # back to the path flown, which is worked out again to draw + module.cmd_map3d(['set', 'missionpath', 'flown']) + assert styles == ['plain', 'flown'] + assert sends[-1] is not None + # and a style there is not, is not taken + module.cmd_map3d(['set', 'missionpath', 'fancy']) + assert module.map3d_settings.missionpath == 'flown' + + def test_mavexplorer_draws_the_style_asked_for(self, monkeypatch): + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + drawn = TestDrawnTrack() + mx = drawn.explorer() + views = [] + + class Viewer(object): + def __init__(self, title=None): + self.styles = [] + self.missions = [] + views.append(self) + + def is_alive(self): + return True + + def set_mission_style(self, style): + self.styles.append(style) + + def set_mission(self, items, track=None): + self.missions.append(track) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + flights = [] + real = mx.plane_mission_track + + def counting(*args, **kwargs): + flights.append(args) + return real(*args, **kwargs) + monkeypatch.setattr(mx, 'plane_mission_track', counting) + log = drawn.log(*([drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.message('MSG', Message='Mission: 1 Takeoff'), + drawn.pos(40, 0, 30), drawn.pos(80, 0, 60)])) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mavlink.MAV_TYPE_FIXED_WING + + class Settings(SimpleNamespace): + def command(self, args): + setattr(self, args[0], args[1]) + settings = Settings(condition=None, showdirection=True, + showlabels=False, labelsize=14, + sync_xmap=False, missionpath='geometry') + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=settings), raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + (view,) = views + assert view.styles == ['geometry'] + assert view.missions == [None] + # the path flown was not worked out for a map not drawing it + assert flights == [] + # asked for later, it is worked out and drawn in the view open + mx.cmd_set(['missionpath', 'flown']) + assert view.styles == ['geometry', 'flown'] + assert view.missions[-1] is not None + assert len(flights) == 1 + # and only the once + mx.cmd_set(['missionpath', 'plain']) + mx.cmd_set(['missionpath', 'flown']) + assert len(flights) == 1 + + def test_the_control_on_the_map_draws_the_style_it_picks(self, + map3d_frame): + em = self.elements() + em.set_mission(self.items(), self.track()) + events = [] + frame = map3d_frame(em, events) + frame.style_choice.SetStringSelection('plain') + frame.on_style_choice(None) + assert em.mission_style == 'plain' + assert len(em.mission_rings) == 1 + # and the setting follows the control, so a view opened next draws + # the mission the same way + assert events == [('mission_style', 'plain')] + + def test_the_live_map_takes_the_style_picked_on_it(self): + module = self.live_module('plain') + module.map.is_alive = lambda: True + module.map.check_events = lambda: [('mission_style', 'flown')] + echoed = [] + module.map.set_mission_style = echoed.append + # the rest of the idle task has nothing to do here + module.send_kml = lambda kml_mod=None: None + module.kml_change_state = None + module.terrain_resolved = False + module.idle_task() + assert module.map3d_settings.missionpath == 'flown' + # the view is told the setting it now has, whatever it was told since + assert echoed == ['flown'] + # and the mission goes again, with the path flown worked out for it + assert module.sent[-1] is not None + + def test_mavexplorer_takes_the_style_picked_on_a_view(self, monkeypatch): + from types import SimpleNamespace + mx = TestDrawnTrack().explorer() + styles = [] + drawn = [] + view = SimpleNamespace( + is_alive=lambda: True, + check_events=lambda: [('mission_style', 'flown')], + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_mission_style=styles.append, + set_mission=lambda items, track=None: drawn.append(track), + mission_to_fly=(['an item'], lambda: ['a path flown'])) + settings = SimpleNamespace(showdirection=True, showlabels=False, + labelsize=14, missionpath='geometry') + monkeypatch.setattr(mx, 'mestate', SimpleNamespace(settings=settings), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', [view]) + mx.poll_map3d_views() + assert settings.missionpath == 'flown' + assert styles == ['flown'] + # the path flown is worked out only now that it is drawn, and once + assert drawn == [['a path flown']] + assert view.mission_to_fly is None + + def unflyable(self, module): + '''give the live module's mission a NAV_DELAY, which cannot be flown + through''' + from types import SimpleNamespace + loader = module.mpstate.module('wp').wploader + loader.wpoints.append(SimpleNamespace( + seq=len(loader.wpoints), command=mavlink.MAV_CMD_NAV_DELAY, + x=0, y=0, z=0, frame=3, param1=10, param2=0, param3=0, param4=0)) + + def test_the_live_map_says_when_a_mission_has_no_path_flown(self, capsys): + module = self.live_module('flown') + self.unflyable(module) + module.send_mission() + assert module.sent[-1] is None + assert 'could not work out the path' in capsys.readouterr().out + # once for the mission, however often it is sent, and when home + # has moved far enough for it to be flown again + module.send_mission() + module.home_amsl += 50.0 + module.send_mission() + assert module.sent[-1] is None + assert capsys.readouterr().out == '' + # and again for another which cannot be flown either + module.mpstate.module('wp').wploader.wpoints[-1].param1 = 20 + module.send_mission() + assert 'could not work out the path' in capsys.readouterr().out + # a mission which can be flown says nothing + module = self.live_module('flown') + module.send_mission() + assert module.sent[-1] is not None + assert capsys.readouterr().out == '' + + def test_the_live_map_says_when_asked_for_a_path_it_cannot_draw( + self, capsys): + from types import SimpleNamespace + from MAVProxy.modules import mavproxy_map3d + module = TestDrawnTrack().live_module('copter') + module.map3d_settings = mavproxy_map3d.make_settings() + styles = [] + module.map = SimpleNamespace( + is_alive=lambda: True, set_fpv_fov=lambda fov: None, + set_mission_arrows=lambda enable: None, + set_mission_labels=lambda enable: None, + set_mission_label_size=lambda size: None, + set_mission_style=styles.append, + set_render_settings=lambda *args: None, + set_mission=lambda items, track=None: module.sent.append(track)) + # flown is where it starts, which is not asking for it + module.send_mission() + assert capsys.readouterr().out == '' + module.cmd_map3d(['set', 'missionpath', 'flown']) + assert 'only a plane' in capsys.readouterr().out + module.send_mission() + assert capsys.readouterr().out == '' + # nor is another style + module.cmd_map3d(['set', 'missionpath', 'plain']) + assert capsys.readouterr().out == '' + # and from the view's own choice + module.map.check_events = lambda: [('mission_style', 'flown')] + module.send_kml = lambda kml_mod=None: None + module.kml_change_state = None + module.terrain_resolved = False + module.idle_task() + assert 'only a plane' in capsys.readouterr().out + + def test_mavexplorer_says_when_a_mission_has_no_path_flown( + self, monkeypatch, capsys): + from types import SimpleNamespace + from MAVProxy.modules.mavproxy_map3d import map3d + drawn = TestDrawnTrack() + mx = drawn.explorer() + views = [] + + class Viewer(object): + def __init__(self, title=None): + self.missions = [] + views.append(self) + + def is_alive(self): + return True + + def set_mission(self, items, track=None): + self.missions.append(track) + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + flights = [] + monkeypatch.setattr(mx, 'plane_mission_track', + lambda *args, **kwargs: flights.append(args)) + + class Settings(SimpleNamespace): + def command(self, args): + setattr(self, args[0], args[1]) + + def open_view(mav_type, style): + log = drawn.log(*([drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.message('MSG', Message='Mission: 1 Takeoff'), + drawn.pos(40, 0, 30), drawn.pos(80, 0, 60)])) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mav_type + settings = Settings(condition=None, showdirection=True, + showlabels=False, labelsize=14, + sync_xmap=False, missionpath=style) + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=settings), raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + return views[-1] + + # a plane's mission which cannot be flown through + view = open_view(mavlink.MAV_TYPE_FIXED_WING, 'flown') + assert view.missions == [None] + assert 'could not work out the path' in capsys.readouterr().out + # and which is not flown again for nothing + assert len(flights) == 1 + mx.cmd_set(['showdirection', 'true']) + assert len(flights) == 1 + # a copter's, left as it starts, says nothing + open_view(mavlink.MAV_TYPE_QUADROTOR, 'flown') + assert capsys.readouterr().out == '' + # but asked for, it does + open_view(mavlink.MAV_TYPE_QUADROTOR, 'geometry') + mx.cmd_set(['missionpath', 'flown']) + assert 'could not work out the path' in capsys.readouterr().out + + +class TestApproachCourse(object): + """where the course of a VTOL landing approach comes from: the wind the + vehicle says it estimates, live, and the course it says it took, in a + log""" + + def live_module(self, approach_landing=True): + module = TestDrawnTrack().live_module( + 'plane', params=dict(PARAMS, Q_ENABLE=1)) + loader = module.mpstate.module('wp').wploader + if approach_landing: + last = loader.wpoints[-1] + (last.command, last.param1) = (mavlink.MAV_CMD_NAV_VTOL_LAND, 1) + module.map.is_alive = lambda: True + return module + + def wind(self, module, direction): + from types import SimpleNamespace + m = SimpleNamespace(direction=direction, speed=5.0, speed_z=0.0) + m.get_type = lambda: 'WIND' + module.mavlink_packet(m) + + def final_course(self, track): + # as the VTOL landing takes over, which is within 30 degrees of + # heading for the landing, or half a radius along the line + return TestVtolApproach().landed(track)[2] + + def test_the_live_map_lands_into_the_wind(self, monkeypatch): + flights = [] + real = plane_track.mission_track + + def recording(*args, **kwargs): + flights.append(kwargs.get('approach')) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module() + module.send_mission() + # still air, until the vehicle says otherwise + assert flights == [None] + self.wind(module, 88.0) + assert flights == [None, 90.0] + assert mp_util.wrap_180(self.final_course(module.sent[-1]) - 90) == \ + pytest.approx(0, abs=45) + # the estimate wandering is not a new flight, and a real change is + self.wind(module, 93.0) + module.send_mission() + assert flights == [None, 90.0] + self.wind(module, 130.0) + assert flights == [None, 90.0, 130.0] + # either side of due south is the one course + self.wind(module, 179.0) + self.wind(module, -176.0) + assert flights == [None, 90.0, 130.0, -180.0] + + def test_a_wind_with_no_estimate_changes_nothing(self, monkeypatch): + # an EKF with no estimate of the wind sends NaN + flights = [] + real = plane_track.mission_track + + def recording(*args, **kwargs): + flights.append(kwargs.get('approach')) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module() + module.send_mission() + self.wind(module, 88.0) + for direction in (float('nan'), float('inf'), -float('inf')): + self.wind(module, direction) + module.send_mission() + # the course stands, so the path is not drawn again + assert module.approach == 90.0 + assert flights == [None, 90.0] + + def test_the_wind_does_not_matter_to_a_mission_landing_otherwise( + self, monkeypatch): + flights = [] + real = plane_track.mission_track + + def recording(*args, **kwargs): + flights.append(kwargs.get('approach')) + return real(*args, **kwargs) + monkeypatch.setattr(plane_track, 'mission_track', recording) + module = self.live_module(approach_landing=False) + module.send_mission() + self.wind(module, 90.0) + module.send_mission() + self.wind(module, 200.0) + module.send_mission() + assert flights == [None] + + def test_mavexplorer_takes_the_course_the_log_says(self, monkeypatch): + from MAVProxy.modules.mavproxy_map3d import map3d + drawn = TestDrawnTrack() + mx = drawn.explorer() + messages = [drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.pos(40, 0, 30), + drawn.message('MSG', Message='Selected an approach path of 45.5'), + drawn.message('MSG', Message='Selected an approach path of -120.0'), + drawn.pos(80, 0, 60)] + result = mx.mission_from_log(drawn.log(*messages)) + assert result[-1] == -120.0 + # none said, none known + messages = [drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.pos(40, 0, 30)] + assert mx.mission_from_log(drawn.log(*messages))[-1] is None + # and it is what the path is flown with + from types import SimpleNamespace + flown_with = [] + + class Viewer(object): + def __init__(self, title=None): + pass + + def __getattr__(self, name): + return lambda *args, **kwargs: None + monkeypatch.setattr(map3d, 'Map3D', Viewer) + monkeypatch.setattr(map3d, 'missing_packages', lambda: []) + monkeypatch.setattr( + mx, 'plane_mission_track', + lambda *args, **kwargs: flown_with.append(args[-1])) + messages = [drawn.pos(0, 0)] + drawn.mission_dump() + [ + drawn.message('MSG', Message='Selected an approach path of 30.0'), + drawn.pos(40, 0, 30), drawn.pos(80, 0, 60)] + log = drawn.log(*messages) + log.rewind = lambda: None + log.params = dict(PARAMS) + log.mav_type = mavlink.MAV_TYPE_FIXED_WING + monkeypatch.setattr(mx, 'mestate', SimpleNamespace( + mlog=log, settings=SimpleNamespace( + condition=None, showdirection=True, showlabels=False, + labelsize=14, sync_xmap=False, missionpath='flown')), + raising=False) + monkeypatch.setattr(mx, 'map3d_views', []) + mx.cmd_map3d([]) + assert flown_with == [30.0] + + def test_mavexplorer_flies_the_course_it_is_given(self): + # the log's mission, landing on an approach, flown both ways + mx = TestDrawnTrack().explorer() + land = offset(0, 0, 60) + far = offset(0, 2000, 150) + cmds = { + 0: (HOME[0], HOME[1], HOME[2], 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 0, (0, 0, 0, 0)), + 1: (far[0], far[1], far[2], 0, + mavlink.MAV_CMD_NAV_WAYPOINT, 1, (0, 0, 0, 0)), + 2: (land[0], land[1], land[2], 0, + mavlink.MAV_CMD_NAV_VTOL_LAND, 2, (1, 0, 0, 0))} + params = dict(PARAMS, Q_ENABLE=1, Q_FW_LND_APR_RAD=250) + mission = mx.resolve_mission_amsl( + mx.mission_items_from_cmds(cmds), HOME[2], params, + mavlink.MAV_TYPE_FIXED_WING) + for approach in (0.0, 90.0): + track = mx.plane_mission_track( + cmds, mission, None, params, mavlink.MAV_TYPE_FIXED_WING, + approach=approach) + assert mp_util.wrap_180(self.final_course(track) - approach) == \ + pytest.approx(0, abs=45)