You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

1965 lines
65KB

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