1#!/usr/local/bin/python3.8 2# -*- coding: UTF-8 3''' 4zerk -- GREIS configurator and packet decoder 5 6usage: zerk [OPTIONS] [server[:port[:device]]] 7''' 8 9# This program conforms to the JAVAD document: 10# GNSS Receiver External Interface Specification 11# Revised: October 11, 2017 12# 13# Hereafter referred to as "the specification" 14# 15# This file is Copyright (c) 2018 by the GPSD project 16# BSD terms apply: see the file COPYING in the distribution root for details. 17# 18# This code runs compatibly under Python 2 and 3.x for x >= 2. 19# Preserve this property! 20# 21# ENVIRONMENT: 22# Options in the ZERKOPTS environment variable will be parsed before 23# the CLI options. A handy place to put your '-f /dev/ttyXX -s SPEED' 24# 25# example usages: 26# Coldboot the GPS: zerk -p COLDBOOT 27# Print current serial port: zerk -c "print,/cur/term" 28# Decode raw log file: zerk -r -f greis-binary.log -v 2 29# Change GPS port speed: zerk -S 230400 30# Watch entire reset cycle: zerk -p RESET -v 2 -w 20 -W 31# poll SVs Status: zerk -W -w 2 -v 2 -c "out,,jps/{CS,ES,GS,Is,WS,QS}" 32# dump local gpsd data zerk -v 2 -w 5 localhost 33# 34# TODO: no CRC16 packets handled yet 35# TODO: more packet decodes 36 37from __future__ import absolute_import, print_function, division 38 39import binascii # for binascii.hexlify() 40import getopt # for getopt.getopt(), to parse CLI options 41import hashlib # for hashlib.sha1 42import os # for os.environ 43import socket # for socket.error 44import stat # for stat.S_ISBLK() 45import struct # for pack() 46import sys 47import time 48import xml.etree.ElementTree # to parse .jpo files 49 50PROG_NAME = 'zerk' 51 52try: 53 import serial 54except ImportError: 55 serial = None # Defer complaining until we know we need it. 56 57try: 58 import gps 59 import gps.misc # for polybyte() polystr() 60except ImportError: 61 # PEP8 says local imports last 62 sys.stderr.write("%s: failed to import gps, check PYTHON_PATH\n" % 63 PROG_NAME) 64 sys.exit(2) 65 66gps_version = '3.20' 67if gps.__version__ != gps_version: 68 sys.stderr.write("%s: ERROR: need gps module version %s, got %s\n" % 69 (PROG_NAME, gps_version, gps.__version__)) 70 sys.exit(1) 71 72 73VERB_QUIET = 0 # quiet 74VERB_NONE = 1 # just output requested data and some info 75VERB_DECODE = 2 # decode all messages 76VERB_INFO = 3 # more info 77VERB_RAW = 4 # raw info 78VERB_PROG = 5 # program trace 79 80# dictionary to hold all user options 81opts = { 82 # command to send to GPS, -c 83 'command': None, 84 # command for -d disable 85 'disable': None, 86 # command for -e enable 87 'enable': None, 88 # default input -f file 89 'input_file_name': None, 90 # default forced wait? -W 91 'input_forced_wait': False, 92 # default port speed -s 93 'input_speed': 115200, 94 # default input wait time -w in seconds 95 'input_wait': 2.0, 96 # the name of an OAF file, extension .jpo 97 'oaf_name': None, 98 # poll command -p 99 'poll': None, 100 # raw log file name 101 'raw_file': None, 102 # open port read only -r 103 'read_only': False, 104 # speed to set GPS -S 105 'set_speed': None, 106 # target gpsd (server:port:device) to connect to 107 'target': {"server": None, "port": gps.GPSD_PORT, "device": None}, 108 # verbosity level, -v 109 'verbosity': VERB_NONE, 110 # contents of environment variable ZERKOPTS 111 'progopts': '', 112} 113 114 115class greis(object): 116 """A class for working with the GREIS GPS message formats 117 118 This class contains functions to decode messages in the Javad GREIS 119 "Receiver Input Language" and "Receiver Messages" formats. 120 """ 121 122 # when a statement identifier is received, it is stored here 123 last_statement_identifier = None 124 # expected statement identifier. 125 expect_statement_identifier = False 126 # ID of current message as a string 127 s_id = '' 128 129 def __init__(self): 130 "Initialize class" 131 132 self.last_statement_identifier = None 133 self.expect_statement_identifier = False 134 # last epoch received in [~~] 135 # epoch == None means never got epoch, epoch == -1 means missing. 136 self.epoch = None 137 138 def f4_s(self, f): 139 "convert an '! f4' to a string" 140 141 if gps.isfinite(f): 142 # yeah, the precision is a guess 143 return "%.6f" % f 144 return 'X' 145 146 def f8_s(self, f): 147 "convert an '! f8' to a string" 148 149 if gps.isfinite(f): 150 # yeah, the precision is a guess 151 return "%.4f" % f 152 return 'X' 153 154 def i1_s(self, i): 155 "convert an '! i1' to a string" 156 return 'X' if i == 127 else str(i) 157 158 def i2_s(self, i): 159 "convert an '! i2' to a string" 160 return 'X' if i == 32767 else str(i) 161 162 def i4_s(self, i): 163 "convert an '! i4' to a string" 164 return 'X' if i == 2147483647 else str(i) 165 166 def u1_s(self, u): 167 "convert an '! u1' to a string" 168 return 'X' if u == 255 else str(u) 169 170 def u2_s(self, u): 171 "convert an '! u2' to a string" 172 return 'X' if u == 65535 else str(u) 173 174 def u4_s(self, u): 175 "convert an '! u4' to a string" 176 return 'X' if u == 4294967295 else str(u) 177 178 def isuchex(self, c): 179 "Is byte an upper case hex char?" 180 if 48 <= c <= 57: 181 # 0 to 9 182 return int(c) - 48 183 if 65 <= c <= 70: 184 # A to F 185 return int(c) - 55 186 return -1 187 188 soltypes = {0: "None", 189 1: "3D", 190 2: "DGPS", 191 3: "RTK float", 192 4: "RTK fixed", 193 5: "fixed" 194 } 195 196 # allowable speeds 197 speeds = (460800, 230400, 153600, 115200, 57600, 38400, 19200, 9600, 198 4800, 2400, 1200, 600, 300) 199 200 def msg_c_(self, payload): 201 "[c?] decode, Smoothing Corrections" 202 203 s = ' smooth' 204 for i in range(0, len(payload) - 1, 2): 205 u = struct.unpack_from('<h', payload, i) 206 s += " " + self.i2_s(u[0]) 207 208 return s + '\n' 209 210 def msg__p(self, payload): 211 "[?p] decode, Integer Relative Carrier Phases" 212 213 s = ' rcp' 214 for i in range(0, len(payload) - 1, 4): 215 u = struct.unpack_from('<l', payload, i) 216 s += " " + self.i4_s(u[0]) 217 218 return s + '\n' 219 220 def msg__d(self, payload): 221 "[?d] decode, Relative Doppler" 222 223 s = ' srdp' 224 for i in range(0, len(payload) - 1, 2): 225 u = struct.unpack_from('<h', payload, i) 226 s += " " + self.i2_s(u[0]) 227 228 return s + '\n' 229 230 def msg__r(self, payload): 231 "[?r] decode, Integer Relative Pseudo-ranges" 232 233 s = ' srdp' 234 for i in range(0, len(payload) - 1, 2): 235 u = struct.unpack_from('<h', payload, i) 236 s += " " + self.i2_s(u[0]) 237 238 return s + '\n' 239 240 def msg__A(self, payload): 241 "[?A] decode, GPS, GALILEO Almanac" 242 m_len = len(payload) 243 244 if ('[EA]' == self.s_id) and (49 > m_len): 245 return " Bad Length %s" % m_len 246 247 u = struct.unpack_from('<BhlBBBfffffffff', payload, 0) 248 249 s = (" sv %u wna %d toa %d healthA %u healthS %u config %u\n" 250 " af1 %f af0 %f rootA %f ecc %f m0 %f\n" 251 " omega0 %f argPer %f delf %f omegaDot %f\n" % u) 252 253 if '[EA]' == self.s_id: 254 u = struct.unpack_from('<H', payload, 46) 255 s += (" iod %d" % (u[0])) 256 return s 257 258 def msg__E(self, payload): 259 "[?E] decode, SNR x 4" 260 261 s = ' cnrX4' 262 for i in range(0, len(payload) - 1, 1): 263 u = struct.unpack_from('<B', payload, i) 264 s += " " + self.u1_s(u[0]) 265 266 return s + '\n' 267 268 def msg_WE(self, payload): 269 "[WE] decode, SBAS Ephemeris" 270 271 u = struct.unpack_from('<BBBBLdddffffffffLHB', payload, 0) 272 s = (" waasPrn %u gpsPrn %u iod %u acc %u tod %u\n" 273 " xg %f yg %f zg %f\n" 274 " vxg %f vyg %f vzg %f\n" 275 " vvxg %f vvyg %f vvzg %f\n" 276 " agf0 %f agf1 %f tow %u wn %u flags %u\n" % u) 277 278 return s 279 280 def msg_r(self, payload): 281 "[r?] decode, Integer Psudeo Ranges" 282 283 s = ' spr' 284 for i in range(0, len(payload) - 1, 4): 285 u = struct.unpack_from('<l', payload, i) 286 s += " " + self.i4_s(u[0]) 287 288 return s + '\n' 289 290 def msg_AZ(self, payload): 291 "[AZ] decode, Satellite Azimuths" 292 293 s = " azim" 294 for i in range(0, len(payload) - 1): 295 # azimuth/2, 0 to 180 degrees 296 s += " " + self.u1_s(payload[i]) 297 298 return s + '\n' 299 300 def msg_BP(self, payload): 301 "[BP] decode" 302 303 u = struct.unpack_from('<f', payload, 0) 304 return " acc %.3e\n" % u[0] 305 306 def msg_D_(self, payload): 307 """[D?] decode, Doppler""" 308 309 s = " dp" 310 for i in range(0, len(payload) - 1, 4): 311 # This is dopple in Hz * 1e4 312 u = struct.unpack_from('<l', payload, i) 313 s += " " + self.i4_s(u[0]) 314 315 return s + '\n' 316 317 def msg_DO(self, payload): 318 "[DO] decode" 319 320 u = struct.unpack_from('<ff', payload, 0) 321 return " val %.3f sval %.3f\n" % u 322 323 def msg_DP(self, payload): 324 "[DP] decode" 325 326 u = struct.unpack_from('<fffBfB', payload, 0) 327 return (" hdop %f vdop %f tdop %f edop %f\n" 328 " solType %s\n" % 329 (u[0], u[1], u[2], u[4], self.soltypes[u[3]])) 330 331 def msg_E_(self, payload): 332 "[E?] decode, SNR" 333 334 s = ' cnr' 335 for i in range(0, len(payload) - 1): 336 s += " " + self.u1_s(payload[i]) 337 338 return s + '\n' 339 340 def msg_ET(self, payload): 341 "[::](ET) decode, Epoch time, end of epoch" 342 343 u = struct.unpack_from('<L', payload, 0) 344 if ((self.epoch is not None and self.epoch != u[0])): 345 if -1 == self.epoch: 346 print("Error: [::](ET) missing [~~](RT)\n") 347 else: 348 print("Error: [::](ET) Wrong Epoch %u, should be %u\n" % 349 (u[0], self.epoch)) 350 # reset epoch 351 self.epoch = -1 352 return "(ET) tod %u\n" % u[0] 353 354 def msg_EL(self, payload): 355 "[EL] decode, Satellite Elevations" 356 357 s = " elev" 358 for i in range(0, len(payload) - 1): 359 # looking for integer (-90 to 90), not byte 360 u = struct.unpack_from('<b', payload, i) 361 s += " " + self.i1_s(u[0]) 362 363 return s + '\n' 364 365 def msg_ER(self, payload): 366 "[ER] decode, Error messages" 367 368 parts = payload.split(b'%') 369 if 1 < len(parts): 370 self.last_statement_identifier = parts[1] 371 372 s_payload = "".join(map(chr, payload)) 373 print("[ER] %s\n" % s_payload) 374 return " %s\n" % s_payload 375 376 def msg_EU(self, payload): 377 "[EU] decode, GALILEO UTC and GPS Time Parameters" 378 379 u = struct.unpack_from('<dfLHbBHbffLHH', payload, 0) 380 return (" ao %f a1 %f tot %u wnt %u dtls %d dn %u wnlsf %u\n" 381 " dtlsf %d a0g %f a1g %f t0g %u wn0g %u flags %#x\n" % u) 382 383 def msg_FC(self, payload): 384 "[FC] [F1] [F2] [F3] [f5] [Fl] decode, Signal Lock Loop Flags" 385 386 s = " flags 0x" 387 for i in range(0, len(payload) - 1): 388 u = struct.unpack_from('<H', payload, i) 389 s += " %2x" % (u[0]) 390 391 return s + '\n' 392 393 def msg__E1(self, payload): 394 "[?E] decode, BeiDos, GPS, GALILEO, IRNSS Ephemeris " 395 m_len = len(payload) 396 # [GE] 397 if ('[IE]' == self.s_id) and (124 > m_len): 398 return " Bad Length %s" % m_len 399 if ('[CN]' == self.s_id) and (132 > m_len): 400 return " Bad Length %s" % m_len 401 if ('[EN]' == self.s_id) and (145 > m_len): 402 return " Bad Length %s" % m_len 403 404 u = struct.unpack_from('<BLBhlbBhfffflhddddddfffffffff', payload, 0) 405 s = (" sv %u tow %u flags %u iodc %d toc %d ura %d healthS %u\n" 406 " wn %d tgd %f af2 %f af1 %f af0 %f toe %d\n" 407 " iode %d rootA %f ecc %f m0 %f omega0 %f\n" 408 " inc0 %f argPer %f deln %f omegaDot %f\n" 409 " incDot %f crc %f crs %f cuc %f\n" 410 " cus %f cic %f cis %f\n" % u) 411 412 if '[EN]' == self.s_id: 413 u = struct.unpack_from('<fffffBB', payload, 122) 414 s += (" bgdE1E5a %f bgdE1E5b %f aio %f ai1 %f ai2 %f\n" 415 " sfi %u navType %u" % u) 416 if 149 <= m_len: 417 # DAf0 added in 3.7.0 418 u = struct.unpack_from('<f', payload, 144) 419 s += (" DAf0 %f" % u) 420 s += '\n' 421 422 if ('[IE]' == self.s_id) and (124 > m_len): 423 u = struct.unpack_from('<B', payload, 122) 424 s += (" navType %u\n" % u[0]) 425 426 if ('[CN]' == self.s_id) and (132 > m_len): 427 u = struct.unpack_from('<fBf', payload, 122) 428 s += (" tgd2 %f navType %u DAf0 %f\n" % u) 429 430 # TODO: decode length 160 168 431 432 return s 433 434 def msg_GT(self, payload): 435 "[GT] decode, GPS Time " 436 437 u = struct.unpack_from('<LH', payload, 0) 438 return " tow %u wn %d\n" % u 439 440 def msg_ID(self, payload): 441 "[ID] Ionosphere Delays" 442 443 s = ' delay' 444 for i in range(0, len(payload) - 1, 4): 445 u = struct.unpack_from('<f', payload, i) 446 s += " %s" % self.f4_s(u[0]) 447 448 return s + '\n' 449 450 def msg_IO(self, payload): 451 "[IO] decode, GPS Ionospheric Parameters" 452 453 u = struct.unpack_from('<LHffffffff', payload, 0) 454 455 return (" tot %d wn %u alpha0 %f alpha1 %f alpha2 %f\n" 456 " alpha3 %f beta0 %u beta1 %d beta2 %f\n" 457 " beta3 %f\n" % u) 458 459 def msg_LO(self, payload): 460 "[LO] decode, undocumented message" 461 462 return " Undocumented message\n" 463 464 def msg_MF(self, payload): 465 "[MF] Messages Format" 466 467 u = struct.unpack_from('<BBBBBBB', payload, 0) 468 return (" id %c%c majorVer %c%c minorVer %c%c order %c\n" % 469 (chr(u[0]), chr(u[1]), chr(u[2]), chr(u[3]), 470 chr(u[4]), chr(u[5]), chr(u[6]))) 471 472 def msg_P_(self, payload): 473 "[P?] decode, Carrier Phases" 474 475 s = " cp" 476 for i in range(0, len(payload) - 1, 8): 477 # carrier phases in cycles 478 u = struct.unpack_from('<d', payload, i) 479 s += " " + self.f8_s(u[0]) 480 481 return s + '\n' 482 483 def msg_PM(self, payload): 484 "[PM] parameters" 485 486 # PM only seems to work after a coldboot, once 487 # zerk -v 2 -w 20 -c 'out,,jps/{PM}' -W 488 return " %s\n" % payload 489 490 def msg_PV(self, payload): 491 "[PV] decode, Cartesian Position and Velocity" 492 493 u = struct.unpack_from('<dddfffffBB', payload, 0) 494 return (" x %s y %s z %s sigma %s\n" 495 " vx %s vy %s vz %s\n" 496 " vsigma %s soltype %s\n" % 497 (self.f8_s(u[0]), self.f8_s(u[1]), self.f8_s(u[2]), 498 self.f4_s(u[3]), self.f4_s(u[4]), self.f4_s(u[5]), 499 self.f4_s(u[6]), self.f4_s(u[7]), self.soltypes[u[8]])) 500 501 def msg_R_(self, payload): 502 """[R?] decode, Pseudo-ranges""" 503 504 s = " pr" 505 for i in range(0, len(payload) - 1, 8): 506 # psuedo in seconds 507 u = struct.unpack_from('<d', payload, i) 508 s += " %s" % self.f8_s(u[0]) 509 510 return s + '\n' 511 512 def msg_RD(self, payload): 513 "[RD] decode, Receiver Date" 514 515 u = struct.unpack_from('<HBBB', payload, 0) 516 return " year %d month %d day %d base %d\n" % u 517 518 def msg_RE(self, payload): 519 "[RE] decode" 520 521 parts = payload.split(b'%') 522 if 1 < len(parts): 523 # Got a statement identifier (ID), save it? 524 # Multiline statement if payload ends with comma or left brace 525 if payload[-1] not in (ord(','), ord('{')): 526 # yes, this is the end 527 self.last_statement_identifier = parts[1] 528 529 # Get the message body 530 part1 = parts[1].split(b',') 531 532 if 'em' == parts[1]: 533 # Probably no parts[2] 534 print("Enable Messages %s" % parts[2]) 535 return " Enable Messages %s\n" % parts[2] 536 537 if 'id' == parts[1]: 538 print("ID: %s" % parts[2]) 539 return " ID %s\n" % parts[2] 540 541 if 'opts' == part1[0]: 542 if 1 < len(part1): 543 s = "OAF %s: %s" % (part1[1], parts[2]) 544 else: 545 s = " OAF: %s" % (parts[2]) 546 print(s) 547 return " %s\n" % s 548 549 if 'serial' == parts[1]: 550 print("SERIAL: %s" % parts[2]) 551 return " SERIAL %s\n" % parts[2] 552 553 if 'vendor' == parts[1]: 554 print("VENDOR: %s" % parts[2]) 555 return " Vendor %s\n" % parts[2] 556 557 if 'ver' == parts[1]: 558 print("VER: %s" % parts[2]) 559 return " Version %s\n" % parts[2] 560 561 # unknown statement identifier 562 s_payload = "".join(map(chr, payload)) 563 print("RE: %s\n" % s_payload) 564 565 return " %s\n" % s_payload 566 567 def msg_RT(self, payload): 568 "[~~](RT) decode, Receiver Time, start of epoch" 569 570 if self.epoch is not None and -1 != self.epoch: 571 print("Error: [~~](RT) missing [::](ET)\n") 572 573 u = struct.unpack_from('<L', payload, 0) 574 # save start of epoch 575 self.epoch = u[0] 576 return "(RT) tod %u\n" % self.epoch 577 578 def msg_S_(self, payload): 579 "[CS], [ES], [GS], [Is], [WS], [NS], [QS], decode, SVs Status" 580 581 # to poll them all: zerk -W -w 2 -v 2 -c "out,,jps/{CS,ES,GS,Is,WS,QS}" 582 # TODO, check @checksum 583 584 return "%s" % payload 585 586 def msg_SE(self, payload): 587 "[SE] decode" 588 589 u = struct.unpack_from('<BBBBB', payload, 0) 590 return " data 0x %x %x %x %x %x\n" % u 591 592 def msg_SG(self, payload): 593 "[SG] decode" 594 595 u = struct.unpack_from('<ffffBB', payload, 0) 596 return (" hpos %s vpos %s hvel %s vvel %s\n" 597 " soltype %s\n" % 598 (self.f4_s(u[0]), self.f4_s(u[1]), self.f4_s(u[2]), 599 self.f4_s(u[3]), self.soltypes[u[4]])) 600 601 def msg_SI(self, payload): 602 "[SI] decode, Satellite Index, deprecated by Javad, use [SX]" 603 604 # [SX] require 3.7 firmware, we use [SI] to support 3.6 605 s = " usi" 606 for i in range(0, len(payload) - 1): 607 s += " %d" % payload[i] 608 609 return s + '\n' 610 611 def msg_SP(self, payload): 612 "[SP] decode, Position Covariance Matrix" 613 614 u = struct.unpack_from('<ffffffffffB', payload, 0) 615 return (" xx % f yy % f zz % f tt % f xy % f\n" 616 " xz % f xt % f yz % f yt % f zt % f\n" 617 " solType %s\n" % 618 (u[0], u[1], u[2], u[3], u[4], 619 u[5], u[6], u[7], u[8], u[9], 620 self.soltypes[u[10]])) 621 622 def msg_SS(self, payload): 623 "[SS] decode, Satellite Navigation Status" 624 625 s = " ns" 626 for i in range(0, len(payload) - 2): 627 s += " %d" % payload[i] 628 629 return (s + '\n solType %s\n' % 630 self.soltypes[payload[len(payload) - 2]]) 631 632 def msg_ST(self, payload): 633 "[ST] decode, Solution Time Tag" 634 635 u = struct.unpack_from('<LBB', payload, 0) 636 return (" time %u ms, soltype %s\n" % 637 (u[0], self.soltypes[u[1]])) 638 639 def msg_SX(self, payload): 640 "[SX] decode, Extended Satellite Indices" 641 642 # [SX] require 3.7 firmware 643 s = " ESI" 644 for i in range(0, len(payload) - 2, 2): 645 u = struct.unpack_from('<BB', payload, i) 646 s += " (%u, %u)" % u 647 648 return s + '\n' 649 650 def msg_TC(self, payload): 651 "[TC] decode, CA/L1 Continous Tracking Time" 652 653 s = " tt" 654 for i in range(0, len(payload) - 1, 2): 655 u = struct.unpack_from('<H', payload, i) 656 s += " %.2f" % u[0] 657 658 return s + '\n' 659 660 def msg_TO(self, payload): 661 "[TO] decode, Reference Time to Receiver Time Offset" 662 663 u = struct.unpack_from('<dd', payload, 0) 664 return " val %.3f sval %.3f\n" % u 665 666 def msg_UO(self, payload): 667 "[UO] decode, GPS UTC Time Parameters" 668 669 u = struct.unpack_from('<dfLHbBHb', payload, 0) 670 return (" a0 %f a1 %f tot %d wnt %d dtls %d\n" 671 " dn %d wnlsf %d dtlsf %d\n" % u) 672 673 def msg_WA(self, payload): 674 "[WA] decode" 675 676 u = struct.unpack_from('<BBBBLdddfffLH', payload, 0) 677 return (" waasPrn %d gpsPrn %d if %d healthS %d tod %d\n" 678 " ECEF %.3f %.3f %.3f, %.3f %.3f %.3f\n" 679 " tow %d wn %d\n" % u) 680 681 def msg_WU(self, payload): 682 "[WU] decode, SBAS UTC Time Parameters" 683 684 u = struct.unpack_from('<dfLHbBHbfbLHB', payload, 0) 685 return (" ao %f a1 %f tot %u wnt %u dtls %d dn %u\n" 686 "wnlsf %u dtlsf %d utcsi %d tow %u wn %u flags %#x\n" % u) 687 688 # table from message id to respective message decoder. 689 # Note: id (%id%) is different than ID (statement identifier) 690 # the id is the first two characters of a GREIS receiver Message 691 # see section 3.3 of the specification 692 messages = { 693 '[0d]': (msg__d, 1), 694 '[1d]': (msg__d, 1), 695 '[1E]': (msg__E, 1), 696 '[1p]': (msg__p, 1), 697 '[1r]': (msg__r, 1), 698 '[2d]': (msg__d, 1), 699 '[2E]': (msg__E, 1), 700 '[2p]': (msg__p, 1), 701 '[2r]': (msg__r, 1), 702 '[3d]': (msg__d, 1), 703 '[3E]': (msg__E, 1), 704 '[3p]': (msg__p, 1), 705 '[3r]': (msg__r, 1), 706 '[5d]': (msg__d, 1), 707 '[5E]': (msg__E, 1), 708 '[5p]': (msg__p, 1), 709 '[5r]': (msg__r, 1), 710 '[AZ]': (msg_AZ, 1), 711 '[BP]': (msg_BP, 5), 712 '[c1]': (msg_c_, 1), 713 '[c2]': (msg_c_, 1), 714 '[c3]': (msg_c_, 1), 715 '[c5]': (msg_c_, 1), 716 '[CA]': (msg__A, 47), 717 '[cc]': (msg_c_, 1), 718 '[CE]': (msg__E, 1), 719 '[cl]': (msg_c_, 1), 720 '[CN]': (msg__E1, 123), 721 '[cp]': (msg__p, 1), 722 '[cr]': (msg__r, 1), 723 '[CS]': (msg_S_, 8), 724 '[D1]': (msg_D_, 1), 725 '[D2]': (msg_D_, 1), 726 '[D3]': (msg_D_, 1), 727 '[D5]': (msg_D_, 1), 728 '[DC]': (msg_D_, 1), 729 '[Dl]': (msg_D_, 1), 730 '[DO]': (msg_DO, 6), 731 '[DP]': (msg_DP, 18), 732 '[DX]': (msg_D_, 1), 733 '[E1]': (msg_E_, 1), 734 '[E2]': (msg_E_, 1), 735 '[E3]': (msg_E_, 1), 736 '[E5]': (msg_E_, 1), 737 '[EA]': (msg__A, 47), 738 '[EC]': (msg_E_, 1), 739 '[El]': (msg_E_, 1), 740 '[EL]': (msg_EL, 1), 741 '[EN]': (msg__E1, 123), 742 '[ER]': (msg_ER, 1), 743 '[ES]': (msg_S_, 8), 744 '[EU]': (msg_EU, 40), 745 '[F1]': (msg_FC, 1), 746 '[F2]': (msg_FC, 1), 747 '[F3]': (msg_FC, 1), 748 '[F5]': (msg_FC, 1), 749 '[FA]': (msg_FC, 1), 750 '[FC]': (msg_FC, 1), 751 '[Fl]': (msg_FC, 1), 752 '[GA]': (msg__A, 47), 753 '[GE]': (msg__E1, 123), 754 '[GS]': (msg_S_, 8), 755 '[GT]': (msg_GT, 7), 756 '[IA]': (msg__A, 47), 757 '[ID]': (msg_ID, 1), 758 '[IE]': (msg__E1, 123), 759 '[IO]': (msg_IO, 39), 760 '[Is]': (msg_S_, 8), 761 '[ld]': (msg__d, 1), 762 '[lE]': (msg__E, 1), 763 '[LO]': (msg_LO, 1), 764 '[lp]': (msg__p, 1), 765 '[lr]': (msg__r, 1), 766 '[MF]': (msg_MF, 9), 767 '[::]': (msg_ET, 4), 768 '[~~]': (msg_RT, 4), 769 '[NS]': (msg_S_, 8), 770 '[P1]': (msg_P_, 1), 771 '[P2]': (msg_P_, 1), 772 '[P3]': (msg_P_, 1), 773 '[P5]': (msg_P_, 1), 774 '[PC]': (msg_P_, 1), 775 '[Pl]': (msg_P_, 1), 776 '[PM]': (msg_PM, 0), 777 '[PV]': (msg_PV, 46), 778 '[QA]': (msg__A, 47), 779 '[QE]': (msg__E1, 123), 780 '[QS]': (msg_S_, 8), 781 '[r1]': (msg_r, 1), 782 '[R1]': (msg_R_, 1), 783 '[r2]': (msg_r, 1), 784 '[R2]': (msg_R_, 1), 785 '[r3]': (msg_r, 1), 786 '[R3]': (msg_R_, 1), 787 '[r5]': (msg_r, 1), 788 '[R5]': (msg_R_, 1), 789 '[rc]': (msg_r, 1), 790 '[RC]': (msg_R_, 1), 791 '[RD]': (msg_RD, 6), 792 '[RE]': (msg_RE, 1), 793 '[rl]': (msg_r, 1), 794 '[Rl]': (msg_R_, 1), 795 '[rx]': (msg_r, 1), 796 '[SE]': (msg_SE, 6), 797 '[SG]': (msg_SG, 18), 798 '[SI]': (msg_SI, 1), 799 '[SP]': (msg_SP, 42), 800 '[SS]': (msg_SS, 1), 801 '[ST]': (msg_ST, 6), 802 '[SX]': (msg_SX, 1), 803 '[TC]': (msg_TC, 1), 804 '[TO]': (msg_TO, 6), 805 '[UO]': (msg_UO, 24), 806 '[WA]': (msg_WA, 51), 807 '[WE]': (msg_WE, 73), 808 '[WS]': (msg_S_, 8), 809 '[WU]': (msg_WU, 40), 810 } 811 812 def decode_msg(self, out): 813 "Decode one message and then return number of chars consumed" 814 815 state = 'BASE' 816 consumed = 0 817 # raw message, sometimes used for checksum calc 818 m_raw = bytearray(0) 819 820 # decode state machine 821 for this_byte in out: 822 consumed += 1 823 if isinstance(this_byte, str): 824 # a character, probably read from a file 825 c = ord(this_byte) 826 else: 827 # a byte, probably read from a serial port 828 c = int(this_byte) 829 830 if VERB_RAW <= opts['verbosity']: 831 if ord(' ') <= c <= ord('~'): 832 # c is printable 833 print("state: %s char %c (%#x)" % (state, chr(c), c)) 834 else: 835 # c is not printable 836 print("state: %s char %#x" % (state, c)) 837 838 m_raw.extend([c]) 839 840 # parse input stream per GREIS Ref Guide Section 3.3.3 841 if 'BASE' == state: 842 # start fresh 843 # place to store 'comments' 844 comment = '' 845 # message id byte one 846 m_id1 = 0 847 # message id byte two 848 m_id2 = 0 849 # message length as integer 850 m_len = 0 851 # byte array to hold payload, including possible checksum 852 m_payload = bytearray(0) 853 m_raw = bytearray(0) 854 m_raw.extend([c]) 855 856 if ord('0') <= c <= ord('~'): 857 # maybe id 1, '0' to '~' 858 state = 'ID1' 859 860 # start the grab 861 m_id1 = c 862 continue 863 864 if ord("%") == c: 865 # start of %ID%, Receiver Input Language 866 # per GREIS Ref Guide Section 2.2 867 state = 'RIL' 868 869 # start fresh 870 comment = "%" 871 continue 872 873 if ord("$") == c: 874 # NMEA line, treat as comment 875 state = 'NMEA' 876 877 # start fresh 878 comment = "$" 879 continue 880 881 if ord("#") == c: 882 # comment line 883 state = 'COMMENT' 884 885 # start fresh 886 comment = "#" 887 continue 888 889 if ord('\n') == c or ord('\r') == c: 890 # stray newline or linefeed, eat it 891 return consumed 892 893 # none of the above, stay in BASE 894 continue 895 896 if state in ('COMMENT', 'JSON', 'RIL'): 897 # inside comment 898 if c in (ord('\n'), ord('\r')): 899 # Got newline or linefeed 900 # GREIS terminates messages on <CR> or <LF> 901 # Done, got a full message 902 if '{"class":"ERROR"' in comment: 903 # always print gpsd errors 904 print(comment) 905 elif VERB_DECODE <= opts['verbosity']: 906 print(comment) 907 return consumed 908 909 # else: 910 comment += chr(c) 911 continue 912 913 if 'ID1' == state: 914 # maybe id 2, '0' to '~' 915 if ord('"') == c: 916 # technically could be GREIS, but likely JSON 917 state = 'JSON' 918 comment += chr(m_id1) + chr(c) 919 elif ord('0') <= c <= ord('~'): 920 state = 'ID2' 921 m_id2 = c 922 else: 923 state = 'BASE' 924 continue 925 926 if 'ID2' == state: 927 # maybe len 1, 'A' to 'F' 928 x = self.isuchex(c) 929 if -1 < x: 930 state = 'LEN1' 931 m_len = x * 256 932 else: 933 state = 'BASE' 934 continue 935 936 if 'LEN1' == state: 937 # maybe len 2, 'A' to 'F' 938 x = self.isuchex(c) 939 if -1 < x: 940 state = 'LEN2' 941 m_len += x * 16 942 else: 943 state = 'BASE' 944 continue 945 946 if 'LEN2' == state: 947 # maybe len 3, 'A' to 'F' 948 x = self.isuchex(c) 949 if -1 < x: 950 state = 'PAYLOAD' 951 m_len += x 952 else: 953 state = 'BASE' 954 continue 955 956 if 'NMEA' == state: 957 # inside NMEA 958 if ord('\n') == c or ord('\r') == c: 959 # Got newline or linefeed 960 # done, got a full message 961 # GREIS terminates messages on <CR> or <LF> 962 if VERB_DECODE <= opts['verbosity']: 963 print(comment) 964 return consumed 965 966 # else: 967 comment += chr(c) 968 continue 969 970 if 'PAYLOAD' == state: 971 # getting payload 972 m_payload.extend([c]) 973 if len(m_payload) < m_len: 974 continue 975 976 # got entire payload 977 self.s_id = "[%c%c]" % (chr(m_id1), chr(m_id2)) 978 if VERB_DECODE <= opts['verbosity']: 979 x_payload = binascii.hexlify(m_payload) 980 981 # [RE], [ER] and more have no 8-bit checksum 982 # assume the rest do 983 if ((self.s_id not in ('[CS]', '[ER]', '[ES]', '[GS]', '[Is]', 984 '[MF]', '[NS]', '[PM]', '[QS]', '[RE]', 985 '[WS]') and 986 not self.checksum_OK(m_raw))): 987 print("ERROR: Bad checksum\n") 988 if VERB_DECODE <= opts['verbosity']: 989 print("DECODE: id: %s len: %d\n" 990 "DECODE: payload: %s\n" % 991 (self.s_id, m_len, x_payload)) 992 # skip it. 993 return consumed 994 995 if self.s_id in self.messages: 996 if VERB_INFO <= opts['verbosity']: 997 print("INFO: id: %s len: %d\n" 998 "INFO: payload: %s\n" % 999 (self.s_id, m_len, x_payload)) 1000 1001 (decode, length) = self.messages[self.s_id] 1002 if m_len < length: 1003 print("DECODE: %s Bad Length %s\n" % 1004 (self.s_id, m_len)) 1005 else: 1006 s = self.s_id + decode(self, m_payload) 1007 if VERB_DECODE <= opts['verbosity']: 1008 print(s) 1009 else: 1010 # unknown message 1011 if VERB_DECODE <= opts['verbosity']: 1012 print("DECODE: Unknown: id: %s len: %d\n" 1013 "DECODE: payload: %s\n" % 1014 (self.s_id, m_len, x_payload)) 1015 return consumed 1016 1017 # give up 1018 state = 'BASE' 1019 1020 # fell out of loop, no more chars to look at 1021 return 0 1022 1023 def checksum_OK(self, raw_msg): 1024 "Check the i8-bit checksum on a message, return True if good" 1025 1026 # some packets from the GPS use CRC16, some i8-bit checksum, some none 1027 # only 8-bit checksum done here for now 1028 calc_checksum = self.checksum(raw_msg, len(raw_msg) - 1) 1029 rcode = raw_msg[len(raw_msg) - 1] == calc_checksum 1030 if VERB_RAW <= opts['verbosity']: 1031 print("Checksum was %#x, calculated %#x" % 1032 (raw_msg[len(raw_msg) - 1], calc_checksum)) 1033 return rcode 1034 1035 def _rol(self, value): 1036 "rotate a byte left 2 bits" 1037 return ((value << 2) | (value >> 6)) & 0x0ff 1038 1039 def checksum(self, msg, m_len): 1040 "Calculate GREIS 8-bit checksum" 1041 1042 # Calculated per section A.1.1 of the specification 1043 # msg may be bytes (incoming messages) or str (outgoing messages) 1044 1045 ck = 0 1046 for c in msg[0:m_len]: 1047 if isinstance(c, str): 1048 # a string, make a byte 1049 c = ord(c) 1050 ck = self._rol(ck) ^ c 1051 1052 return self._rol(ck) & 0x0ff 1053 1054 def make_pkt(self, m_data): 1055 "Build an output message, always ASCII, add checksum and terminator" 1056 1057 # build core message 1058 1059 # no leading spaces, checksum includes the @ 1060 m_data = m_data.lstrip() + b'@' 1061 1062 chk = self.checksum(m_data, len(m_data)) 1063 1064 # all commands end with CR and/or LF 1065 return m_data + (b'%02X' % chk) + b'\n' 1066 1067 def gps_send(self, m_data): 1068 "Send message to GREIS GPS" 1069 1070 m_all = self.make_pkt(m_data) 1071 if not opts['read_only']: 1072 io_handle.ser.write(m_all) 1073 if VERB_INFO <= opts['verbosity']: 1074 print("sent:", m_all) 1075 self.decode_msg(m_all) 1076 sys.stdout.flush() 1077 1078 # Table of known options. From table 4-2 of the specification. 1079 oafs = ( 1080 b"_AJM", 1081 b"AUTH", 1082 b"_BLT", 1083 b"_CAN", 1084 b"CDIF", 1085 b"CMRI", 1086 b"CMRO", 1087 b"COMP", 1088 b"COOP", 1089 b"COPN", 1090 b"CORI", 1091 b"_CPH", 1092 b"DEVS", 1093 b"DIST", 1094 b"_DTM", 1095 b"_E5B", 1096 b"_E6_", 1097 b"EDEV", 1098 b"ETHR", 1099 b"EVNT", 1100 b"_FRI", 1101 b"_FRO", 1102 b"_FTP", 1103 b"_GAL", 1104 b"GBAI", 1105 b"GBAO", 1106 b"GCLB", 1107 b"_GEN", 1108 b"_GEO", 1109 b"_GLO", 1110 b"_GPS", 1111 b"_GSM", 1112 b"HTTP", 1113 b"_IMU", 1114 b"INFR", 1115 b"IRIG", 1116 b"IRNS", 1117 b"JPSI", 1118 b"JPSO", 1119 b"_L1C", 1120 b"_L1_", 1121 b"_L2C", 1122 b"_L2_", 1123 b"_L5_", 1124 b"LAT1", 1125 b"LAT2", 1126 b"LAT3", 1127 b"LAT4", 1128 b"LCS2", 1129 b"L_CS", 1130 b"_LIM", 1131 b"LON1", 1132 b"LON2", 1133 b"LON3", 1134 b"LON4", 1135 b"MAGN", 1136 b"_MEM", 1137 b"_MPR", 1138 b"OCTO", 1139 b"OMNI", 1140 b"_PAR", 1141 b"PDIF", 1142 b"_POS", 1143 b"_PPP", 1144 b"_PPS", 1145 b"PRTT", 1146 b"_PTP", 1147 b"QZSS", 1148 b"RAIM", 1149 b"_RAW", 1150 b"RCVT", 1151 b"RM3I", 1152 b"RM3O", 1153 b"RS_A", 1154 b"RS_B", 1155 b"RS_C", 1156 b"RS_D", 1157 b"RTMI", 1158 b"RTMO", 1159 b"SPEC", 1160 b"TCCL", 1161 b"_TCP", 1162 b"TCPO", 1163 b"_TLS", 1164 b"TRST", 1165 b"UDPO", 1166 b"_UHF", 1167 b"_USB", 1168 b"WAAS", 1169 b"WIFI", 1170 b"_WPT", 1171 ) 1172 1173 def send_able_4hz(self, able): 1174 "enable basic GREIS messages at 4Hz" 1175 1176 self.expect_statement_identifier = 'greis' 1177 1178 # the messages we want 1179 # [SX] requires 3.7 firmware, we use [SI] to support 3.6 1180 messages = b"jps/{RT,UO,GT,PV,SG,DP,SI,EL,AZ,EC,SS,ET}" 1181 1182 if able: 1183 # Message rate must be an integer multiple of /par/raw/msint 1184 # Default msint is 0.100 seconds, so that must be changed first 1185 self.gps_send(b"%msint%set,/par/raw/msint,250") 1186 1187 self.gps_send(b"%greis%em,," + messages + b":0.25") 1188 else: 1189 self.gps_send(b"%greis%dm,," + messages) 1190 1191 def send_able_comp(self, able): 1192 "dis/enable COMPASS, aka BeiDou" 1193 self.expect_statement_identifier = 'cons' 1194 en_dis = b'y' if 1 == able else b'n' 1195 self.gps_send(b"%cons%set,/par/pos/sys/comp," + en_dis) 1196 1197 def send_able_constellations(self, able): 1198 "dis/enable all constellations" 1199 self.expect_statement_identifier = 'cons7' 1200 en_dis = b'y' if 1 == able else b'n' 1201 self.gps_send(b"%cons1%set,/par/pos/sys/comp," + en_dis) 1202 self.gps_send(b"%cons2%set,/par/pos/sys/gal," + en_dis) 1203 # this will fail on TR-G2H, as it has no GLONASS 1204 self.gps_send(b"%cons3%set,/par/pos/sys/glo," + en_dis) 1205 self.gps_send(b"%cons4%set,/par/pos/sys/gps," + en_dis) 1206 self.gps_send(b"%cons5%set,/par/pos/sys/irnss," + en_dis) 1207 self.gps_send(b"%cons6%set,/par/pos/sys/sbas," + en_dis) 1208 self.gps_send(b"%cons7%set,/par/pos/sys/qzss," + en_dis) 1209 1210 def send_able_defmsg(self, able): 1211 "dis/enable default messages at 1Hz" 1212 self.expect_statement_identifier = 'defmsg' 1213 if able: 1214 self.gps_send(b"%defmsg%em,,jps/RT,/msg/def:1,jps/ET") 1215 else: 1216 # leave RT and ET to break less? 1217 self.gps_send(b"%defmsg%dm,,/msg/def:1") 1218 1219 def send_able_gal(self, able): 1220 "dis/enable GALILEO" 1221 self.expect_statement_identifier = 'cons' 1222 en_dis = b'y' if 1 == able else b'n' 1223 self.gps_send(b"%cons%set,/par/pos/sys/gal," + en_dis) 1224 1225 def send_able_glo(self, able): 1226 "dis/enable GLONASS" 1227 self.expect_statement_identifier = 'cons' 1228 en_dis = b'y' if 1 == able else b'n' 1229 self.gps_send(b"%cons%set,/par/pos/sys/glo," + en_dis) 1230 1231 def send_able_gps(self, able): 1232 "dis/enable GPS" 1233 self.expect_statement_identifier = 'cons' 1234 en_dis = b'y' if 1 == able else b'n' 1235 self.gps_send(b"%cons%set,/par/pos/sys/gps," + en_dis) 1236 1237 def send_able_ipr(self, able): 1238 "dis/enable all Integer Psuedo-Range messages" 1239 self.expect_statement_identifier = 'em' 1240 if able: 1241 self.gps_send(b"%em%em,,jps/{rx,rc,r1,r2,r3,r5,rl}:0.25") 1242 else: 1243 self.gps_send(b"%em%dm,,jps/{rx,rc,r1,r2,r3,r5,rl}") 1244 1245 def send_able_irnss(self, able): 1246 "dis/enable IRNSS" 1247 self.expect_statement_identifier = 'cons' 1248 en_dis = b'y' if 1 == able else b'n' 1249 self.gps_send(b"%cons%set,/par/pos/sys/irnss," + en_dis) 1250 1251 def send_able_nmea41(self, able): 1252 "dis/enable basic NMEA 4.1e messages at 4Hz" 1253 1254 self.expect_statement_identifier = 'nmea' 1255 1256 messages = b"nmea/{GBS,GGA,GSA,GST,GSV,RMC,VTG,ZDA}" 1257 1258 if able: 1259 # set NMEA version 4.1e 1260 self.gps_send(b"%nmeaver%set,/par/nmea/ver,v4.1e") 1261 1262 # Message rate must be an integer multiple of /par/raw/msint 1263 # Default msint is 0.100 seconds, so that must be changed first 1264 self.gps_send(b"%msint%set,/par/raw/msint,250") 1265 1266 # now we can set the messages we want 1267 self.gps_send(b"%nmea%em,," + messages + b":0.25") 1268 else: 1269 # disable 1270 self.gps_send(b"%nmea%dm,," + messages) 1271 1272 def send_able_raw(self, able): 1273 """dis/enable Raw mode messages""" 1274 self.expect_statement_identifier = 'raw' 1275 1276 messages = (b"jps/{RT,UO,GT,PV,SG,DP,SI,EL,AZ,EC,SS," 1277 b"PC,P1,P2,P3,P5,Pl," 1278 b"RC,R1,R2,R3,R5,Rl," 1279 b"DC,D1,D2,D3,D5,Dl," 1280 b"ET}") 1281 1282 if able: 1283 self.gps_send(b"%raw%em,," + messages + b":1") 1284 else: 1285 self.gps_send(b"%raw%dm,," + messages) 1286 1287 def send_able_sbas(self, able): 1288 "dis/enable SBAS" 1289 self.expect_statement_identifier = 'cons' 1290 en_dis = b'y' if 1 == able else b'n' 1291 self.gps_send(b"%cons%set,/par/pos/sys/sbas," + en_dis) 1292 1293 def send_able_qzss(self, able): 1294 "dis/enable QZSS" 1295 self.expect_statement_identifier = 'cons' 1296 en_dis = b'y' if 1 == able else b'n' 1297 self.gps_send(b"%cons%set,/par/pos/sys/qzss," + en_dis) 1298 1299 def send_able_snr(self, able): 1300 "dis/enable all SNR messages, except [EC]" 1301 self.expect_statement_identifier = 'em' 1302 if able: 1303 self.gps_send(b"%em%em,,jps/{E1,E2,E3,E5,El}:0.25") 1304 else: 1305 self.gps_send(b"%em%dm,,jps/{E1,E2,E3,E5,El}") 1306 1307 able_commands = { 1308 # en/disable basic GREIS messages at 4HZ 1309 "4HZ": {"command": send_able_4hz, 1310 "help": "basic GREIS messages at 4Hz"}, 1311 # en/disable all constellations 1312 "CONS": {"command": send_able_constellations, 1313 "help": "all constellations"}, 1314 # en/disable COMPASS, aka Beidou 1315 "COMPASS": {"command": send_able_comp, 1316 "help": "COMPASS"}, 1317 # en/disable default message set. 1318 "DEFMSG": {"command": send_able_defmsg, 1319 "help": "default message set at 1Hz"}, 1320 # en/disable GALILEO 1321 "GALILEO": {"command": send_able_gal, 1322 "help": "GALILEO"}, 1323 # en/disable GLONASS 1324 "GLONASS": {"command": send_able_glo, 1325 "help": "GLONASS"}, 1326 # en/disable GPS 1327 "GPS": {"command": send_able_gps, 1328 "help": "GPS"}, 1329 # en/disable Integer Psuedo Range messages 1330 "IPR": {"command": send_able_ipr, 1331 "help": "all Integer Psuedo Range messages"}, 1332 # en/disable IRNSS 1333 "IRNSS": {"command": send_able_irnss, 1334 "help": "IRNSS"}, 1335 # en/disable NMEA 4.1e 1336 "NMEA": {"command": send_able_nmea41, 1337 "help": "basic messages NMEA 4.1 at 4Hz"}, 1338 # en/disable Psuedo Range, Carrier Phase and Doppler messages 1339 "RAW": {"command": send_able_raw, 1340 "help": "Raw mode messages"}, 1341 # en/disable SBAS 1342 "SBAS": {"command": send_able_sbas, 1343 "help": "SBAS"}, 1344 # en/disable all SNRs 1345 "SNR": {"command": send_able_snr, 1346 "help": "all SNR messages, except [EC]"}, 1347 # en/disable QZSS 1348 "QZSS": {"command": send_able_qzss, 1349 "help": "QZSS"}, 1350 } 1351 1352 def send_coldboot(self): 1353 "Delete NVRAM (almanac, ephemeris, location) and restart" 1354 self.expect_statement_identifier = 'coldboot' 1355 self.gps_send(b"%coldboot%init,/dev/nvm/a") 1356 1357 def send_constellations(self): 1358 "poll all constellations" 1359 self.expect_statement_identifier = 'cons' 1360 self.gps_send(b"%cons%print,/par/pos/sys:on") 1361 1362 def send_get_id(self): 1363 "get receiver id" 1364 self.expect_statement_identifier = 'id' 1365 self.gps_send(b"%id%print,/par/rcv/id") 1366 1367 def send_get_oaf(self): 1368 "poll OAF (GPS opts)" 1369 1370 self.expect_statement_identifier = 'opts,_WPT' 1371 if VERB_RAW <= opts['verbosity']: 1372 # get list of all opts 1373 self.gps_send(b"%opts,list%list,/par/opts") 1374 1375 # request opts one at a time from canned list 1376 for s in self.oafs: 1377 self.gps_send(b"%%opts,%s%%print,/par/opts/%s" % (s, s)) 1378 1379 def send_get_serial(self): 1380 "get receiver serial number" 1381 self.expect_statement_identifier = 'serial' 1382 self.gps_send(b"%serial%print,/par/rcv/sn") 1383 1384 def send_reset(self): 1385 "reset (reboot) the GPS" 1386 self.expect_statement_identifier = 'reset' 1387 self.gps_send(b"%reset%set,/par/reset,y") 1388 1389 def send_set_dm(self): 1390 "disable all messages" 1391 self.expect_statement_identifier = 'dm' 1392 self.gps_send(b"%dm%dm") 1393 1394 def send_set_ipr(self): 1395 "poll Integer Psuedo-Range messages" 1396 self.expect_statement_identifier = 'out' 1397 self.gps_send(b"%out%out,,jps/{rx,rc,r1,r2,r3,r5,rl}") 1398 1399 def send_get_snr(self): 1400 "poll all SNR messages" 1401 # nothing we can wait on, depending on GPS model/configuration 1402 # we may never see some of E2, E3, E5 or El 1403 self.gps_send(b"%out%out,,jps/{EC,E1,E2,E3,E5,El}") 1404 1405 def send_set_speed(self, set_speed): 1406 "change GPS speed" 1407 self.expect_statement_identifier = 'setspeed' 1408 self.gps_send(b"%%setspeed%%set,/par/cur/term/rate,%d" % 1409 set_speed) 1410 1411 def send_get_vendor(self): 1412 "get receiver vendor" 1413 self.expect_statement_identifier = 'vendor' 1414 self.gps_send(b"%vendor%print,/par/rcv/vendor") 1415 1416 def send_get_ver(self): 1417 "get receiver version, per section 4.4.3 of the specification" 1418 self.expect_statement_identifier = 'ver' 1419 self.gps_send(b"%ver%print,/par/rcv/ver") 1420 1421 # list of canned commands that can be sent to the receiver 1422 commands = { 1423 "COLDBOOT": {"command": send_coldboot, 1424 "help": "cold boot the GPS"}, 1425 "CONS": {"command": send_constellations, 1426 "help": "poll enabled constellations"}, 1427 "DM": {"command": send_set_dm, 1428 "help": "disable all periodic messages"}, 1429 "ID": {"command": send_get_id, 1430 "help": "poll receiver ID"}, 1431 "IPR": {"command": send_set_ipr, 1432 "help": "poll all Integer Psuedo-range messages"}, 1433 "OAF": {"command": send_get_oaf, 1434 "help": "poll all OAF options"}, 1435 "RESET": {"command": send_reset, 1436 "help": "reset (reboot) the GPS"}, 1437 "SERIAL": {"command": send_get_serial, 1438 "help": "poll receiver serial number"}, 1439 "SNR": {"command": send_get_snr, 1440 "help": "poll all SNR messages"}, 1441 "VENDOR": {"command": send_get_vendor, 1442 "help": "poll GPS vendor"}, 1443 "VER": {"command": send_get_ver, 1444 "help": "poll GPS version"}, 1445 } 1446 1447 1448class gps_io(object): 1449 """All the GPS I/O in one place" 1450 1451 Three types of GPS I/O 1452 1. read only from a file 1453 2. read/write through a device 1454 3. read only from a gpsd instance 1455 """ 1456 1457 out = b'' 1458 ser = None 1459 input_is_device = False 1460 1461 def __init__(self): 1462 "Initialize class" 1463 1464 Serial = serial 1465 Serial_v3 = Serial and Serial.VERSION.split('.')[0] >= '3' 1466 # buffer to hold read data 1467 self.out = b'' 1468 1469 # open the input: device, file, or gpsd 1470 if opts['input_file_name'] is not None: 1471 # check if input file is a file or device 1472 try: 1473 mode = os.stat(opts['input_file_name']).st_mode 1474 except OSError: 1475 sys.stderr.write('%s: failed to open input file %s\n' % 1476 (PROG_NAME, opts['input_file_name'])) 1477 sys.exit(1) 1478 1479 if stat.S_ISCHR(mode): 1480 # character device, need not be read only 1481 self.input_is_device = True 1482 1483 if ((opts['disable'] or opts['enable'] or opts['poll'] or 1484 opts['oaf_name'])): 1485 1486 # check that we can write 1487 if opts['read_only']: 1488 sys.stderr.write('%s: read-only mode, ' 1489 'can not send commands\n' % PROG_NAME) 1490 sys.exit(1) 1491 if self.input_is_device is False: 1492 sys.stderr.write('%s: input is plain file, ' 1493 'can not send commands\n' % PROG_NAME) 1494 sys.exit(1) 1495 1496 if opts['target']['server'] is not None: 1497 # try to open local gpsd 1498 try: 1499 self.ser = gps.gpscommon(host=None) 1500 self.ser.connect(opts['target']['server'], 1501 opts['target']['port']) 1502 1503 # alias self.ser.write() to self.write_gpsd() 1504 self.ser.write = self.write_gpsd 1505 # ask for raw, not rare, data 1506 data_out = b'?WATCH={' 1507 if opts['target']['device'] is not None: 1508 # add in the requested device 1509 data_out += (b'"device":"' + opts['target']['device'] + 1510 b'",') 1511 data_out += b'"enable":true,"raw":2}\r\n' 1512 if VERB_RAW <= opts['verbosity']: 1513 print("sent: ", data_out) 1514 self.ser.send(data_out) 1515 except socket.error as err: 1516 sys.stderr.write('%s: failed to connect to gpsd %s\n' % 1517 (PROG_NAME, err)) 1518 sys.exit(1) 1519 1520 elif self.input_is_device: 1521 # configure the serial connections (the parameters refer to 1522 # the device you are connecting to) 1523 1524 # pyserial Ver 3.0+ changes writeTimeout to write_timeout 1525 # Using the wrong one causes an error 1526 write_timeout_arg = ('write_timeout' 1527 if Serial_v3 else 'writeTimeout') 1528 try: 1529 self.ser = Serial.Serial( 1530 baudrate=opts['input_speed'], 1531 # 8N1 is GREIS default 1532 bytesize=Serial.EIGHTBITS, 1533 parity=Serial.PARITY_NONE, 1534 port=opts['input_file_name'], 1535 stopbits=Serial.STOPBITS_ONE, 1536 # read timeout 1537 timeout=0.05, 1538 **{write_timeout_arg: 0.5} 1539 ) 1540 except AttributeError: 1541 sys.stderr.write('%s: failed to import pyserial\n' % PROG_NAME) 1542 sys.exit(2) 1543 except Serial.serialutil.SerialException: 1544 # this exception happens on bad serial port device name 1545 sys.stderr.write('%s: failed to open serial port "%s"\n' 1546 ' Your computer has these serial ports:\n' 1547 % (PROG_NAME, opts['input_file_name'])) 1548 1549 # print out list of supported ports 1550 import serial.tools.list_ports as List_Ports 1551 ports = List_Ports.comports() 1552 for port in ports: 1553 sys.stderr.write(" %s: %s\n" % 1554 (port.device, port.description)) 1555 sys.exit(1) 1556 1557 # flush input buffer, discarding all its contents 1558 # pyserial 3.0+ deprecates flushInput() in favor of 1559 # reset_input_buffer(), but flushInput() is still present. 1560 self.ser.flushInput() 1561 1562 else: 1563 # Read from a plain file of GREIS messages 1564 try: 1565 self.ser = open(opts['input_file_name'], 'rb') 1566 except IOError: 1567 sys.stderr.write('%s: failed to open input %s\n' % 1568 (PROG_NAME, opts['input_file_name'])) 1569 sys.exit(1) 1570 1571 def read(self, read_opts): 1572 "Read from device, until timeout or expected message" 1573 1574 # are we expecting a certain message? 1575 if gps_model.expect_statement_identifier: 1576 # assume failure, until we see expected message 1577 ret_code = 1 1578 else: 1579 # not expecting anything, so OK if we did not see it. 1580 ret_code = 0 1581 1582 try: 1583 if read_opts['target']['server'] is not None: 1584 # gpsd input 1585 start = gps.monotonic() 1586 while read_opts['input_wait'] > (gps.monotonic() - start): 1587 # First priority is to be sure the input buffer is read. 1588 # This is to prevent input buffer overuns 1589 if 0 < self.ser.waiting(): 1590 # We have serial input waiting, get it 1591 # No timeout possible 1592 # RTCM3 JSON can be over 4.4k long, so go big 1593 new_out = self.ser.sock.recv(8192) 1594 if raw is not None: 1595 # save to raw file 1596 raw.write(new_out) 1597 self.out += new_out 1598 1599 consumed = gps_model.decode_msg(self.out) 1600 self.out = self.out[consumed:] 1601 if ((gps_model.expect_statement_identifier and 1602 (gps_model.expect_statement_identifier == 1603 gps_model.last_statement_identifier))): 1604 # Got what we were waiting for. Done? 1605 ret_code = 0 1606 if not read_opts['input_forced_wait']: 1607 # Done 1608 break 1609 1610 elif self.input_is_device: 1611 # input is a serial device 1612 start = gps.monotonic() 1613 while read_opts['input_wait'] > (gps.monotonic() - start): 1614 # First priority is to be sure the input buffer is read. 1615 # This is to prevent input buffer overuns 1616 # pyserial 3.0+ deprecates inWaiting() in favor of 1617 # in_waiting, but inWaiting() is still present. 1618 if 0 < self.ser.inWaiting(): 1619 # We have serial input waiting, get it 1620 # 1024 is comfortably large, almost always the 1621 # Read timeout is what causes ser.read() to return 1622 new_out = self.ser.read(1024) 1623 if raw is not None: 1624 # save to raw file 1625 raw.write(new_out) 1626 self.out += new_out 1627 1628 consumed = gps_model.decode_msg(self.out) 1629 self.out = self.out[consumed:] 1630 if ((gps_model.expect_statement_identifier and 1631 (gps_model.expect_statement_identifier == 1632 gps_model.last_statement_identifier))): 1633 # Got what we were waiting for. Done? 1634 ret_code = 0 1635 if not read_opts['input_forced_wait']: 1636 # Done 1637 break 1638 else: 1639 # ordinary file, so all read at once 1640 self.out += self.ser.read() 1641 if raw is not None: 1642 # save to raw file 1643 raw.write(self.out) 1644 1645 while True: 1646 consumed = gps_model.decode_msg(self.out) 1647 self.out = self.out[consumed:] 1648 if 0 >= consumed: 1649 break 1650 1651 except IOError: 1652 # This happens on a good device name, but gpsd already running. 1653 # or if USB device unplugged 1654 sys.stderr.write('%s: failed to read %s\n' 1655 '%s: Is gpsd already holding the port?\n' 1656 % (PROG_NAME, PROG_NAME, 1657 read_opts['input_file_name'])) 1658 return 1 1659 1660 if 0 < ret_code: 1661 # did not see the message we were expecting to see 1662 sys.stderr.write('%s: waited %0.2f seconds for, ' 1663 'but did not get: %%%s%%\n' 1664 % (PROG_NAME, read_opts['input_wait'], 1665 gps_model.expect_statement_identifier)) 1666 return ret_code 1667 1668 def write_gpsd(self, data): 1669 "write data to gpsd daemon" 1670 1671 # HEXDATA_MAX = 512, from gps.h, The max hex digits can write. 1672 # Input data is binary, converting to hex doubles its size. 1673 # Limit binary data to length 255, so hex data length less than 510. 1674 if 255 < len(data): 1675 sys.stderr.write('%s: trying to send %d bytes, max is 255\n' 1676 % (PROG_NAME, len(data))) 1677 return 1 1678 1679 if opts['target']['device'] is not None: 1680 # add in the requested device 1681 data_out = b'?DEVICE={"path":"' + opts['target']['device'] + b'",' 1682 else: 1683 data_out = b'?DEVICE={' 1684 1685 # Convert binary data to hex and build the message. 1686 data_out += b'"hexdata":"' + binascii.hexlify(data) + b'"}\r\n' 1687 if VERB_RAW <= opts['verbosity']: 1688 print("sent: ", data_out) 1689 self.ser.send(data_out) 1690 return 0 1691 1692 1693def usage(): 1694 "Print usage information, and exit" 1695 1696 print("usage: %s [-?hrVW] [-c C] [-d D] [-e E] [-f F] [-O O] [-p P]\n" 1697 " [-R R] [-S S] [-s S] [-v V] [-w W]\n" 1698 " [server[:port[:device]]]\n\n" % PROG_NAME) 1699 print('usage: %s [options]\n' 1700 ' -? print this help\n' 1701 ' -c C send command C to GPS\n' 1702 ' -d D disable D\n' 1703 ' -e E enable E\n' 1704 ' -f F open F as file/device\n' 1705 ' default: %s\n' 1706 ' -h print this help\n' 1707 ' -O O send OAF file to GPS\n' 1708 ' -p P send preset GPS command P\n' 1709 ' -R R save raw data from GPS in file R\n' 1710 ' -r open file/device read only\n' 1711 ' default: %s\n' 1712 ' -S S configure GPS speed to S\n' 1713 ' -s S set port speed to S\n' 1714 ' default: %d bps\n' 1715 ' -V print version\n' 1716 ' -v V Set verbosity level to V, 0 to 4\n' 1717 ' default: %d\n' 1718 ' -W force entire wait time, no exit early\n' 1719 ' -w W wait time, exit early on -p result\n' 1720 ' default: %s seconds\n' 1721 ' [server[:port[:device]]] Connect to gpsd\n' 1722 ' default port: 2947\n' 1723 ' default device: None\n' 1724 '\n' 1725 'D and E can be one of:' % 1726 (PROG_NAME, opts['input_file_name'], opts['raw_file'], 1727 opts['input_speed'], opts['verbosity'], opts['input_wait']) 1728 ) 1729 1730 # print list of enable/disable commands 1731 for item in sorted(gps_model.able_commands.keys()): 1732 print(" %-12s %s" % (item, gps_model.able_commands[item]["help"])) 1733 1734 print('\nthe preset GPS command P can be one of:') 1735 1736 # print list of possible canned commands 1737 for item in sorted(gps_model.commands.keys()): 1738 print(" %-12s %s" % (item, gps_model.commands[item]["help"])) 1739 print('\nOptions can be placed in the ZERKOPTS environment variable.\n' 1740 'ZERKOPTS is processed before the CLI options.') 1741 sys.exit(0) 1742 1743 1744# create the GREIS instance 1745gps_model = greis() 1746 1747if 'ZERKOPTS' in os.environ: 1748 # grab the ZERKOPTS environment variable for options 1749 opts['progopts'] = os.environ['ZERKOPTS'] 1750 options = opts['progopts'].split(' ') + sys.argv[1:] 1751else: 1752 options = sys.argv[1:] 1753 1754try: 1755 (options, arguments) = getopt.getopt(options, 1756 "?c:d:e:f:hrp:s:w:v:O:R:S:WV") 1757except getopt.GetoptError as err: 1758 sys.stderr.write("%s: %s\n" 1759 "Try '%s -h' for more information.\n" % 1760 (PROG_NAME, str(err), PROG_NAME)) 1761 sys.exit(2) 1762 1763for (opt, val) in options: 1764 if opt == '-c': 1765 # command 1766 opts['command'] = val 1767 elif opt == '-d': 1768 # disable 1769 opts['disable'] = val 1770 elif opt == '-e': 1771 # enable 1772 opts['enable'] = val 1773 elif opt == '-f': 1774 # file input 1775 opts['input_file_name'] = val 1776 elif opt in ('-h', '-?'): 1777 # help 1778 usage() 1779 elif opt == '-p': 1780 # preprogrammed command 1781 opts['poll'] = val 1782 elif opt == '-r': 1783 # read only 1784 opts['read_only'] = True 1785 elif opt == '-s': 1786 # serial port speed 1787 opts['input_speed'] = int(val) 1788 if opts['input_speed'] not in gps_model.speeds: 1789 sys.stderr.write('%s: -s invalid speed %s\n' % 1790 (PROG_NAME, opts['input_speed'])) 1791 sys.exit(1) 1792 1793 elif opt == '-w': 1794 # max wait time, seconds 1795 opts['input_wait'] = float(val) 1796 elif opt in '-v': 1797 # verbosity level 1798 opts['verbosity'] = int(val) 1799 elif opt in '-O': 1800 # OAF .jpo file 1801 opts['oaf_name'] = val 1802 elif opt in '-R': 1803 # raw log file 1804 opts['raw_file'] = val 1805 elif opt in '-S': 1806 # set GPS serial port speed 1807 opts['set_speed'] = int(val) 1808 if opts['set_speed'] not in gps_model.speeds: 1809 sys.stderr.write('%s: -S invalid speed %s\n' % 1810 (PROG_NAME, opts['set_speed'])) 1811 sys.exit(1) 1812 1813 elif opt == '-W': 1814 # forced wait, no early exit on command completion 1815 opts['input_forced_wait'] = True 1816 elif opt == '-V': 1817 # version 1818 sys.stderr.write('zerk: Version %s\n' % gps_version) 1819 sys.exit(0) 1820 1821if opts['input_file_name'] is None: 1822 # no input file given 1823 # default to local gpsd 1824 opts['target']['server'] = "localhost" 1825 opts['target']['port'] = gps.GPSD_PORT 1826 opts['target']['device'] = None 1827 if arguments: 1828 # server[:port[:device]] 1829 arg_parts = arguments[0].split(':') 1830 opts['target']['server'] = arg_parts[0] 1831 if 1 < len(arg_parts): 1832 opts['target']['port'] = arg_parts[1] 1833 if 2 < len(arg_parts): 1834 opts['target']['device'] = arg_parts[2] 1835 1836elif arguments: 1837 sys.stderr.write('%s: Both input file and server specified\n' % PROG_NAME) 1838 sys.exit(1) 1839 1840if VERB_PROG <= opts['verbosity']: 1841 # dump all options 1842 print('Options:') 1843 for option in sorted(opts): 1844 print(" %s: %s" % (option, opts[option])) 1845 1846# done parsing arguments from environment and CLI 1847 1848try: 1849 # raw log file requested? 1850 raw = None 1851 if opts['raw_file']: 1852 try: 1853 raw = open(opts['raw_file'], 'w') 1854 except IOError: 1855 sys.stderr.write('%s: failed to open raw file %s\n' % 1856 (PROG_NAME, opts['raw_file'])) 1857 sys.exit(1) 1858 1859 # create the I/O instance 1860 io_handle = gps_io() 1861 1862 # keep it simple, only one of -O, -c -d -e or -S 1863 if opts['oaf_name'] is not None: 1864 # parse an OAF file 1865 try: 1866 oaf_root = xml.etree.ElementTree.parse(opts['oaf_name']).getroot() 1867 oaf = dict() 1868 for tag in ('id', 'oaf', 'hash'): 1869 oaf[tag] = oaf_root.find(tag).text 1870 oaf['oaf'] = oaf['oaf'].split('\n') 1871 if VERB_PROG <= opts['verbosity']: 1872 print(oaf) 1873 except xml.etree.ElementTree.ParseError: 1874 sys.stderr.write('%s: failed to parse OAF "%s"\n' 1875 % (PROG_NAME, opts['oaf_name'])) 1876 sys.exit(1) 1877 except IOError: 1878 sys.stderr.write('%s: failed to read OAF "%s"\n' 1879 % (PROG_NAME, opts['oaf_name'])) 1880 sys.exit(1) 1881 1882 # calculate hash 1883 oaf_s = '\n'.join(oaf['oaf']) 1884 hash_s = hashlib.sha1(oaf_s).hexdigest() 1885 if hash_s != oaf['hash']: 1886 sys.stderr.write('%s: OAF bad hash "%s", s/b %s\n' 1887 % (PROG_NAME, hash_s, oaf['hash'])) 1888 sys.exit(1) 1889 1890 # TODO: probably should check the ID first... 1891 # TODO: prolly should send one command per handshake 1892 # blasting all commands at once, seems to not work reliably 1893 for command in oaf['oaf']: 1894 time.sleep(0.1) # wait 0.1 seconds each 1895 gps_model.gps_send(command) 1896 # this will detect when it is all done 1897 gps_model.gps_send(b'%DONE%') 1898 gps_model.expect_statement_identifier = 'DONE' 1899 1900 elif opts['command'] is not None: 1901 # zero length is OK to send 1902 if 1 < len(opts['command']) and '%' != opts['command'][0]: 1903 # add ID, if missing 1904 gps_model.expect_statement_identifier = 'CMD' 1905 opts['command'] = "%CMD%" + opts['command'] 1906 1907 # add trailing new line 1908 opts['command'] += "\n" 1909 1910 if VERB_QUIET < opts['verbosity']: 1911 sys.stderr.write('%s: command %s\n' % (PROG_NAME, opts['command'])) 1912 gps_model.gps_send(opts['command']) 1913 1914 elif opts['disable'] is not None: 1915 if VERB_QUIET < opts['verbosity']: 1916 sys.stderr.write('%s: disable %s\n' % (PROG_NAME, opts['disable'])) 1917 if opts['disable'] in gps_model.able_commands: 1918 command = gps_model.able_commands[opts['disable']] 1919 command["command"](gps_model, 0) 1920 else: 1921 sys.stderr.write('%s: disable %s not found\n' % 1922 (PROG_NAME, opts['disable'])) 1923 sys.exit(1) 1924 1925 elif opts['enable'] is not None: 1926 if VERB_QUIET < opts['verbosity']: 1927 sys.stderr.write('%s: enable %s\n' % (PROG_NAME, opts['enable'])) 1928 if opts['enable'] in gps_model.able_commands: 1929 command = gps_model.able_commands[opts['enable']] 1930 command["command"](gps_model, 1) 1931 else: 1932 sys.stderr.write('%s: enable %s not found\n' % 1933 (PROG_NAME, opts['enable'])) 1934 sys.exit(1) 1935 1936 elif opts['poll'] is not None: 1937 if VERB_QUIET < opts['verbosity']: 1938 sys.stderr.write('%s: poll %s\n' % (PROG_NAME, opts['poll'])) 1939 if opts['poll'] in gps_model.commands: 1940 command = gps_model.commands[opts['poll']] 1941 command["command"](gps_model) 1942 else: 1943 sys.stderr.write('%s: poll %s not found\n' % 1944 (PROG_NAME, opts['poll'])) 1945 sys.exit(1) 1946 1947 elif opts['set_speed'] is not None: 1948 gps_model.send_set_speed(opts['set_speed']) 1949 1950 exit_code = io_handle.read(opts) 1951 1952 if ((VERB_RAW <= opts['verbosity']) and io_handle.out): 1953 # dump raw left overs 1954 print("Left over data:") 1955 print(io_handle.out) 1956 1957 sys.stdout.flush() 1958 io_handle.ser.close() 1959 1960except KeyboardInterrupt: 1961 print('') 1962 exit_code = 1 1963 1964sys.exit(exit_code) 1965