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