gpsprof

   1 #!/usr/bin/env python
   2 #
   3 # This file is Copyright (c) 2010 by the GPSD project
   4 # BSD terms apply: see the file COPYING in the distribution root for details.
   5 #
   6 # Collect and plot latency-profiling data from a running gpsd.
   7 # Requires gnuplot.
   8 #
   9 import sys, os, time, getopt, tempfile, time, socket, math, copy
  10 import gps
  11
  12 class Baton:
  13     "Ship progress indication to stderr."
  14     def __init__(self, prompt, endmsg=None):
  15         self.stream = sys.stderr
  16         self.stream.write(prompt + "...")
  17         if os.isatty(self.stream.fileno()):
  18             self.stream.write(" \010")
  19         self.stream.flush()
  20         self.count = 0
  21         self.endmsg = endmsg
  22         self.time = time.time()
  23         return
  24
  25     def twirl(self, ch=None):
  26         if self.stream is None:
  27             return
  28         if ch:
  29             self.stream.write(ch)
  30         elif os.isatty(self.stream.fileno()):
  31             self.stream.write("-/|\\"[self.count % 4])
  32             self.stream.write("\010")
  33         self.count = self.count + 1
  34         self.stream.flush()
  35         return
  36
  37     def end(self, msg=None):
  38         if msg == None:
  39             msg = self.endmsg
  40         if self.stream:
  41             self.stream.write("...(%2.2f sec) %s.\n" % (time.time() - self.time, msg))
  42         return
  43
  44 class spaceplot:
  45     "Total times without instrumentation."
  46     name = "space"
  47     def __init__(self):
  48         self.fixes = []
  49     def d(self, a, b):
  50         return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
  51     def gather(self, session):
  52         # Include altitude, not used here, for 3D plot experiments.
  53         # Watch out for the NaN value from gps.py.
  54         self.fixes.append((session.fix.latitude, session.fix.longitude, session.fix.altitude))
  55         return True
  56     def header(self, session):
  57         res = "# Position uncertainty, %s, %s, %ds cycle\n" % \
  58                  (title, session.gps_id, session.cycle)
  59         return res
  60     def data(self, session):
  61         res = ""
  62         for i in range(len(self.recentered)):
  63             (lat, lon) = self.recentered[i][:2]
  64             (raw1, raw2, alt) = self.fixes[i]
  65             res += "%f\t%f\t%f\t%f\t%f\n" % (lat, lon, raw1, raw2, alt)
  66         return res
  67     def plot(self, title, session):
  68         if len(self.fixes) == 0:
  69             sys.stderr.write("No fixes collected, can't estimate accuracy.")
  70             sys.exit(1)
  71         # centroid is just arithmetic avg of lat,lon
  72         self.centroid = (sum(map(lambda x:x[0], self.fixes))/len(self.fixes), sum(map(lambda x:x[1], self.fixes))/len(self.fixes))
  73         # Sort fixes by distance from centroid
  74         self.fixes.sort(lambda x, y: cmp(self.d(self.centroid, x), self.d(self.centroid, y)))
  75         # Convert fixes to offsets from centroid in meters
  76         self.recentered = map(lambda fix: gps.MeterOffset(self.centroid, fix[:2]), self.fixes)
  77         # Compute CEP(50%)
  78         cep_meters = gps.EarthDistance(self.centroid[:2], self.fixes[len(self.fixes)/2][:2])
  79         alt_sum = 0
  80         alt_num = 0
  81         alt_fixes = []
  82         lon_max = -9999
  83         for i in range(len(self.recentered)):
  84             (lat, lon) = self.recentered[i][:2]
  85             (raw1, raw2, alt) = self.fixes[i]
  86             if not gps.isnan(alt):
  87                     alt_sum += alt
  88                     alt_fixes.append( alt)
  89                     alt_num += 1
  90             if lon > lon_max :
  91                     lon_max = lon
  92         if alt_num == 0:
  93             alt_avg = gps.NaN
  94             alt_ep = gps.NaN
  95         else:
  96             alt_avg = alt_sum / alt_num
  97             # Sort fixes by distance from average altitude
  98             alt_fixes.sort(lambda x, y: cmp(abs(alt_avg - x), abs(alt_avg - y)))
  99             alt_ep = abs( alt_fixes[ len(alt_fixes)/2 ] - alt_avg)
 100         if self.centroid[0] < 0:
 101             latstring = "%fS" % -self.centroid[0]
 102         elif self.centroid[0] == 0:
 103             latstring = "0"
 104         else:
 105             latstring = "%fN" % self.centroid[0]
 106         if self.centroid[1] < 0:
 107             lonstring = "%fW" % -self.centroid[1]
 108         elif self.centroid[1] == 0:
 109             lonstring = "0"
 110         else:
 111             lonstring = "%fE" % self.centroid[1]
 112         fmt = "set autoscale\n"
 113         fmt += 'set key below\n'
 114         fmt += 'set key title "%s"\n' % time.asctime()
 115         fmt += 'set size ratio -1\n'
 116         fmt += 'set style line 2 pt 1\n'
 117         fmt += 'set style line 3 pt 2\n'
 118         fmt += 'set xlabel "Meters east from %s"\n' % lonstring
 119         fmt += 'set ylabel "Meters north from %s"\n' % latstring
 120         fmt += 'set border 15\n'
 121         if not gps.isnan(alt_avg):
 122             fmt += 'set y2label "Meters Altitude from %f"\n' % alt_avg
 123             fmt += 'set ytics nomirror\n'
 124             fmt += 'set y2tics\n'
 125         fmt += 'cep=%f\n' % self.d((0,0), self.recentered[len(self.fixes)/2])
 126         fmt += 'set parametric\n'
 127         fmt += 'set trange [0:2*pi]\n'
 128         fmt += 'cx(t, r) = sin(t)*r\n'
 129         fmt += 'cy(t, r) = cos(t)*r\n'
 130         fmt += 'chlen = cep/20\n'
 131         fmt += "set arrow from -chlen,0 to chlen,0 nohead\n"
 132         fmt += "set arrow from 0,-chlen to 0,chlen nohead\n"
 133         fmt += 'plot  cx(t, cep),cy(t, cep) title "CEP (50%%) = %f meters",  ' % (cep_meters)
 134         fmt += ' "-" using 1:2 with points ls 3 title "%d GPS fixes" ' % (len(self.fixes))
 135         if not gps.isnan(alt_avg):
 136             fmt += ', "-" using ( %f ):($5 < 100000 ? $5 - %f : 1/0) axes x1y2 with points ls 2 title " %d Altitude fixes, Average = %f, EP (50%%) = %f" \n' % (lon_max +1, alt_avg, alt_num, alt_avg, alt_ep)
 137         else:
 138             fmt += "\n"
 139         fmt += self.header(session)
 140         fmt += self.data(session)
 141         if not gps.isnan(alt_avg):
 142             fmt += "e\n" + self.data(session)
 143         return fmt
 144
 145 class uninstrumented:
 146     "Total times without instrumentation."
 147     name = "uninstrumented"
 148     def __init__(self):
 149         self.stats = []
 150     def gather(self, session):
 151         if session.fix.time:
 152             seconds = time.time() - session.fix.time
 153             self.stats.append(seconds)
 154             return True
 155         else:
 156             return False
 157     def header(self, session):
 158         return "# Uninstrumented total latency, %s, %s, %dN%d, cycle %ds\n" % \
 159                  (title,
 160                   session.gps_id, session.baudrate,
 161                   session.stopbits, session.cycle)
 162     def data(self, session):
 163         res = ""
 164         for seconds in self.stats:
 165             res += "%2.6lf\n" % seconds
 166         return res
 167     def plot(self, title, session):
 168         fmt = '''
 169 set autoscale
 170 set key below
 171 set key title "Uninstrumented total latency, %s, %s, %dN%d, cycle %ds"
 172 plot "-" using 0:1 title "Total time" with impulses
 173 '''
 174         res = fmt % (title,
 175                       session.gps_id, session.baudrate,
 176                       session.stopbits, session.cycle)
 177         res += self.header(session)
 178         return res + self.data(session)
 179
 180 class rawplot:
 181     "All measurement, no deductions."
 182     name = "raw"
 183     def __init__(self):
 184         self.stats = []
 185     def gather(self, session):
 186         self.stats.append(copy.copy(session.timings))
 187         return True
 188     def header(self, session):
 189         res = "# Raw latency data, %s, %s, %dN%d, cycle %ds\n" % \
 190                  (title,
 191                   session.gps_id, session.baudrate,
 192                   session.stopbits, session.cycle)
 193         res += "# tag    len    xmit                  "
 194         for hn in ("T1", "D1", "E2", "T2", "D2"):
 195             res += "%-13s" % hn
 196         res += "\n#------- -----  --------------------"
 197         for i in range(0, 5):
 198             res += "  " + ("-" * 11)
 199         return res + "\n"
 200     def data(self, session):
 201         res = ""
 202         for timings in self.stats:
 203             res += "% 8s  %4d  %2.9f  %2.9f  %2.9f  %2.9f  %2.9f  %2.9f\n" \
 204                 % (timings.tag,
 205                    timings.len,
 206                    timings.xmit,
 207                    timings.recv - timings.xmit,
 208                    timings.decode - timings.recv,
 209                    timings.emit - timings.decode,
 210                    timings.c_recv - timings.emit,
 211                    timings.c_decode - timings.c_recv)
 212         return res
 213     def plot(self, file, session):
 214         fmt = '''
 215 set autoscale
 216 set key below
 217 set key title "Raw latency data, %s, %s, %dN%d, cycle %ds"
 218 plot \
 219      "-" using 0:8 title "D2 = Client decode time" with impulses, \
 220      "-" using 0:7 title "T2 =     TCP/IP latency" with impulses, \
 221      "-" using 0:6 title "E2 = Daemon encode time" with impulses, \
 222      "-" using 0:5 title "D1 = Daemon decode time" with impulses, \
 223      "-" using 0:4 title "T1 =         RS232 time" with impulses
 224 '''
 225         res = fmt % (title,
 226                       session.gps_id, session.baudrate,
 227                       session.stopbits, session.cycle)
 228         res += self.header(session)
 229         for dummy in range(0, 5):
 230             res += self.data(session) + "e\n"
 231         return res
 232
 233 class splitplot:
 234     "Discard base time, use color to indicate different tags."
 235     name = "split"
 236     sentences = []
 237     def __init__(self):
 238         self.stats = []
 239     def gather(self, session):
 240         self.stats.append(copy.copy(session.timings))
 241         if session.timings.tag not in self.sentences:
 242             self.sentences.append(session.timings.tag)
 243         return True
 244     def header(self, session):
 245         res = "# Split latency data, %s, %s, %dN%d, cycle %ds\n#" % \
 246                  (title,
 247                   session.gps_id, session.baudrate,
 248                   session.stopbits, session.cycle)
 249         for s in splitplot.sentences:
 250             res += "%8s\t" % s
 251         for hn in ("T1", "D1", "E2", "T2", "D2", "length"):
 252             res += "%8s\t" % hn
 253         res += "tag\n# "
 254         for s in tuple(splitplot.sentences) + ("T1", "D1", "E2", "T2", "D2", "length"):
 255             res += "---------\t"
 256         return res + "--------\n"
 257     def data(self, session):
 258         res = ""
 259         for timings in self.stats:
 260             for s in splitplot.sentences:
 261                 if s == timings.tag:
 262                     res += "%2.6f\t" % timings.xmit
 263                 else:
 264                     res += "-       \t"
 265             res += "%2.6f\t%2.6f\t%2.6f\t%2.6f\t%2.6f\t%8d\t# %s\n" \
 266                      % (timings.recv - timings.xmit,
 267                         timings.decode - timings.recv,
 268                         timings.emit - timings.decode,
 269                         timings.c_recv - timings.emit,
 270                         timings.c_decode - timings.c_recv,
 271                         timings.len,
 272                         timings.tag)
 273         return res
 274     def plot(self, title, session):
 275         fixed = '''
 276 set autoscale
 277 set key below
 278 set key title "Filtered latency data, %s, %s, %dN%d, cycle %ds"
 279 plot \
 280      "-" using 0:%d title "D2 = Client decode time" with impulses, \
 281      "-" using 0:%d title "T2 = TCP/IP latency" with impulses, \
 282      "-" using 0:%d title "E2 = Daemon encode time" with impulses, \
 283      "-" using 0:%d title "D1 = Daemon decode time" with impulses, \
 284      "-" using 0:%d title "T1 = RS3232 time" with impulses, \
 285 '''
 286         sc = len(splitplot.sentences)
 287         fmt = fixed % (title,
 288                        session.gps_id, session.baudrate,
 289                        session.stopbits, session.cycle,
 290                        sc+5,
 291                        sc+4,
 292                        sc+3,
 293                        sc+2,
 294                        sc+1)
 295         for i in range(sc):
 296             fmt += '     "-" using 0:%d title "%s" with impulses,' % \
 297                    (i+1, self.sentences[i])
 298         res = fmt[:-1] + "\n"
 299         res += self.header(session)
 300         for dummy in range(sc+5):
 301             res += self.data(session) + "e\n"
 302         return res
 303
 304 class cycle:
 305     "Send-cycle analysis."
 306     name = "cycle"
 307     def __init__(self):
 308         self.stats = []
 309     def gather(self, session):
 310         self.stats.append(copy.copy(session.timings))
 311         return True
 312     def plot(self, title, session):
 313         msg = ""
 314         def roundoff(n):
 315             # Round a time to hundredths of a second
 316             return round(n*100) / 100.0
 317         intervals = {}
 318         last_seen = {}
 319         for timing in self.stats:
 320             # Throw out everything but the leader in each GSV group
 321             if timing.tag[-3:] == "GSV" and last_command[-3:] == "GSV":
 322                 continue
 323             last_command = timing.tag
 324             # Record timings
 325             received = timing.d_received()
 326             if not timing.tag in intervals:
 327                 intervals[timing.tag] = []
 328             if timing.tag in last_seen:
 329                 intervals[timing.tag].append(roundoff(received - last_seen[timing.tag]))
 330             last_seen[timing.tag] = received
 331
 332         # Step three: get command frequencies and the basic send cycle time
 333         frequencies = {}
 334         for (key, interval_list) in intervals.items():
 335             frequencies[key] = {}
 336             for interval in interval_list:
 337                 frequencies[key][interval] = frequencies[key].get(interval, 0) + 1
 338         # filter out noise
 339         for key in frequencies:
 340             distribution = frequencies[key]
 341             for interval in distribution.keys():
 342                 if distribution[interval] < 2:
 343                     del distribution[interval]
 344         cycles = {}
 345         for key in frequencies:
 346             distribution = frequencies[key]
 347             if len(frequencies[key].values()) == 1:
 348                 # The value is uniqe after filtering
 349                 cycles[key] = distribution.keys()[0]
 350             else:
 351                 # Compute the mode
 352                 maxfreq = 0
 353                 for (interval, frequency) in distribution.items():
 354                     if distribution[interval] > maxfreq:
 355                         cycles[key] = interval
 356                         maxfreq = distribution[interval]
 357         msg += "Cycle report %s, %s, %dN%d, cycle %ds" % \
 358                  (title,
 359                   session.gps_id, session.baudrate,
 360                   session.stopbits, session.cycle)
 361         msg += "The sentence set emitted by this GPS is: %s\n" % " ".join(intervals.keys())
 362         for key in cycles:
 363             if len(frequencies[key].values()) == 1:
 364                 if cycles[key] == 1:
 365                     msg += "%s: is emitted once a second.\n" % key
 366                 else:
 367                     msg += "%s: is emitted once every %d seconds.\n" % (key, cycles[key])
 368             else:
 369                 if cycles[key] == 1:
 370                     msg += "%s: is probably emitted once a second.\n" % key
 371                 else:
 372                     msg += "%s: is probably emitted once every %d seconds.\n" % (key, cycles[key])
 373         sendcycle = min(*cycles.values())
 374         if sendcycle == 1:
 375             msg += "Send cycle is once per second.\n"
 376         else:
 377             msg += "Send cycle is once per %d seconds.\n" % sendcycle
 378         return msg
 379
 380 formatters = (spaceplot, uninstrumented, rawplot, splitplot, cycle)
 381
 382 def plotframe(await, fname, speed, threshold, title):
 383     "Return a string containing a GNUplot script "
 384     if fname:
 385         for formatter in formatters:
 386             if formatter.name == fname:
 387                 plotter = formatter()
 388                 break
 389         else:
 390             sys.stderr.write("gpsprof: no such formatter.\n")
 391             sys.exit(1)
 392     try:
 393         session = gps.gps(verbose=verbose)
 394     except socket.error:
 395         sys.stderr.write("gpsprof: gpsd unreachable.\n")
 396         sys.exit(1)
 397     # Initialize
 398     session.poll()
 399     if session.version == None:
 400         print >>sys.stderr, "gpsprof: requires gpsd to speak new protocol."
 401         sys.exit(1)
 402     session.send("?DEVICES;")
 403     while session.poll() != -1:
 404         if session.data["class"] == "DEVICES":
 405             break
 406     if len(session.data.devices) != 1:
 407         print >>sys.stderr, "gpsprof: exactly one device must be attached."
 408         sys.exit(1)
 409     device = session.data.devices[0]
 410     path = device["path"]
 411     session.baudrate = device["bps"]
 412     session.parity = device["bps"]
 413     session.stopbits = device["stopbits"]
 414     session.cycle = device["cycle"]
 415     session.gps_id = device["driver"]
 416     # Set parameters
 417     if speed:
 418         session.send('?DEVICE={"path":"%s","bps:":%d}' % (path, speed))
 419         session.poll()
 420         if session.baudrate != speed:
 421             sys.stderr.write("gpsprof: baud rate change failed.\n")
 422     options = ""
 423     if formatter not in (spaceplot, uninstrumented):
 424         options = ',"timing":true'
 425     try:
 426         #session.set_raw_hook(lambda x: sys.stderr.write(`x`+"\n"))
 427         session.send('?WATCH={"enable":true%s}' % options)
 428         baton = Baton("gpsprof: looking for fix", "done")
 429         countdown = await
 430         basetime = time.time()
 431         while countdown > 0:
 432             if session.poll() == -1:
 433                 sys.stderr.write("gpsprof: gpsd has vanished.\n")
 434                 sys.exit(1)
 435             baton.twirl()
 436             if session.data["class"] == "WATCH":
 437                 if "timing" in options and not session.data.get("timing"):
 438                     sys.stderr.write("gpsprof: timing is not enabled.\n")
 439                     sys.exit(1)
 440             # We can get some funky artifacts at start of session
 441             # apparently due to RS232 buffering effects. Ignore
 442             # them.
 443             if threshold and time.time()-basetime < session.cycle * threshold:
 444                 continue
 445             if session.fix.mode <= gps.MODE_NO_FIX:
 446                 continue
 447             if countdown == await:
 448                 sys.stderr.write("first fix in %.2fsec, gathering %d samples..." % (time.time()-basetime,await))
 449             if plotter.gather(session):
 450                 countdown -= 1
 451         baton.end()
 452     finally:
 453         session.send('?WATCH={"enable":false,"timing":false}')
 454     command = plotter.plot(title, session)
 455     del session
 456     return command
 457
 458 if __name__ == '__main__':
 459     try:
 460         (options, arguments) = getopt.getopt(sys.argv[1:], "f:hm:n:s:t:D:")
 461
 462         formatter = "space"
 463         raw = False
 464         speed = 0
 465         title = time.ctime()
 466         threshold = 0
 467         await = 100
 468         verbose = 0
 469         for (switch, val) in options:
 470             if (switch == '-f'):
 471                 formatter = val
 472             elif (switch == '-m'):
 473                 threshold = int(val)
 474             elif (switch == '-n'):
 475                 await = int(val)
 476             elif (switch == '-s'):
 477                 speed = int(val)
 478             elif (switch == '-t'):
 479                 title = val
 480             elif (switch == '-D'):
 481                 verbose = int(val)
 482             elif (switch == '-h'):
 483                 sys.stderr.write(\
 484                     "usage: gpsprof [-h] [-D debuglevel] [-m threshold] [-n samplecount] \n"
 485                      + "\t[-f {" + "|".join(map(lambda x: x.name, formatters)) + "}] [-s speed] [-t title]\n")
 486                 sys.exit(0)
 487         sys.stdout.write(plotframe(await,formatter,speed,threshold,title))
 488     except KeyboardInterrupt:
 489         pass
 490
 491 # The following sets edit modes for GNU EMACS
 492 # Local Variables:
 493 # mode:python
 494 # End: