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