path: root/gpsprof

#!/usr/bin/env python
#
# Collect and plot latency-profiling data from a running gpsd.
# Requires gnuplot.
#
import sys, os, time, getopt, gps, tempfile, time, socket, math, copy

class Baton:
    "Ship progress indication to stderr."
    def __init__(self, prompt, endmsg=None):
        self.stream = sys.stderr
        self.stream.write(prompt + "... \010")
        self.stream.flush()
        self.count = 0
        self.endmsg = endmsg
        self.time = time.time()
        return

    def twirl(self, ch=None):
        if self.stream is None:
            return
        if ch:
            self.stream.write(ch)
        else:
            self.stream.write("-/|\\"[self.count % 4])
            self.stream.write("\010")
        self.count = self.count + 1
        self.stream.flush()
        return

    def end(self, msg=None):
        if msg == None:
            msg = self.endmsg
        if self.stream:
            self.stream.write("...(%2.2f sec) %s.\n" % (time.time() - self.time, msg))
        return

class spaceplot:
    "Total times without instrumentation."
    name = "space"
    def __init__(self, fp):
        self.fixes = []
        self.fp = fp
    def gather(self, session):
        # Include altitude, not used here, for 3D plot experiments.
        # Watch out for the ALTITUDE_NOT_VALID value from gps.py.
        self.fixes.append((session.fix.latitude, session.fix.longitude, session.fix.altitude))
        return True
    def data(self, session):
        self.fp.write("# Position uncertainty, %s, %s, %ds cycle\n" % \
                 (title, session.gps_id, session.cycle))
    def plot(self, file, title, session):
        if len(self.fixes) == 0:
            sys.stderr.write("No fixes collected, can't estimate accuracy.")
            sys.exit(1)
        else:
            centroid = (sum(map(lambda x:x[0], self.fixes))/len(self.fixes), sum(map(lambda x:x[1], self.fixes))/len(self.fixes))
            # Sort fixes by distance from centroid
            def d(a, b): return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
            self.fixes.sort(lambda x, y: cmp(d(centroid, x), d(centroid, y)))
            # Compute CEP(50%)
            cep_meters = gps.EarthDistance(centroid[:2], self.fixes[len(self.fixes)/2][:2])
            # Convert fixes to offsets from centroid in meters
            recentered = map(lambda fix: gps.MeterOffset(centroid, fix[:2]), self.fixes)
	    alt_sum = 0
	    alt_num = 0
	    lon_max = -9999
            for i in range(len(recentered)):
                (lat, lon) = recentered[i][:2]
                (raw1, raw2, alt) = self.fixes[i]
                if alt > -999 :
			alt_sum += alt
			alt_num += 1
		if lon > lon_max :
			lon_max = lon
                self.fp.write("%f	%f	%f	%f	%f\n" % (lat, lon, raw1, raw2, alt))
            self.fp.flush()
	    alt_avg = alt_sum / alt_num
            if centroid[0] < 0:
                latstring = "%fS" % -centroid[0]
            elif centroid[0] == 0:
                latstring = "0"
            else:
                latstring = "%fN" % centroid[0]
            if centroid[1] < 0:
                lonstring = "%fW" % -centroid[1]
            elif centroid[1] == 0:
                lonstring = "0"
            else:
                lonstring = "%fE" % centroid[1]
            fmt = ""
            fmt += "set autoscale\n"
            fmt += 'set key below\n'
            fmt += 'set key title "%s"\n' % time.asctime()
            fmt += 'set size ratio -1\n'
            fmt += 'set style line 2 pt 1\n'
            fmt += 'set style line 3 pt 2 # Looks good on X11\n'
            fmt += 'set xlabel "Meters east from %s"\n' % lonstring
            fmt += 'set ylabel "Meters north from %s"\n' % latstring
            fmt += 'set y2label "Meters Altitude from %f"\n' % alt_avg
            fmt += 'set border 15\n'
            fmt += 'set ytics nomirror\n'
            fmt += 'set y2tics\n'
            fmt += 'cep=%f\n' % d((0,0), recentered[len(self.fixes)/2])
            fmt += 'set parametric\n'
            fmt += 'set trange [0:2*pi]\n'
            fmt += 'cx(t, r) = sin(t)*r\n'
            fmt += 'cy(t, r) = cos(t)*r\n'
            fmt += 'chlen = cep/20\n'
            fmt += "set arrow from -chlen,0 to chlen,0 nohead\n"
            fmt += "set arrow from 0,-chlen to 0,chlen nohead\n"
            fmt += 'plot cx(t, cep),cy(t, cep) title "CEP (50%%) = %f meters",  ' % (cep_meters)
            fmt += ' "%s" using 1:2 with points ls 3 title "%d GPS fixes" , ' % (file, len(self.fixes))
            fmt += '"%s" using ( %f ):($5 > -999 ? $5 - %f : 1/0) axes x1y2 with points ls 2 title " %d Altitude fixes, Average = %f" \n' % ( file, lon_max + 1, alt_avg, alt_num, alt_avg)
            return fmt

class uninstrumented:
    "Total times without instrumentation."
    name = "uninstrumented"
    def __init__(self, fp):
        self.fp = fp
        self.stats = []
    def gather(self, session):
        if session.gps_time:
            seconds = time.time() - session.gps_time + gps.tzoffset()
            self.stats.append(seconds)
            return True
        else:
            return False
    def data(self, session):
        self.fp.write("# Uninstrumented total latency, %s, %s, %dN%d, cycle %ds\n" % \
                 (title,
                  session.gps_id, session.baudrate,
                  session.stopbits, session.cycle))
        for seconds in self.stats:
            self.fp.write("%2.6lf\n" % seconds)
    def plot(self, file, title, session):
        fmt = '''
set autoscale
set key below
set key title "Uninstrumented total latency, %s, %s, %dN%d, cycle %ds"
plot "%s" using 0:1 title "Total time" with impulses
'''
        return fmt % (title,
                      session.gps_id, session.baudrate,
                      session.stopbits, session.cycle,
                      file)

class rawplot:
    "All measurement, no deductions."
    name = "raw"
    def __init__(self, fp):
        self.fp = fp
        self.stats = []
    def gather(self, session):
        if session.gps_time:
            self.stats.append(copy.copy(session.timings))
            return True
        else:
            return False
    def data(self, session):
        self.fp.write("# Raw latency data, %s, %s, %dN%d, cycle %ds\n" % \
                 (title,
                  session.gps_id, session.baudrate,
                  session.stopbits, session.cycle))
        self.fp.write("#\t")
        for hn in ("T1", "E1", "D1", "W", "E2", "T2", "D2"):
            self.fp.write("%8s\t" % hn)
        self.fp.write("tag\n#-\t")
        for i in range(0, 7):
            self.fp.write("--------\t")
        self.fp.write("--------\n")
        for timings in self.stats:
            self.fp.write("%2d	%2.6f	%2.6f	%2.6f	%2.6f	%2.6f	%2.6f	%2.6f  	# %s\n" \
              % (timings.length,
                 timings.d_xmit_time, 
                 timings.d_recv_time,
                 timings.d_decode_time,
                 timings.poll_time,
                 timings.emit_time,
                 timings.c_recv_time,
                 timings.c_decode_time,
                 timings.tag))
    def plot(self, file, title, session):
        fmt = '''
set autoscale
set key below
set key title "Raw latency data, %s, %s, %dN%d, cycle %ds"
plot \
     "%s" using 0:8 title "D2 = Client decode time" with impulses, \
     "%s" using 0:7 title "T2 =     TCP/IP latency" with impulses, \
     "%s" using 0:6 title "E2 = Daemon encode time" with impulses, \
     "%s" using 0:5 title "W  =     Poll wait time" with impulses, \
     "%s" using 0:4 title "D1 = Daemon decode time" with impulses, \
     "%s" using 0:3 title "T1 =         RS232 time" with impulses, \
     "%s" using 0:2 title "E1 =        GPS latency" with impulses
'''
        return fmt % (title,
                      session.gps_id, session.baudrate,
                      session.stopbits, session.cycle,
                      file, file, file, file, file, file, file)

class splitplot:
    "Discard base time, use color to indicate different tags."
    name = "split"
    sentences = []
    def __init__(self, fp):
        self.found = {}
        self.fp = fp
        self.stats = []
    def gather(self, session):
        if session.gps_time:
            self.stats.append(copy.copy(session.timings))
            if session.timings.tag not in splitplot.sentences:
                splitplot.sentences.append(session.timings.tag)
            return True
        else:
            return False
    def data(self, session):
        self.fp.write("# Split latency data, %s, %s, %dN%d, cycle %ds\n" % \
                 (title,
                  session.gps_id, session.baudrate,
                  session.stopbits, session.cycle))
        self.fp.write("#")
        for s in splitplot.sentences:
            self.fp.write("%8s\t" % s)
        for hn in ("T1", "D1", "W", "E2", "T2", "D2", "length"):
            self.fp.write("%8s\t" % hn)
        self.fp.write("tag\n# ")
        for s in tuple(splitplot.sentences) + ("T1", "D1", "W", "E2", "T2", "D2", "length"):
            self.fp.write("---------\t")
        self.fp.write("--------\n")
        for timings in self.stats:
            for s in splitplot.sentences:
                if s == timings.tag:
                    self.fp.write("%2.6f\t"% timings.d_xmit_time)
                    self.found[s] = True
                else:
                    self.fp.write("-       \t")
            self.fp.write("%2.6f	%2.6f	%2.6f	%2.6f	%2.6f	%2.6f	%8d	# %s\n" \
                     % (timings.d_recv_time,
                        timings.d_decode_time,
                        timings.poll_time,
                        timings.emit_time,
                        timings.c_recv_time,
                        timings.c_decode_time,
                        timings.length,
                        timings.tag))
    def plot(self, file, title, session):
        fixed = '''
set autoscale
set key below
set key title "Filtered latency data, %s, %s, %dN%d, cycle %ds"
plot \\
     "%s" using 0:%d title "D2 = Client decode time" with impulses, \
     "%s" using 0:%d title "T2 = TCP/IP latency" with impulses, \
     "%s" using 0:%d title "E2 = Daemon encode time" with impulses, \
     "%s" using 0:%d title "W  = Poll wait time" with impulses, \
     "%s" using 0:%d title "D1 = Daemon decode time" with impulses, \
     "%s" using 0:%d title "T1 = RS3232 time" with impulses, \
'''
        sc = len(splitplot.sentences)
        fmt = fixed % (title,
                       session.gps_id, session.baudrate,
                       session.stopbits, session.cycle,
                       file, sc+6,
                       file, sc+5,
                       file, sc+4,
                       file, sc+3,
                       file, sc+2,
                       file, sc+1)
        for i in range(sc):
            if splitplot.sentences[i] in self.found:
                fmt += '     "%s" using 0:%d title "%s" with impulses, \\\n' % \
                       (file, i+1, splitplot.sentences[i])
        return fmt[:-4] + "\n"

formatters = (spaceplot, rawplot, splitplot, uninstrumented)

def plotframe(await, fname, file, speed, threshold, title):
    "Return a string containing a GNUplot script "
    if file:
        out = open(file, "w")
    elif fname == None:
        out = sys.stdout
    else:
        out = tempfile.NamedTemporaryFile()
    if fname:
        for formatter in formatters:
            if formatter.name == fname:
                plotter = formatter(out)
                break
        else:
            sys.stderr.write("gpsprof: no such formatter.\n")
            sys.exit(1)
    try:
        session = gps.gps()
    except socket.error:
        sys.stderr.write("gpsprof: gpsd unreachable.\n")
        sys.exit(1)
    try:
        if speed:
            session.query("b=%d" % speed)
            if session.baudrate != speed:
                sys.stderr.write("gpsprof: baud rate change failed.\n")
        session.query("w+bci")
        if formatter not in (spaceplot, uninstrumented):
            session.query("z+")
        #session.set_raw_hook(lambda x: sys.stderr.write(`x`+"\n"))
        baton = Baton("gpsprof: looking for fix", "done")
        countdown = await
        while countdown > 0:
            if session.poll() == None:
                sys.stderr.write("gpsprof: gpsd has vanished.\n")
                sys.exit(1)
            baton.twirl()
            if session.fix.mode <= gps.MODE_NO_FIX:
                continue
            if countdown == await:
                sys.stderr.write("gathering samples...")
            # We can get some funky artifacts at start of session
            # apparently due to RS232 buffering effects. Ignore
            # them.
            if threshold and session.timings.c_decode_time > session.cycle * threshold:
                continue
            if plotter.gather(session):
                countdown -= 1
        baton.end()
    finally:
        session.query("w-z-")
    plotter.data(session)
    out.flush()
    command = plotter.plot(out.name, title, session)
    del session
    return (out, command)

if __name__ == '__main__':
    try:
        (options, arguments) = getopt.getopt(sys.argv[1:], "f:hm:n:o:rs:t:T:")
        formatter = "space"
        raw = False
        file = None
        speed = 0
        terminal = None
        title = time.ctime()
        threshold = 0
        await = 100
        for (switch, val) in options:
            if (switch == '-f'):
                formatter = val
            elif (switch == '-m'):
                threshold = int(val)
            elif (switch == '-n'):
                await = int(val)
            elif (switch == '-o'):
                file = val
            elif (switch == '-r'):
                raw = True
            elif (switch == '-s'):
                speed = int(val)
            elif (switch == '-t'):
                title = val
            elif (switch == '-T'):
                terminal = val
            elif (switch == '-h'):
                sys.stderr.write(\
                    "usage: gpsprof [-h] [-r] [-m threshold] [-n samplecount] \n"
                     + "\t[-f {" + "|".join(map(lambda x: x.name, formatters)) + "}] [-s speed] [-t title] [-o file]\n")
                sys.exit(0)
        (out, gnuplot) = plotframe(await,formatter,file,speed,threshold,title)
        if not raw:
            if terminal:
                gnuplot = "set terminal " + terminal + "\n" + gnuplot
            pfp = os.popen("gnuplot -persist", "w")
            pfp.write(gnuplot)
            pfp.close()
        out.close()
    except KeyboardInterrupt:
        pass