mavgraph.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 graph a MAVLink log file
00004 Andrew Tridgell August 2011
00005 '''
00006 
00007 import sys, struct, time, os, datetime
00008 import math, re
00009 import matplotlib
00010 from math import *
00011 
00012 try:
00013     from pymavlink.mavextra import *
00014 except:
00015     print("WARNING: Numpy missing, mathematical notation will not be supported.")
00016 
00017 # cope with rename of raw_input in python3
00018 try:
00019     input = raw_input
00020 except NameError:
00021     pass
00022 
00023 colourmap = {
00024     'apm' : {
00025         'MANUAL'    : (1.0,   0,   0),
00026         'AUTO'      : (  0, 1.0,   0),
00027         'LOITER'    : (  0,   0, 1.0),
00028         'FBWA'      : (1.0, 0.5,   0),
00029         'RTL'       : (  1,   0, 0.5),
00030         'STABILIZE' : (0.5, 1.0,   0),
00031         'LAND'      : (  0, 1.0, 0.5),
00032         'STEERING'  : (0.5,   0, 1.0),
00033         'HOLD'      : (  0, 0.5, 1.0),
00034         'ALT_HOLD'  : (1.0, 0.5, 0.5),
00035         'CIRCLE'    : (0.5, 1.0, 0.5),
00036         'POSITION'  : (1.0, 0.0, 1.0),
00037         'GUIDED'    : (0.5, 0.5, 1.0),
00038         'ACRO'      : (1.0, 1.0,   0),
00039         'CRUISE'    : (  0, 1.0, 1.0)
00040         },
00041     'px4' : {
00042         'MANUAL'    : (1.0,   0,   0),
00043         'SEATBELT'  : (  0.5, 0.5,   0),
00044         'EASY'      : (  0, 1.0,   0),
00045         'AUTO'    : (  0,   0, 1.0),
00046         'UNKNOWN'    : (  1.0,   1.0, 1.0)
00047         }
00048     }
00049 
00050 edge_colour = (0.1, 0.1, 0.1)
00051 
00052 lowest_x = None
00053 highest_x = None
00054 
00055 def plotit(x, y, fields, colors=[]):
00056     '''plot a set of graphs using date for x axis'''
00057     global lowest_x, highest_x
00058     pylab.ion()
00059     fig = pylab.figure(num=1, figsize=(12,6))
00060     ax1 = fig.gca()
00061     ax2 = None
00062     xrange = 0.0
00063     for i in range(0, len(fields)):
00064         if len(x[i]) == 0: continue
00065         if lowest_x is None or x[i][0] < lowest_x:
00066             lowest_x = x[i][0]
00067         if highest_x is None or x[i][-1] > highest_x:
00068             highest_x = x[i][-1]
00069     if highest_x is None or lowest_x is None:
00070         return
00071     xrange = highest_x - lowest_x
00072     xrange *= 24 * 60 * 60
00073     formatter = matplotlib.dates.DateFormatter('%H:%M:%S')
00074     interval = 1
00075     intervals = [ 1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600,
00076                   900, 1800, 3600, 7200, 5*3600, 10*3600, 24*3600 ]
00077     for interval in intervals:
00078         if xrange / interval < 15:
00079             break
00080     locator = matplotlib.dates.SecondLocator(interval=interval)
00081     if not args.xaxis:
00082         ax1.xaxis.set_major_locator(locator)
00083         ax1.xaxis.set_major_formatter(formatter)
00084     empty = True
00085     ax1_labels = []
00086     ax2_labels = []
00087     for i in range(0, len(fields)):
00088         if len(x[i]) == 0:
00089             print("Failed to find any values for field %s" % fields[i])
00090             continue
00091         if i < len(colors):
00092             color = colors[i]
00093         else:
00094             color = 'red'
00095         (tz, tzdst) = time.tzname
00096         if axes[i] == 2:
00097             if ax2 == None:
00098                 ax2 = ax1.twinx()
00099             ax = ax2
00100             if not args.xaxis:
00101                 ax2.xaxis.set_major_locator(locator)
00102                 ax2.xaxis.set_major_formatter(formatter)
00103             label = fields[i]
00104             if label.endswith(":2"):
00105                 label = label[:-2]
00106             ax2_labels.append(label)
00107         else:
00108             ax1_labels.append(fields[i])
00109             ax = ax1
00110         if args.xaxis:
00111             if args.marker is not None:
00112                 marker = args.marker
00113             else:
00114                 marker = '+'
00115             if args.linestyle is not None:
00116                 linestyle = args.linestyle
00117             else:
00118                 linestyle = 'None'
00119             ax.plot(x[i], y[i], color=color, label=fields[i],
00120                     linestyle=linestyle, marker=marker)
00121         else:
00122             if args.marker is not None:
00123                 marker = args.marker
00124             else:
00125                 marker = 'None'
00126             if args.linestyle is not None:
00127                 linestyle = args.linestyle
00128             else:
00129                 linestyle = '-'
00130             ax.plot_date(x[i], y[i], color=color, label=fields[i],
00131                          linestyle=linestyle, marker=marker, tz=None)
00132         empty = False
00133     if args.flightmode is not None:
00134         for i in range(len(modes)-1):
00135             c = colourmap[args.flightmode].get(modes[i][1], edge_colour)
00136             ax1.axvspan(modes[i][0], modes[i+1][0], fc=c, ec=edge_colour, alpha=0.1)
00137         c = colourmap[args.flightmode].get(modes[-1][1], edge_colour)
00138         ax1.axvspan(modes[-1][0], ax1.get_xlim()[1], fc=c, ec=edge_colour, alpha=0.1)
00139     if ax1_labels != []:
00140         ax1.legend(ax1_labels,loc=args.legend)
00141     if ax2_labels != []:
00142         ax2.legend(ax2_labels,loc=args.legend2)
00143     if empty:
00144         print("No data to graph")
00145         return
00146 
00147 
00148 from argparse import ArgumentParser
00149 parser = ArgumentParser(description=__doc__)
00150 
00151 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
00152 parser.add_argument("--planner", action='store_true', help="use planner file format")
00153 parser.add_argument("--condition", default=None, help="select packets by a condition")
00154 parser.add_argument("--labels", default=None, help="comma separated field labels")
00155 parser.add_argument("--legend", default='upper left', help="default legend position")
00156 parser.add_argument("--legend2", default='upper right', help="default legend2 position")
00157 parser.add_argument("--marker", default=None, help="point marker")
00158 parser.add_argument("--linestyle", default=None, help="line style")
00159 parser.add_argument("--xaxis", default=None, help="X axis expression")
00160 parser.add_argument("--multi", action='store_true', help="multiple files with same colours")
00161 parser.add_argument("--zero-time-base", action='store_true', help="use Z time base for DF logs")
00162 parser.add_argument("--flightmode", default=None,
00163                     help="Choose the plot background according to the active flight mode of the specified type, e.g. --flightmode=apm for ArduPilot or --flightmode=px4 for PX4 stack logs.  Cannot be specified with --xaxis.")
00164 parser.add_argument("--dialect", default="ardupilotmega", help="MAVLink dialect")
00165 parser.add_argument("--output", default=None, help="provide an output format")
00166 parser.add_argument("--timeshift", type=float, default=0, help="shift time on first graph in seconds")
00167 parser.add_argument("logs_fields", metavar="<LOG or FIELD>", nargs="+")
00168 args = parser.parse_args()
00169 
00170 from pymavlink import mavutil
00171 
00172 if args.flightmode is not None and args.xaxis:
00173     print("Cannot request flightmode backgrounds with an x-axis expression")
00174     sys.exit(1)
00175 
00176 if args.flightmode is not None and args.flightmode not in colourmap:
00177     print("Unknown flight controller '%s' in specification of --flightmode" % args.flightmode)
00178     sys.exit(1)
00179 
00180 
00181 if args.output is not None:
00182     matplotlib.use('Agg')
00183 
00184 import pylab
00185 
00186 filenames = []
00187 fields = []
00188 for f in args.logs_fields:
00189     if os.path.exists(f):
00190         filenames.append(f)
00191     else:
00192         fields.append(f)
00193 msg_types = set()
00194 multiplier = []
00195 field_types = []
00196 
00197 colors = [ 'red', 'green', 'blue', 'orange', 'olive', 'black', 'grey', 'yellow', 'brown', 'darkcyan', 'cornflowerblue', 'darkmagenta', 'deeppink', 'darkred']
00198 
00199 # work out msg types we are interested in
00200 x = []
00201 y = []
00202 modes = []
00203 axes = []
00204 first_only = []
00205 re_caps = re.compile('[A-Z_][A-Z0-9_]+')
00206 for f in fields:
00207     caps = set(re.findall(re_caps, f))
00208     msg_types = msg_types.union(caps)
00209     field_types.append(caps)
00210     y.append([])
00211     x.append([])
00212     axes.append(1)
00213     first_only.append(False)
00214 
00215 def add_data(t, msg, vars, flightmode):
00216     '''add some data'''
00217     mtype = msg.get_type()
00218     if args.flightmode is not None and (len(modes) == 0 or modes[-1][1] != flightmode):
00219         modes.append((t, flightmode))
00220     if mtype not in msg_types:
00221         return
00222     for i in range(0, len(fields)):
00223         if mtype not in field_types[i]:
00224             continue
00225         f = fields[i]
00226         if f.endswith(":2"):
00227             axes[i] = 2
00228             f = f[:-2]
00229         if f.endswith(":1"):
00230             first_only[i] = True
00231             f = f[:-2]
00232         v = mavutil.evaluate_expression(f, vars)
00233         if v is None:
00234             continue
00235         if args.xaxis is None:
00236             xv = t
00237         else:
00238             xv = mavutil.evaluate_expression(args.xaxis, vars)
00239             if xv is None:
00240                 continue
00241         y[i].append(v)
00242         x[i].append(xv)
00243 
00244 def process_file(filename, timeshift):
00245     '''process one file'''
00246     print("Processing %s" % filename)
00247     mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps, zero_time_base=args.zero_time_base, dialect=args.dialect)
00248     vars = {}
00249 
00250     while True:
00251         msg = mlog.recv_match(args.condition)
00252         if msg is None: break
00253         try:
00254             tdays = matplotlib.dates.date2num(datetime.datetime.fromtimestamp(msg._timestamp+timeshift))
00255         except ValueError:
00256             # this can happen if the log is corrupt
00257             # ValueError: year is out of range
00258             break
00259         add_data(tdays, msg, mlog.messages, mlog.flightmode)
00260 
00261 if len(filenames) == 0:
00262     print("No files to process")
00263     sys.exit(1)
00264 
00265 if args.labels is not None:
00266     labels = args.labels.split(',')
00267     if len(labels) != len(fields)*len(filenames):
00268         print("Number of labels (%u) must match number of fields (%u)" % (
00269             len(labels), len(fields)*len(filenames)))
00270         sys.exit(1)
00271 else:
00272     labels = None
00273 
00274 timeshift = args.timeshift
00275 
00276 for fi in range(0, len(filenames)):
00277     f = filenames[fi]
00278     process_file(f, timeshift)
00279     timeshift = 0
00280     for i in range(0, len(x)):
00281         if first_only[i] and fi != 0:
00282             x[i] = []
00283             y[i] = []
00284     if labels:
00285         lab = labels[fi*len(fields):(fi+1)*len(fields)]
00286     else:
00287         lab = fields[:]
00288     if args.multi:
00289         col = colors[:]
00290     else:
00291         col = colors[fi*len(fields):]
00292     plotit(x, y, lab, colors=col)
00293     for i in range(0, len(x)):
00294         x[i] = []
00295         y[i] = []
00296 if args.output is None:
00297     pylab.show()
00298     pylab.draw()
00299     input('press enter to exit....')
00300 else:
00301     pylab.legend(loc=2,prop={'size':8})
00302     pylab.savefig(args.output, bbox_inches='tight', dpi=200)


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57