Draw the path a plane flies its mission along, and fix the arc/loiter drawing

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)

MAVProxy/modules/lib/mission_item_protocol.py

@@ -884,6 +884,7 @@ def cmd_show(self, args):
             print("usage: wp show <filename>")
             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):

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.