00001
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
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
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
00257
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)