Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

MAVExplorer: allow arbitrary expressions to be plotted on the map #1416

Merged
merged 1 commit into from
Jul 4, 2024
Merged

MAVExplorer: allow arbitrary expressions to be plotted on the map #1416

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion MAVProxy/tools/MAVExplorer.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
262 changes: 128 additions & 134 deletions MAVProxy/tools/mavflightview.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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 = {}
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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")
Expand Down