MAVExplorer: allow arbitrary expressions to be plotted on the map

this allows mavextra to construct new mappable objects with Lat and
Lng fields, allowing for derived positions from logs

this also generalises and simplifies the handling of instances

Author: tridge

MAVProxy/tools/MAVExplorer.py

@@ -455,7 +455,7 @@ def cmd_map(args):
     options.colour_source='flightmode'
     options.nkf_sample = 1
     if len(args) > 0:
-        options.types = ','.join(args)
+        options.types = ':'.join(args)
         if len(options.types) > 1:
             options.colour_source='type'
     mfv_mav_ret = mavflightview.mavflightview_mav(mestate.mlog, options, mestate.flightmode_selections)

MAVProxy/tools/mavflightview.py

@@ -253,6 +253,64 @@ def colour_for_point_type(mlog, point, instance, options):
              'violet', 'purple', 'grey', 'black']
     return map_colours[instance]
 
+def message_to_latlon(type, m, is_expression=False):
+    '''convert a message to a lat/lon'''
+    if type in ['GPS','GPS2'] and not is_expression:
+        status = getattr(m, 'Status', None)
+        nsats = getattr(m, 'NSats', None)
+        # prevent mapping when no fix
+        if status is None:
+            status = getattr(m, 'FixType', None)
+        if status is None:
+            return None
+        if nsats is None:
+            nsats = 0
+        if status < 2 and nsats < 5:
+            return None
+        # flash log
+        lat = m.Lat
+        lng = getattr(m, 'Lng', None)
+        if lng is None:
+            lng = getattr(m, 'Lon', None)
+        if lng is None:
+            return None
+        return lat, lng
+
+    if hasattr(m,'Lat') and hasattr(m,'Lng'):
+        return m.Lat, m.Lng
+    if hasattr(m,'Lat') and hasattr(m,'Lon'):
+        return m.Lat, m.Lon
+    if hasattr(m,'PN') and hasattr(m,'PE'):
+        pos = mavextra.ekf1_pos(m)
+        if pos is None:
+            return None
+        return pos[0], pos[1]
+    if hasattr(m,'lat') and hasattr(m,'lon'):
+        return m.lat*1.0e-7, m.lon*1.0e-7
+    if hasattr(m,'lat') and hasattr(m,'lng'):
+        return m.lat*1.0e-7, m.lng*1.0e-7
+    if hasattr(m,'latitude') and hasattr(m,'longitude'):
+        return m.latitude*1.0e-7, m.longitude*1.0e-7
+    return None
+
+class PosExpression:
+    '''object repesenting a map expression, with the types that we need to look for in the log'''
+    def __init__(self, expression):
+        self.expression = expression
+        re_caps = re.compile('[A-Z_][A-Z0-9_]+')
+        caps = set(re.findall(re_caps, expression))
+        self.recv_match_types = caps
+
+    def __repr__(self):
+        return "Expression(%s,%s)" % (self.expression, self.recv_match_types)
+
+def pos_expressions(type_list):
+    '''return a list of PosExpression objects for a type list'''
+    ret = []
+    for t in type_list:
+        ret.append(PosExpression(t))
+    return ret
+
 def mavflightview_mav(mlog, options=None, flightmode_selections=[]):
     '''create a map for a log file'''
     wp = mavwp.MAVWPLoader()
@@ -266,51 +324,45 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]):
         if s:
             all_false = False
     idx = 0
-    path = [[]]
-    instances = {}
+    path = []
     ekf_counter = 0
     nkf_counter = 0
+    expressions = []
+
     types = ['MISSION_ITEM', 'MISSION_ITEM_INT', 'CMD']
     if options.types is not None:
-        types.extend(options.types.split(','))
+        if options.types.find(':'):
+            type_list = options.types.split(':')
+        else:
+            type_list = options.types.split(',')
+        expressions.extend(pos_expressions(type_list))
     else:
-        types.extend(['POS','GLOBAL_POSITION_INT'])
+        expressions.extend(pos_expressions(['POS', 'GLOBAL_POSITION_INT']))
         if options.rawgps or options.dualgps:
-            types.extend(['GPS', 'GPS_RAW_INT'])
+            expressions.extend(pos_expressions(['GPS', 'GPS_RAW_INT']))
         if options.rawgps2 or options.dualgps:
-            types.extend(['GPS2_RAW','GPS2'])
+            expressions.extend(pos_expressions(['GPS2_RAW','GPS2']))
         if options.ekf:
-            types.extend(['EKF1', 'GPS'])
+            expressions.extend(pos_expressions(['EKF1', 'GPS']))
         if options.nkf:
-            types.extend(['NKF1', 'GPS'])
+            expressions.extend(pos_expressions(['NKF1', 'GPS']))
         if options.ahr2:
-            types.extend(['AHR2', 'AHRS2', 'GPS'])
-
-    # handle forms like GPS[0], mapping to GPS for recv_match_types
-
-    # it may be possible to pass conditions in to recv_match_types,
-    # but for now we filter to desired instances later.
-    want_instances = {}
-    recv_match_types = types[:]
-    for i in range(len(recv_match_types)):
-        match = re.match('(?P<name>.*)\[(?P<instancenum>[^\]]+)\]', recv_match_types[i])
-        if match is not None:
-            name = match.group("name")
-            number = match.group("instancenum")
-            if name not in want_instances:
-                want_instances[name] = set()
-            want_instances[name].add(number)
-            recv_match_types[i] = name
+            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()
+    for e in expressions:
+        recv_match_types.update(set(e.recv_match_types))
 
     colour_source = getattr(options, "colour_source")
-    re_caps = re.compile('[A-Z_][A-Z0-9_]+')
 
     if colour_source is not None:
         # stolen from mavgraph.py
+        re_caps = re.compile('[A-Z_][A-Z0-9_]+')
         caps = set(re.findall(re_caps, colour_source))
-        recv_match_types.extend(caps)
+        recv_match_types.update(caps)
 
-    print("Looking for types %s" % str(recv_match_types))
+    print("Looking for types %s" % str(list(recv_match_types)))
 
     last_timestamps = {}
     used_flightmodes = {}
@@ -377,126 +429,70 @@ def mavflightview_mav(mlog, options=None, flightmode_selections=[]):
 
         if not mlog.check_condition(options.condition):
             continue
-        if options.mode is not None and mlog.flightmode.lower() != options.mode.lower():
-            continue
 
-        if not type in types and type not in want_instances:
-            # may only be present for colour-source expressions to work
+        if options.mode is not None and mlog.flightmode.lower() != options.mode.lower():
             continue
 
-        type_with_instance = type
-        try:
-            # remember that "m" here might be a mavlink message.
-            instance_field = m.fmt.instance_field
-            m_instance_field_value = eval(f"m.{instance_field}")
-            if (type in want_instances and
-                str(m_instance_field_value) not in want_instances[type]
-                ):
-                continue
-
-            type_with_instance = '%s[%u]' % (type, m_instance_field_value)
-        except Exception:
-            pass
-
         if not all_false and len(flightmode_selections) > 0 and idx < len(options._flightmodes) and m._timestamp >= options._flightmodes[idx][2]:
             idx += 1
         elif (idx < len(flightmode_selections) and flightmode_selections[idx]) or all_false or len(flightmode_selections) == 0:
             used_flightmodes[mlog.flightmode] = 1
-            (lat, lng) = (None,None)
-            if type in ['GPS','GPS2']:
-                status = getattr(m, 'Status', None)
-                nsats = getattr(m, 'NSats', None)
-                if status is None:
-                    status = getattr(m, 'FixType', None)
-                    if status is None:
-                        print("Can't find status on GPS message")
-                        print(m)
-                        break
-                if nsats is None:
-                    nsats = 0
-                if status < 2 and nsats < 5:
+            for instance in range(len(expressions)):
+                expression = expressions[instance]
+                if not type in expression.recv_match_types:
                     continue
-                # flash log
-                lat = m.Lat
-                lng = getattr(m, 'Lng', None)
-                if lng is None:
-                    lng = getattr(m, 'Lon', None)
-                    if lng is None:
-                        print("Can't find longitude on GPS message")
-                        print(m)
-                        break
-            elif type in ['EKF1', 'ANU1']:
-                pos = mavextra.ekf1_pos(m)
-                if pos is None:
+
+                # evaluate the expression
+                is_expression = (type != expression.expression)
+                if not is_expression:
+                    # this is a simple type as an expression
+                    v = m
+                else:
+                    # we need to evaluate the expression to produce an object
+                    try:
+                        v = mavutil.evaluate_expression(expression.expression, mlog.messages)
+                    except Exception:
+                        continue
+                if v is None:
                     continue
-                ekf_counter += 1
-                if ekf_counter % options.ekf_sample != 0:
+                latlng = message_to_latlon(type, v, is_expression)
+                if latlng is None:
                     continue
-                (lat, lng, alt) = pos
-            elif type in ['NKF1','XKF1']:
-                pos = mavextra.ekf1_pos(m)
-                if pos is None:
+                lat,lng = latlng
+
+                while len(path) <= instance:
+                    path.append([])
+
+                # only plot thing we have a valid-looking location for:
+                if abs(lat)<=0.01 and abs(lng)<=0.01:
                     continue
-                nkf_counter += 1
-                if nkf_counter % options.nkf_sample != 0:
+
+                colour = colour_for_point(mlog, (lat, lng), instance, options)
+                if colour is None:
                     continue
-                (lat, lng, alt) = pos
-            elif type in ['ANU5']:
-                (lat, lng) = (m.Alat*1.0e-7, m.Alng*1.0e-7)
-            elif type in ['AHR2', 'POS', 'CHEK']:
-                (lat, lng) = (m.Lat, m.Lng)
-            elif type == 'AHRS2':
-                (lat, lng) = (m.lat*1.0e-7, m.lng*1.0e-7)
-            elif type == 'ORGN':
-                (lat, lng) = (m.Lat, m.Lng)
-            elif type == 'SIM':
-                (lat, lng) = (m.Lat, m.Lng)
-            elif type == 'GUID':
-                if (m.Type == 0):
-                    (lat, lng) = (m.pX*1.0e-7, m.pY*1.0e-7)
-            else:
-                if hasattr(m,'Lat'):
-                    lat = m.Lat
-                if hasattr(m,'Lon'):
-                    lng = m.Lon
-                if hasattr(m,'Lng'):
-                    lng = m.Lng
-                if hasattr(m,'lat'):
-                    lat = m.lat * 1.0e-7
-                if hasattr(m,'lon'):
-                    lng = m.lon * 1.0e-7
-                if hasattr(m,'lng'):
-                    lng = m.lng * 1.0e-7
-                if hasattr(m,'latitude'):
-                    lat = m.latitude * 1.0e-7
-                if hasattr(m,'longitude'):
-                    lng = m.longitude * 1.0e-7
-
-            if lat is None or lng is None:
-                continue
-
-            # automatically add new types to instances
-            if type_with_instance not in instances:
-                instances[type_with_instance] = len(instances)
-                while len(instances) >= len(path):
-                    path.append([])
-            instance = instances[type_with_instance]
 
-            # only plot thing we have a valid-looking location for:
-            if abs(lat)<=0.01 and abs(lng)<=0.01:
-                continue
+                tdays = grapher.timestamp_to_days(m._timestamp)
+                point = (lat, lng, colour, tdays)
+
+                if options.rate == 0 or not expression.expression in last_timestamps or m._timestamp - last_timestamps[expression.expression] > 1.0/options.rate:
+                    last_timestamps[expression.expression] = m._timestamp
+                    path[instance].append(point)
 
-            colour = colour_for_point(mlog, (lat, lng), instance, options)
-            if colour is None:
-                continue
+    # remove any empty paths and construct instances array
+    paths2 = []
+    instances = {}
+    for instance in range(len(expressions)):
+        e = expressions[instance]
+        if instance >= len(path):
+            break
+        if len(path[instance]) == 0:
+            continue
+        paths2.append(path[instance])
+        instances[e.expression] = instance
 
-            tdays = grapher.timestamp_to_days(m._timestamp)
-            point = (lat, lng, colour, tdays)
+    path = paths2
 
-            if options.rate == 0 or not type_with_instance in last_timestamps or m._timestamp - last_timestamps[type_with_instance] > 1.0/options.rate:
-                last_timestamps[type_with_instance] = m._timestamp
-                path[instance].append(point)
-    if len(path[0]) == 0:
+    if len(path) == 0:
         print("No points to plot")
         return None
 
@@ -691,8 +687,6 @@ def __init__(self):
     parser.add_option("--debug", action='store_true', default=False, help="show debug info")
     parser.add_option("--multi", action='store_true', default=False, help="show multiple flights on one map")
     parser.add_option("--types", default=None, help="types of position messages to show")
-    parser.add_option("--ekf-sample", type='int', default=1, help="sub-sampling of EKF messages")
-    parser.add_option("--nkf-sample", type='int', default=1, help="sub-sampling of NKF messages")
     parser.add_option("--rate", type='int', default=0, help="maximum message rate to display (0 means all points)")
     parser.add_option("--colour-source", type="str", default="flightmode", help="expression with range 0f..255f used for point colour")
     parser.add_option("--no-flightmode-legend", action="store_false", default=True, dest="show_flightmode_legend", help="hide legend for colour used for flight modes")