1#!/usr/local/bin/python3.8
2#
3'''
4Collect and plot latency-profiling data from a running gpsd.
5Requires gnuplot, but gnuplot can be on another host.
6'''
7
8# This file is Copyright (c) 2010 by the GPSD project
9# SPDX-License-Identifier: BSD-2-clause
10#
11# Updated to conform with RCC-219-00, RCC/IRIG Standard 261-00
12# "STANDARD REPORT FORMAT FOR GLOBAL POSITIONING SYSTEM (GPS) RECEIVERS AND
13#  SYSTEMS ACCURACY TESTS AND EVALUATIONS"
14#
15# TODO: put date from data on plot, not time of replot.
16# TODO: add lat/lon to polar plots
17#
18# This code runs compatibly under Python 2 and 3.x for x >= 2.
19# Preserve this property!
20from __future__ import absolute_import, print_function, division
21
22import copy
23import getopt
24import math
25import os
26import signal
27import socket
28import sys
29import time
30
31# pylint wants local modules last
32try:
33    import gps
34except ImportError as e:
35    sys.stderr.write(
36        "gpsprof: can't load Python gps libraries -- check PYTHONPATH.\n")
37    sys.stderr.write("%s\n" % e)
38    sys.exit(1)
39
40gps_version = '3.20'
41if gps.__version__ != gps_version:
42    sys.stderr.write("gpsprof: ERROR: need gps module version %s, got %s\n" %
43                     (gps_version, gps.__version__))
44    sys.exit(1)
45
46debug = False
47
48
49def dist_2d(a, b):
50    "calculate distance between a[x,y] and b[x,y]"
51
52    # x and y are orthogonal, probably lat/lon in meters
53    # ignore altitude change.
54    return math.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2)
55
56
57def dist_3d(a, b):
58    "calculate distance between a[x,y,z] and b[x,y,z]"
59    # x, y, and z are othogonal, probably ECEF, probably in meters
60    return math.sqrt((a[0] - b[0]) ** 2 +
61                     (a[1] - b[1]) ** 2 +
62                     (a[2] - b[2]) ** 2)
63
64
65def wgs84_to_ecef(wgs84):
66    "Convert wgs84 coordinates to ECEF ones"
67
68    # unpack args
69    (lat, lon, alt) = wgs84
70    # convert lat/lon/altitude in degrees and altitude in meters
71    # to ecef x, y, z in meters
72    # see
73    # http://www.mathworks.de/help/toolbox/aeroblks/llatoecefposition.html
74    lat = math.radians(lat)
75    lon = math.radians(lon)
76
77    rad = 6378137.0          # Radius of the Earth (in meters)
78    f = 1.0 / 298.257223563  # Flattening factor WGS84 Model
79    cosLat = math.cos(lat)
80    sinLat = math.sin(lat)
81    FF = (1.0 - f) ** 2
82    C = 1 / math.sqrt((cosLat ** 2) + (FF * sinLat ** 2))
83    S = C * FF
84
85    x = (rad * C + alt) * cosLat * math.cos(lon)
86    y = (rad * C + alt) * cosLat * math.sin(lon)
87    z = (rad * S + alt) * sinLat
88
89    return (x, y, z)
90
91
92class Baton(object):
93    "Ship progress indication to stderr."
94
95    def __init__(self, prompt, endmsg=None):
96        self.stream = sys.stderr
97        self.stream.write(prompt + "...")
98        if os.isatty(self.stream.fileno()):
99            self.stream.write(" \b")
100        self.stream.flush()
101        self.count = 0
102        self.endmsg = endmsg
103        self.time = time.time()
104
105    def twirl(self, ch=None):
106        "Twirl the baton"
107        if self.stream is None:
108            return
109        if ch:
110            self.stream.write(ch)
111        elif os.isatty(self.stream.fileno()):
112            self.stream.write("-/|\\"[self.count % 4])
113            self.stream.write("\b")
114        self.count = self.count + 1
115        self.stream.flush()
116        return
117
118    def end(self, msg=None):
119        "Write the end message"
120        if msg is None:
121            msg = self.endmsg
122        if self.stream:
123            self.stream.write("...(%2.2f sec) %s.\n"
124                              % (time.time() - self.time, msg))
125
126
127class stats(object):
128    "Class for 1D stats: min, max, mean, sigma, skewness, kurtosis"
129
130    def __init__(self):
131        self.min = 0.0
132        self.max = 0.0
133        self.mean = 0.0
134        self.median = 0.0
135        self.sigma = 0.0
136        self.skewness = 0.0
137        self.kurtosis = 0.0
138
139    def __str__(self):
140        "return a nice string, for debug"
141        return ("min %f, max %f, mean %f, median %f, sigma %f, skewedness %f, "
142                "kurtosis %f" %
143                (self.min, self.max, self.mean, self.median,
144                 self.sigma, self.skewness, self.kurtosis))
145
146    def min_max_mean(self, fixes, index):
147        "Find min, max, and mean of fixes[index]"
148
149        if not fixes:
150            return
151
152        # might be fast to go through list once?
153        if isinstance(fixes[0], tuple):
154            self.mean = (sum([x[index] for x in fixes]) / len(fixes))
155            self.min = min([x[index] for x in fixes])
156            self.max = max([x[index] for x in fixes])
157        else:
158            # must be float
159            self.mean = (sum([x for x in fixes]) / len(fixes))
160            self.min = min([x for x in fixes])
161            self.max = max([x for x in fixes])
162
163        return
164
165    def moments(self, fixes, index):
166        "Find and set the (sigma, skewness, kurtosis) of fixes[index]"
167
168        # The skewness of a random variable X is the third standardized
169        # moment and is a dimension-less ratio. ntpviz uses the Pearson's
170        # moment coefficient of skewness.  Wikipedia describes it
171        # best: "The qualitative interpretation of the skew is complicated
172        # and unintuitive."  A normal distribution has a skewness of zero.
173        self.skewness = float('nan')
174
175        # The kurtosis of a random variable X is the fourth standardized
176        # moment and is a dimension-less ratio.  Here we use the Pearson's
177        # moment coefficient of kurtosis.  A normal distribution has a
178        # kurtosis of three.  NIST describes a kurtosis over three as
179        # "heavy tailed" and one under three as "light tailed".
180        self.kurtosis = float('nan')
181
182        if not fixes:
183            return
184
185        m3 = 0.0
186        m4 = 0.0
187        if isinstance(fixes[0], tuple):
188            sum_squares = [(x[index] - self.mean) ** 2 for x in fixes]
189            sigma = math.sqrt(sum(sum_squares) / (len(fixes) - 1))
190            for fix in fixes:
191                m3 += pow(fix[index] - sigma, 3)
192                m4 += pow(fix[index] - sigma, 4)
193        else:
194            # must be float
195            sum_squares = [(x - self.mean) ** 2 for x in fixes]
196            sigma = math.sqrt(sum(sum_squares) / (len(fixes) - 1))
197            for fix in fixes:
198                m3 += pow(fix - sigma, 3)
199                m4 += pow(fix - sigma, 4)
200
201        self.sigma = sigma
202        if sigma > 0.0001:
203            self.skewness = m3 / (len(fixes) * pow(sigma, 3))
204            self.kurtosis = m4 / (len(fixes) * pow(sigma, 4))
205
206        return
207
208
209class plotter(object):
210    "Generic class for gathering and plotting sensor statistics."
211
212    def __init__(self):
213        self.device = None
214        self.fixes = []
215        self.in_replot = False
216        self.session = None
217        self.start_time = int(time.time())
218        self.watch = set(['TPV'])
219
220    def whatami(self):
221        "How do we identify this plotting run?"
222        desc = "%s, %s, " % \
223               (gps.misc.isotime(self.start_time),
224                self.device.get('driver', "unknown"))
225        if 'bps' in self.device:
226            desc += "%d %dN%d, cycle %.3gs" % \
227                (self.device['bps'], 9 - self.device['stopbits'],
228                 self.device['stopbits'], self.device['cycle'])
229        else:
230            desc += self.device['path']
231        if 'subtype' in self.device:
232            desc += "\\n%s" % self.device['subtype']
233
234        return desc
235
236    def collect(self, verb, log_fp=None):
237        "Collect data from the GPS."
238
239        try:
240            self.session = gps.gps(host=host, port=port, verbose=verb)
241        except socket.error:
242            sys.stderr.write("gpsprof: gpsd unreachable.\n")
243            sys.exit(1)
244        # Initialize
245        self.session.read()
246        if self.session.version is None:
247            sys.stderr.write("gpsprof: requires gpsd to speak new protocol.\n")
248            sys.exit(1)
249        # Set parameters
250        flags = gps.WATCH_ENABLE | gps.WATCH_JSON
251        if self.requires_time:
252            flags |= gps.WATCH_TIMING
253        if device:
254            flags |= gps.WATCH_DEVICE
255        try:
256            signal.signal(signal.SIGUSR1,
257                          lambda empty, unused: sys.stderr.write(
258                              "%d of %d (%d%%)..."
259                              % (wait - countdown, wait,
260                                 ((wait - countdown) * 100.0 / wait))))
261            signal.siginterrupt(signal.SIGUSR1, False)
262            self.session.stream(flags, device)
263            baton = Baton("gpsprof: %d looking for fix" % os.getpid(), "done")
264            countdown = wait
265            basetime = time.time()
266            while countdown > 0:
267                if self.session.read() == -1:
268                    sys.stderr.write("gpsprof: gpsd has vanished.\n")
269                    sys.exit(1)
270                baton.twirl()
271                if self.session.data["class"] == "ERROR":
272                    sys.stderr.write(" ERROR: %s.\n"
273                                     % self.session.data["message"])
274                    sys.exit(1)
275                if self.session.data["class"] == "DEVICES":
276                    if len(self.session.data["devices"]) != 1 and not device:
277                        sys.stderr.write("ERROR: multiple devices connected, "
278                                         "you must explicitly specify the "
279                                         "device.\n")
280                        sys.exit(1)
281                    for i in range(len(self.session.data["devices"])):
282                        self.device = copy.copy(
283                            self.session.data["devices"][i])
284                        if self.device['path'] == device:
285                            break
286                if self.session.data["class"] == "WATCH":
287                    if ((self.requires_time and
288                         not self.session.data.get("timing"))):
289                        sys.stderr.write("timing is not enabled.\n")
290                        sys.exit(1)
291                # Log before filtering - might be good for post-analysis.
292                if log_fp:
293                    log_fp.write(self.session.response)
294                # Ignore everything but what we're told to
295                if self.session.data["class"] not in self.watch:
296                    continue
297                # We can get some funky artifacts at start of self.session
298                # apparently due to RS232 buffering effects. Ignore
299                # them.
300                if ((threshold and
301                     time.time() - basetime < self.session.cycle * threshold)):
302                    continue
303                if self.session.fix.mode <= gps.MODE_NO_FIX:
304                    continue
305                if self.sample():
306                    if countdown == wait:
307                        sys.stderr.write("first fix in %.2fsec, gathering %d "
308                                         "samples..."
309                                         % (time.time() - basetime, wait))
310                    countdown -= 1
311            baton.end()
312        finally:
313            self.session.stream(gps.WATCH_DISABLE | gps.WATCH_TIMING)
314            signal.signal(signal.SIGUSR1, signal.SIG_DFL)
315
316    def replot(self, infp):
317        "Replot from a JSON log file."
318        self.in_replot = True
319        baton = Baton("gpsprof: replotting", "done")
320        self.session = gps.gps(host=None)
321        for line in infp:
322            baton.twirl()
323            self.session.unpack(line)
324            if self.session.data["class"] == "DEVICES":
325                self.device = copy.copy(self.session.data["devices"][0])
326            elif self.session.data["class"] not in self.watch:
327                continue
328            self.sample()
329        baton.end()
330
331    def dump(self):
332        "Dump the raw data for post-analysis."
333        return self.header() + self.data()
334
335
336class spaceplot(plotter):
337    "Spatial scattergram of fixes."
338    name = "space"
339    requires_time = False
340
341    def __init__(self):
342        "Initialize class spaceplot"
343
344        plotter.__init__(self)
345        self.centroid = None
346        self.centroid_ecef = None
347        self.recentered = []
348
349    def sample(self):
350        "Grab samples"
351
352        # Watch out for the NaN value from gps.py.
353        if (((self.in_replot or self.session.valid) and
354             self.session.data["class"] == "TPV")):
355            # get sat used count
356            sats_used = 0
357            for sat in self.session.satellites:
358                if sat.used:
359                    sats_used += 1
360
361            if 'altHAE' not in self.session.data:
362                self.session.data['altHAE'] = gps.NaN
363
364            self.fixes.append((self.session.data['lat'],
365                               self.session.data['lon'],
366                               self.session.data['altHAE'], sats_used))
367        return True
368
369    def header(self):
370        "Return header"
371        return "\n# Position uncertainty, %s\n" % self.whatami()
372
373    def postprocess(self):
374        "Postprocess the sample data"
375        return
376
377    def data(self):
378        "Format data for dump"
379        res = ""
380        for i in range(len(self.recentered)):
381            (lat, lon) = self.recentered[i][:2]
382            (raw1, raw2, alt) = self.fixes[i]
383            res += "%.9f\t%.9f\t%.9f\t%.9f\t%.9f\n" \
384                % (lat, lon, raw1, raw2, alt)
385        return res
386
387    def plot(self):
388        "Plot the data"
389        stat_lat = stats()
390        stat_lon = stats()
391        stat_alt = stats()
392        stat_used = stats()
393        # recentered stats
394        stat_lat_r = stats()
395        stat_lon_r = stats()
396        stat_alt_r = stats()
397
398        sats_used = []
399        for x in self.fixes:
400            # skip missing sats, if any, often missing at start
401            if x[3] != 0:
402                sats_used.append(x[3])
403
404        # calc sats used data: mean, min, max, sigma
405        stat_used.min_max_mean(sats_used, 0)
406        stat_used.moments(sats_used, 0)
407
408        # find min, max and mean of lat/lon
409        stat_lat.min_max_mean(self.fixes, 0)
410        stat_lon.min_max_mean(self.fixes, 1)
411
412        # centroid is just arithmetic avg of lat,lon
413        self.centroid = (stat_lat.mean, stat_lon.mean)
414
415        # Sort fixes by distance from centroid
416        # sorted to make getting CEP() easy
417        self.fixes.sort(key=lambda p: dist_2d(self.centroid, p[:2]))
418
419        # compute min/max as meters, ignoring altitude
420        # EarthDistance always returns a positve value
421        lat_min_o = -gps.EarthDistance((stat_lat.min, self.centroid[1]),
422                                       self.centroid[:2])
423        lat_max_o = gps.EarthDistance((stat_lat.max, self.centroid[1]),
424                                      self.centroid[:2])
425
426        lon_min_o = -gps.EarthDistance((self.centroid[0], stat_lon.min),
427                                       self.centroid[:2])
428        lon_max_o = gps.EarthDistance((self.centroid[0], stat_lon.max),
429                                      self.centroid[:2])
430
431        # Convert fixes to offsets from centroid in meters
432        self.recentered = [
433            gps.MeterOffset(fix[:2], self.centroid) for fix in self.fixes]
434
435        stat_lat_r.min_max_mean(self.recentered, 0)
436        stat_lon_r.min_max_mean(self.recentered, 1)
437        # compute sigma, skewness and kurtosis of lat/lon
438        stat_lat_r.moments(self.recentered, 0)
439        stat_lon_r.moments(self.recentered, 1)
440
441        # CEP(50) calculated per RCC 261-00, Section 3.1.1
442        calc_cep = 0.5887 * (stat_lat_r.sigma + stat_lon_r.sigma)
443
444        # 2DRMS calculated per RCC 261-00, Section 3.1.4
445        calc_2drms = 2 * math.sqrt(stat_lat_r.sigma ** 2 +
446                                   stat_lon_r.sigma ** 2)
447
448        # Compute measured CEP(50%)
449        # same as median distance from centroid, 50% closer, 50% further
450        cep_meters = gps.misc.EarthDistance(
451            self.centroid[:2], self.fixes[int(len(self.fixes) * 0.50)][:2])
452
453        # Compute measured CEP(95%)
454        # distance from centroid, 95% closer, 5% further
455        cep95_meters = gps.misc.EarthDistance(
456            self.centroid[:2], self.fixes[int(len(self.fixes) * 0.95)][:2])
457
458        # Compute measured CEP(99%)
459        # distance from centroid, 99% closer, 1% further
460        cep99_meters = gps.misc.EarthDistance(
461            self.centroid[:2], self.fixes[int(len(self.fixes) * 0.99)][:2])
462
463        # Compute CEP(100%)
464        # max distance from centroid, 100% closer, 0% further
465        cep100_meters = gps.misc.EarthDistance(
466            self.centroid[:2], self.fixes[len(self.fixes) - 1][:2])
467
468        # init altitude data
469        alt_ep = gps.NaN
470        alt_ep95 = gps.NaN
471        alt_ep99 = gps.NaN
472        dist_3d_max = 0.0
473        alt_fixes = []
474        alt_fixes_r = []
475        latlon_data = ""
476        alt_data = ""
477        # init calcualted hep, sep and mrse
478        calc_hep = gps.NaN
479        calc_sep = gps.NaN
480        calc_mrse = gps.NaN
481
482        # grab and format the fixes as gnuplot will use them
483        for i in range(len(self.recentered)):
484            # grab valid lat/lon data, recentered and raw
485            (lat, lon) = self.recentered[i][:2]
486            alt = self.fixes[i][2]
487            latlon_data += "%.9f\t%.9f\n" % (lat, lon)
488
489            if not math.isnan(alt):
490                # only keep good fixes
491                alt_fixes.append(alt)
492                # micro meters should be good enough
493                alt_data += "%.6f\n" % (alt)
494
495        if alt_fixes:
496            # got altitude data
497
498            # Convert fixes to offsets from avg in meters
499            alt_data_centered = ""
500
501            # find min, max and mean of altitude
502            stat_alt.min_max_mean(alt_fixes, 0)
503            for alt in alt_fixes:
504                alt_fixes_r.append(alt - stat_alt.mean)
505                alt_data_centered += "%.6f\n" % (alt - stat_alt.mean)
506
507            stat_alt_r.min_max_mean(alt_fixes_r, 0)
508            stat_alt_r.moments(alt_fixes_r, 0)
509
510            # centroid in ECEF
511            self.centroid_ecef = wgs84_to_ecef([stat_lat.mean,
512                                                stat_lon.mean,
513                                                stat_alt.mean])
514
515            # once more through the data, looking for 3D max
516            for fix_lla in self.fixes:
517                if not math.isnan(fix_lla[2]):
518                    fix_ecef = wgs84_to_ecef(fix_lla[:3])
519                    dist3d = dist_3d(self.centroid_ecef, fix_ecef)
520                    if dist_3d_max < dist3d:
521                        dist_3d_max = dist3d
522
523            # Sort fixes by distance from average altitude
524            alt_fixes_r.sort(key=lambda a: abs(a))
525            # so we can rank fixes for EPs
526            alt_ep = abs(alt_fixes_r[int(len(alt_fixes_r) * 0.50)])
527            alt_ep95 = abs(alt_fixes_r[int(len(alt_fixes_r) * 0.95)])
528            alt_ep99 = abs(alt_fixes_r[int(len(alt_fixes_r) * 0.99)])
529
530            # HEP(50) calculated per RCC 261-00, Section 3.1.2
531            calc_hep = 0.6745 * stat_alt_r.sigma
532
533            # SEP(50) calculated per RCC 261-00, Section 3.1.3 (3)
534            calc_sep = 0.51 * (stat_lat_r.sigma +
535                               stat_lon_r.sigma +
536                               stat_alt_r.sigma)
537
538            # MRSE calculated per RCC 261-00, Section 3.1.5
539            calc_mrse = math.sqrt(stat_lat_r.sigma ** 2 +
540                                  stat_lon_r.sigma ** 2 +
541                                  stat_alt_r.sigma ** 2)
542
543        fmt_lab11a = ('hep = %.3f meters\\n'
544                      'sep = %.3f meters\\n'
545                      'mrse = %.3f meters\\n'
546                      ) % (calc_hep, calc_sep, calc_mrse)
547
548        if self.centroid[0] < 0.0:
549            latstring = "%.9fS" % -self.centroid[0]
550        elif stat_lat.mean > 0.0:
551            latstring = "%.9fN" % self.centroid[0]
552        else:
553            latstring = "0.0"
554
555        if self.centroid[1] < 0.0:
556            lonstring = "%.9fW" % -self.centroid[1]
557        elif stat_lon.mean > 0.0:
558            lonstring = "%.9fE" % self.centroid[1]
559        else:
560            lonstring = "0.0"
561
562        # oh, this is fun, mixing gnuplot and python string formatting
563        # Grrr, python implements %s max width or precision incorrectly...
564        # and the old and new styles also disagree...
565        fmt = ('set xlabel "Meters east from %s"\n'
566               'set ylabel "Meters north from %s"\n'
567               'cep=%.9f\n'
568               'cep95=%.9f\n'
569               'cep99=%.9f\n'
570               ) % (lonstring, latstring,
571                    cep_meters, cep95_meters, cep99_meters)
572        fmt += ('set autoscale\n'
573                'set multiplot\n'
574                # plot to use 95% of width
575                # set x and y scales to same distance
576                'set size ratio -1 0.95,0.7\n'
577                # leave room at bottom for computed variables
578                'set origin 0.025,0.30\n'
579                'set format x "%.3f"\n'
580                'set format y "%.3f"\n'
581                'set key left at screen 0.6,0.30 vertical\n'
582                'set key noautotitle\n'
583                'set style line 2 pt 1\n'
584                'set style line 3 pt 2\n'
585                'set style line 5 pt 7 ps 1\n'
586                'set xtic rotate by -45\n'
587                'set border 15\n'
588                # now the CEP stuff
589                'set parametric\n'
590                'set trange [0:2*pi]\n'
591                'cx(t, r) = sin(t)*r\n'
592                'cy(t, r) = cos(t)*r\n'
593                'chlen = cep/20\n'
594                # what do the next two lines do??
595                'set arrow from -chlen,0 to chlen,0 nohead\n'
596                'set arrow from 0,-chlen to 0,chlen nohead\n')
597
598        fmt += ('set label 11 at screen 0.01, screen 0.30 '
599                '"RCC 261-00\\n'
600                'cep = %.3f meters\\n'
601                '2drms = %.3f meters\\n%s'
602                '2d max = %.3f meters\\n'
603                '3d max = %.3f meters"\n'
604                ) % (calc_cep, calc_2drms, fmt_lab11a, cep100_meters,
605                     dist_3d_max)
606
607        # row labels
608        fmt += ('set label 12 at screen 0.01, screen 0.12 '
609                '"RCC 261-00\\n'
610                '\\n'
611                'Lat\\n'
612                'Lon\\n'
613                'AltHAE\\n'
614                'Used"\n')
615
616        # mean
617        fmt += ('set label 13 at screen 0.06, screen 0.12 '
618                '"\\n'
619                '        mean\\n'
620                '%s\\n'
621                '%s\\n'
622                '%s\\n'
623                '%s"\n'
624                ) % ('{:>15}'.format(latstring),
625                     '{:>15}'.format(lonstring),
626                     '{:>15.3f}'.format(stat_alt.mean),
627                     '{:>15.1f}'.format(stat_used.mean))
628
629        fmt += ('set label 14 at screen 0.23, screen 0.12 '
630                '"\\n'
631                '        min        max      sigma              '
632                'skewness kurtosis\\n'
633                '%s %s %s meters %s %s\\n'
634                '%s %s %s meters %s %s\\n'
635                '%s %s %s meters %s %s\\n'
636                '%12d %12d %s sats"\n'
637                ) % ('{:>10.3f}'.format(lat_min_o),
638                     '{:>10.3f}'.format(lat_max_o),
639                     '{:>10.3f}'.format(stat_lat_r.sigma),
640                     '{:>10.1f}'.format(stat_lat_r.skewness),
641                     '{:>10.1f}'.format(stat_lat_r.kurtosis),
642                     '{:>10.3f}'.format(lon_min_o),
643                     '{:>10.3f}'.format(lon_max_o),
644                     '{:>10.3f}'.format(stat_lon_r.sigma),
645                     '{:>10.1f}'.format(stat_lon_r.skewness),
646                     '{:>10.1f}'.format(stat_lon_r.kurtosis),
647                     '{:>10.3f}'.format(stat_alt_r.min),
648                     '{:>10.3f}'.format(stat_alt_r.max),
649                     '{:>10.3f}'.format(stat_alt_r.sigma),
650                     '{:>10.1f}'.format(stat_alt_r.skewness),
651                     '{:>10.1f}'.format(stat_alt_r.kurtosis),
652                     stat_used.min,
653                     stat_used.max,
654                     '{:>10.1f}'.format(stat_used.sigma))
655
656        if debug:
657            fmt += ('set label 15 at screen 0.6, screen 0.12 '
658                    '"\\n'
659                    '      min\\n'
660                    '%s\\n'
661                    '%s\\n'
662                    '%s"\n'
663                    ) % ('{:>15.9f}'.format(stat_lat_r.min),
664                         '{:>15.9f}'.format(stat_lon_r.min),
665                         '{:>15.3f}'.format(stat_alt.min))
666
667            fmt += ('set label 16 at screen 0.75, screen 0.12 '
668                    '"\\n'
669                    '      max\\n'
670                    '%s\\n'
671                    '%s\\n'
672                    '%s"\n'
673                    ) % ('{:>15.9f}'.format(stat_lat_r.max),
674                         '{:>15.9f}'.format(stat_lon_r.max),
675                         '{:>15.3f}'.format(stat_alt.max))
676
677        if len(self.fixes) > 1000:
678            plot_style = 'dots'
679        else:
680            plot_style = 'points'
681
682        # got altitude data?
683        if not math.isnan(stat_alt.mean):
684            fmt += ('set ytics nomirror\n'
685                    'set y2tics\n'
686                    'set format y2 "%.3f"\n')
687            fmt += (('set y2label "AltHAE from %.3f meters"\n') %
688                    (stat_alt.mean))
689
690            # add ep(50)s
691
692            altitude_x = cep100_meters * 1.2
693            fmt += ('$EPData << EOD\n'
694                    '%.3f %.3f\n'
695                    '%.3f %.3f\n'
696                    'EOD\n'
697                    ) % (altitude_x, alt_ep,
698                         altitude_x, -alt_ep)
699
700            fmt += ('$EP95Data << EOD\n'
701                    '%.3f %.3f\n'
702                    '%.3f %.3f\n'
703                    'EOD\n'
704                    ) % (altitude_x, alt_ep95,
705                         altitude_x, -alt_ep95)
706
707            fmt += ('$EP99Data << EOD\n'
708                    '%.3f %.3f\n'
709                    '%.3f %.3f\n'
710                    'EOD\n'
711                    ) % (altitude_x, alt_ep99,
712                         altitude_x, -alt_ep99)
713
714        # setup now done, plot it!
715        fmt += ('plot "-" using 1:2 with %s ls 3 title "%d GPS fixes" '
716                ', cx(t,cep),cy(t,cep) ls 1 title "CEP (50%%) = %.3f meters"'
717                ', cx(t,cep95),cy(t,cep95) title "CEP (95%%) = %.3f meters"'
718                ', cx(t,cep99),cy(t,cep99) title "CEP (99%%) = %.3f meters"'
719                ) % (plot_style, len(self.fixes),
720                     cep_meters, cep95_meters, cep99_meters)
721
722        if not math.isnan(stat_alt.mean):
723            # add plot of altitude
724            fmt += (', "-" using ( %.3f ):( $1 - %.3f ) '
725                    'axes x1y2 with points ls 2 lc "green"'
726                    ' title " %d AltHAE fixes"'
727                    ) % (cep100_meters * 1.1, stat_alt.mean, len(alt_fixes))
728
729            # altitude EPs
730            fmt += (', $EPData using 1:2 '
731                    'axes x1y2 with points ls 5 lc "dark-green"'
732                    ' title " EP(50%%) = %.3f meters"'
733                    ) % (alt_ep)
734
735            fmt += (', $EP95Data using 1:2 '
736                    'axes x1y2 with points ls 5 lc "blue"'
737                    ' title " EP(95%%) = %.3f meters"'
738                    ) % (alt_ep95)
739
740            fmt += (', $EP99Data using 1:2 '
741                    'axes x1y2 with points ls 5 lc "red"'
742                    ' title " EP(99%%) = %.3f meters"'
743                    ) % (alt_ep99)
744
745        fmt += self.header() + latlon_data
746        if not math.isnan(stat_alt.mean):
747            # add altitude samples
748            fmt += 'e\n' + alt_data
749        return fmt
750
751
752class polarplot(plotter):
753    "Polar plot of signal strength"
754    name = "polar"
755    requires_time = False
756    seen_used = []          # count of seen and used in each SKY
757
758    def __init__(self):
759        plotter.__init__(self)
760        self.watch = set(['SKY'])
761
762    def sample(self):
763        "Grab samples"
764        if self.session.data["class"] == "SKY":
765            sats = self.session.data['satellites']
766            seen = 0
767            used = 0
768            for sat in sats:
769                seen += 1
770                # u'ss': 42, u'el': 15, u'PRN': 18, u'az': 80, u'used': True
771                if sat['used'] is True:
772                    used += 1
773                    if 'polarunused' == self.name:
774                        continue
775                if (('polarused' == self.name) and (sat['used'] is False)):
776                    continue
777                self.fixes.append((sat['PRN'], sat['ss'], sat['az'],
778                                  sat['el'], sat['used']))
779            self.seen_used.append((seen, used))
780
781        return True
782
783    def header(self):
784        "Return header"
785        return "# Polar plot of signal strengths, %s\n" % self.whatami()
786
787    def postprocess(self):
788        "Postprocess the sample data"
789        return
790
791    def data(self):
792        "Format data for dump"
793        res = ""
794        for (prn, ss, az, el, used) in self.fixes:
795            res += "%d\t%d\t%d\t%d\t%s\n" % (prn, ss, az, el, used)
796        return res
797
798    def plot(self):
799        "Format data for dump"
800
801        # calc SNR: mean, min, max, sigma
802        stat_ss = stats()
803        stat_ss.min_max_mean(self.fixes, 1)
804        stat_ss.moments(self.fixes, 1)
805
806        # calc sats seen data: mean, min, max, sigma
807        stat_seen = stats()
808        stat_seen.min_max_mean(self.seen_used, 0)
809        stat_seen.moments(self.seen_used, 0)
810
811        # calc sats used data: mean, min, max, sigma
812        stat_used = stats()
813        stat_used.min_max_mean(self.seen_used, 1)
814        stat_used.moments(self.seen_used, 1)
815
816        fmt = '''\
817unset border
818set polar
819set angles degrees # set gnuplot on degrees instead of radians
820
821set style line 10 lt 1 lc 0 lw 0.3 #redefine a new line style for the grid
822
823set grid polar 45 #set the grid to be displayed every 45 degrees
824set grid ls 10
825
826# x is angle, go from 0 to 360 degrees
827# y is radius, go from 90 at middle to 0 at edge
828set xrange [0:360]
829set rrange [90:0]  # 90 at center
830set yrange [-90:90]
831
832# set xtics axis #display the xtics on the axis instead of on the border
833# set ytics axis
834set xtics axis nomirror; set ytics axis nomirror
835
836# "remove" the tics so that only the y tics are displayed
837set xtics scale 0
838# set the xtics only go from 0 to 90 with increment of 30
839# but do not display anything. This has to be done otherwise the grid
840# will not be displayed correctly.
841set xtics ("" 90, "" 60, "" 30,)
842
843# make the ytics go from the center (0) to 360 with incrment of 90
844# set ytics 0, 45, 360
845set ytics scale 0
846
847# set the ytics only go from 0 to 90 with increment of 30
848# but do not display anything. This has to be done otherwise the grid
849# will not be displayed correctly.
850set ytics ("" 90, "" 60, "" 30,)
851
852set size square
853
854set key lmargin
855
856# this places a compass label on the outside
857set_label(x, text) = sprintf("set label '%s' at (93*cos(%f)), (93*sin(%f)) center", text, x, x)
858
859# here all labels are created
860# we compute North (0) at top, East (90) at right
861# bug gnuplot puts 0 at right, 90 at top
862eval set_label(0, "E")
863eval set_label(90, "N")
864eval set_label(180, "W")
865eval set_label(270, "S")
866
867set style line 11 pt 2 ps 2 #set the line style for the plot
868
869set style fill transparent solid 0.8 noborder
870
871# set rmargin then put colorbox in the margin.
872set lmargin at screen .08
873set rmargin at screen .85
874set cbrange [10:60]
875set palette defined (100 "blue", 200 "green", 300 "red")
876set colorbox user origin .92, .15 size .03, .6
877set label 10 at screen 0.89, screen 0.13 "SNR dBHz"
878'''
879
880        count = len(self.fixes)
881        fmt += '''\
882set label 11 at screen 0.01, screen 0.15 "%s plot, samples %d"
883set label 12 at screen 0.01, screen 0.10 "\\nSS\\nSeen\\nUsed"
884''' % (self.name, count)
885
886        fmt += '''\
887set label 13 at screen 0.11, screen 0.10 "min\\n%d\\n%d\\n%d" right
888''' % (stat_ss.min, stat_seen.min, stat_used.min)
889
890        fmt += '''\
891set label 14 at screen 0.21, screen 0.10 "max\\n%d\\n%d\\n%d" right
892''' % (stat_ss.max, stat_seen.max, stat_used.max)
893
894        fmt += '''\
895set label 15 at screen 0.31, screen 0.10 "mean\\n%.1f\\n%.1f\\n%.1f" right
896''' % (stat_ss.mean, stat_seen.mean, stat_used.mean)
897
898        fmt += '''\
899set label 16 at screen 0.41, screen 0.10 "sigma\\n%.1f\\n%.1f\\n%.1f" right
900''' % (stat_ss.sigma, stat_seen.sigma, stat_used.sigma)
901
902        fmt += '''\
903# and finally the plot
904# flip azimuth to plot north up, east right
905# plot "-" u (90 - $3):4 t "Sat" with points ls 11
906plot "-" u (90 - $3):4:(1):($2) t "Sat" w circles lc palette
907'''
908        # return fmt + self.header() + self.data()
909        return self.header() + fmt + self.data()
910
911
912class polarplotunused(polarplot):
913    "Polar plot of unused sats signal strength"
914    name = "polarunused"
915
916
917class polarplotused(polarplot):
918    "Polar plot of used sats signal strength"
919    name = "polarused"
920
921
922class timeplot(plotter):
923    "Time drift against PPS."
924    name = "time"
925    requires_time = True
926
927    def __init__(self):
928        plotter.__init__(self)
929        self.watch = set(['PPS'])
930
931    def sample(self):
932        "Grab samples"
933        if self.session.data["class"] == "PPS":
934            self.fixes.append((self.session.data['real_sec'],
935                               self.session.data['real_nsec'],
936                               self.session.data['clock_sec'],
937                               self.session.data['clock_nsec']))
938        return True
939
940    def header(self):
941        "Return header"
942        return "# Time drift against PPS, %s\n" % self.whatami()
943
944    def postprocess(self):
945        "Postprocess the sample data"
946        return
947
948    def data(self):
949        "Format data for dump"
950        res = ""
951        for (real_sec, real_nsec, clock_sec, clock_nsec) in self.fixes:
952            res += "%d\t%d\t%d\t%d\n" % (real_sec, real_nsec, clock_sec,
953                                         clock_nsec)
954        return res
955
956    def plot(self):
957        "Format data for dump"
958        fmt = '''\
959set autoscale
960set key below
961set ylabel "System clock delta from GPS time (nsec)"
962plot "-" using 0:((column(1)-column(3))*1e9 + (column(2)-column(4))) \
963 title "Delta" with impulses
964'''
965        return fmt + self.header() + self.data()
966
967
968class uninstrumented(plotter):
969    "Total times without instrumentation."
970    name = "uninstrumented"
971    requires_time = False
972
973    def __init__(self):
974        plotter.__init__(self)
975
976    def sample(self):
977        "Grab samples"
978        if self.session.fix.time:
979            seconds = time.time() - gps.misc.isotime(self.session.data.time)
980            self.fixes.append(seconds)
981            return True
982
983        return False
984
985    def header(self):
986        "Return header"
987        return "# Uninstrumented total latency, " + self.whatami() + "\n"
988
989    def postprocess(self):
990        "Postprocess the sample data"
991        return
992
993    def data(self):
994        "Format data for dump"
995        res = ""
996        for seconds in self.fixes:
997            res += "%2.6lf\n" % seconds
998        return res
999
1000    def plot(self):
1001        "Plot the data"
1002
1003        fmt = '''\
1004set autoscale
1005set key below
1006set key title "Uninstrumented total latency"
1007plot "-" using 0:1 title "Total time" with impulses
1008'''
1009        return fmt + self.header() + self.data()
1010
1011
1012class instrumented(plotter):
1013    "Latency as analyzed by instrumentation."
1014    name = "instrumented"
1015    requires_time = True
1016
1017    def __init__(self):
1018        "Initialize class instrumented()"
1019
1020        plotter.__init__(self)
1021
1022    def sample(self):
1023        "Grab the samples"
1024
1025        if 'rtime' in self.session.data:
1026            self.fixes.append((gps.misc.isotime(self.session.data['time']),
1027                               self.session.data["chars"],
1028                               self.session.data['sats'],
1029                               self.session.data['sor'],
1030                               self.session.data['rtime'],
1031                               time.time()))
1032            return True
1033
1034        return False
1035
1036    def header(self):
1037        "Return the header"
1038        res = "# Analyzed latency, " + self.whatami() + "\n"
1039        res += "#-- Fix time --  - Chars -  --   Latency  - RS232-  " \
1040               "Analysis  - Recv -\n"
1041        return res
1042
1043    def postprocess(self):
1044        "Postprocess the sample data"
1045        return
1046
1047    def data(self):
1048        "Format data for dump"
1049        res = ""
1050        for (fix_time, chars, sats, start, xmit, recv) in self.fixes:
1051            rs232_time = (chars * 10.0) / self.device['bps']
1052            res += "%.3f  %9u  %2u  %.6f  %.6f  %.6f  %.6f\n" \
1053                % (fix_time, chars, sats, start - fix_time,
1054                   (start - fix_time) + rs232_time, xmit - fix_time,
1055                   recv - fix_time)
1056        return res
1057
1058    def plot(self):
1059        "Do the plot"
1060        legends = (
1061            "Reception delta",
1062            "Analysis time",
1063            "RS232 time",
1064            "Fix latency",
1065        )
1066        fmt = '''\
1067set autoscale
1068set key title "Analyzed latency"
1069set key below
1070plot \\\n'''
1071        for (i, legend) in enumerate(legends):
1072            j = len(legends) - i + 4
1073            fmt += '    "-" using 0:%d title "%s" with impulses, \\\n' \
1074                % (j, legend)
1075        fmt = fmt[:-4] + "\n"
1076        return fmt + self.header() + (self.data() + "e\n") * len(legends)
1077
1078
1079formatters = (polarplot, polarplotunused, polarplotused, spaceplot, timeplot,
1080              uninstrumented, instrumented)
1081
1082
1083def usage():
1084    "Print help, then exit"
1085    sys.stderr.write('''\
1086usage: gpsprof [OPTION]... [server[:port[:device]]]
1087    [-D debuglevel]
1088    [-d dumpfile]
1089    [-f {%s}]
1090    [-h]
1091    [-l logfile]
1092    [-m threshold]
1093    [-n samplecount]
1094    [-r]
1095    [-s speed]
1096    [-S subtitle]
1097    [-T terminal]
1098    [-t title]
1099    [-V]
1100
1101''' % ("|".join([x.name for x in formatters])))
1102    sys.exit(0)
1103
1104
1105if __name__ == '__main__':
1106    try:
1107        (options, arguments) = getopt.getopt(sys.argv[1:],
1108                                             "d:f:hl:m:n:rs:t:D:S:T:V")
1109
1110        plotmode = "space"
1111        raw = False
1112        title = None
1113        subtitle = None
1114        threshold = 0
1115        wait = 100
1116        verbose = 0
1117        terminal = None
1118        dumpfile = None
1119        logfp = None
1120        redo = False
1121        for (switch, val) in options:
1122            if switch == '-d':
1123                dumpfile = val
1124            elif switch == '-D':
1125                # set debug level, 0 off, 1 medium, 2 high
1126                verbose = int(val)
1127            elif switch == '-f':
1128                plotmode = val
1129            elif switch == '-h':
1130                # print help, then exit
1131                usage()
1132
1133            elif switch == '-l':
1134                logfp = open(val, "w")
1135            elif switch == '-m':
1136                threshold = int(val)
1137            elif switch == '-n':
1138                if val[-1] == 'h':
1139                    wait = int(val[:-1]) * 360
1140                else:
1141                    wait = int(val)
1142            elif switch == '-r':
1143                redo = True
1144            elif switch == '-t':
1145                # replace title
1146                title = val
1147            elif switch == '-S':
1148                # add sub title
1149                subtitle = val
1150            elif switch == '-T':
1151                terminal = val
1152            elif switch == '-V':
1153                sys.stderr.write("gpsprof: Version %s\n" % gps_version)
1154                sys.exit(0)
1155
1156        (host, port, device) = ("localhost", "2947", None)
1157        if arguments:
1158            args = arguments[0].split(":")
1159            if len(args) >= 1:
1160                host = args[0]
1161            if len(args) >= 2:
1162                port = args[1]
1163            if len(args) >= 3:
1164                device = args[2]
1165
1166        # Select the plotting mode
1167        if plotmode:
1168            for formatter in formatters:
1169                if formatter.name == plotmode:
1170                    plot = formatter()
1171                    break
1172            else:
1173                sys.stderr.write("gpsprof: no such formatter.\n")
1174                sys.exit(1)
1175        # Get fix data from the GPS
1176        if redo:
1177            plot.replot(sys.stdin)
1178        else:
1179            plot.collect(verbose, logfp)
1180        plot.postprocess()
1181        # Save the timing data (only) for post-analysis if required.
1182        if dumpfile:
1183            with open(dumpfile, "w") as fp:
1184                fp.write(plot.dump())
1185        if logfp:
1186            logfp.close()
1187        # Ship the plot to standard output
1188        if not title:
1189            title = plot.whatami()
1190            # escape " for gnuplot
1191            title = title.replace('"', '\\"')
1192        if subtitle:
1193            title += '\\n' + subtitle
1194        if terminal:
1195            sys.stdout.write("set terminal %s truecolor enhanced size"
1196                             " 800,950\n"
1197                             % terminal)
1198        # double quotes on title so \n is parsed by gnuplot
1199        sys.stdout.write('set title noenhanced "%s\\n\\n"\n' % title)
1200        sys.stdout.write(plot.plot())
1201    except getopt.GetoptError as error:
1202        print(error)
1203        print("")
1204        usage()
1205        exit(1)
1206    except KeyboardInterrupt:
1207        pass
1208
1209# The following sets edit modes for GNU EMACS
1210# Local Variables:
1211# mode:python
1212# End:
1213