extcap/ubxdump.py

   1 #!/usr/bin/env python3
   2
   3 # Extcap for u-blox GNSS receiver
   4 # By Timo Warns <timo.warns@gmail.com>
   5 # Copyright 2024 Timo Warns
   6 #
   7 # The extcap is based on Wireshark's extcap_example.py with
   8 # Copyright 2014 Roland Knall <rknall [AT] gmail.com>
   9 #
  10 # Wireshark - Network traffic analyzer
  11 # By Gerald Combs <gerald@wireshark.org>
  12 # Copyright 1998 Gerald Combs
  13 #
  14 # SPDX-License-Identifier: GPL-2.0-or-later
  15 #
  16
  17 """
  18 Extcap for UBX messages from an u-blox GNSS receiver.
  19 Tested with UBX protocol version 18.
  20 """
  21
  22 import argparse, serial.tools.list_ports, serial, struct, sys, time
  23 from threading import Thread
  24
  25 VERSION  = "0.1"
  26
  27
  28 ################################
  29 # u-blox / UBX related constants
  30 ################################
  31
  32 UBLOX_DEV_DESCRIPTION = 'u-blox GNSS receiver'
  33
  34 # UBX message structure-related definitions
  35 UBX_PREAMBLE_1 = 0xb5
  36 UBX_PREAMBLE_2 = 0x62
  37 UBX_HEADER_SIZE = 6
  38 UBX_CHKSUM_SIZE = 2
  39 UBX_PAYLOAD_LEN_OFFSET = 4
  40
  41 # UBX GNSS Identifiers
  42 UBX_GNSS_ID_GPS     = 0
  43 UBX_GNSS_ID_SBAS    = 1
  44 UBX_GNSS_ID_GALILEO = 2
  45 UBX_GNSS_ID_BEIDOU  = 3
  46 UBX_GNSS_ID_IMES    = 4
  47 UBX_GNSS_ID_QZSS    = 5
  48 UBX_GNSS_ID_GLONASS = 6
  49
  50 # UBX message class and identifiers
  51 UBX_NAV         = 0x01
  52 UBX_NAV_POSECEF = [UBX_NAV, 0x01]
  53 UBX_NAV_DOP     = [UBX_NAV, 0x04]
  54 UBX_NAV_PVT     = [UBX_NAV, 0x07]
  55 UBX_NAV_ODO     = [UBX_NAV, 0x09]
  56 UBX_NAV_VELECEF = [UBX_NAV, 0x11]
  57 UBX_NAV_TIMEGPS = [UBX_NAV, 0x20]
  58 UBX_NAV_TIMEUTC = [UBX_NAV, 0x21]
  59 UBX_NAV_TIMELS  = [UBX_NAV, 0x26]
  60 UBX_NAV_SBAS    = [UBX_NAV, 0x32]
  61 UBX_NAV_SAT     = [UBX_NAV, 0x35]
  62 UBX_NAV_EOE     = [UBX_NAV, 0x61]
  63
  64 UBX_RXM       = 0x02
  65 UBX_RXM_SFRBX = [UBX_RXM, 0x13]
  66 UBX_RXM_MEASX = [UBX_RXM, 0x14]
  67 UBX_RXM_RAWX  = [UBX_RXM, 0x15]
  68
  69 UBX_CFG      = 0x06
  70 UBX_CFG_MSG  = [UBX_CFG, 0x01]
  71 UBX_CFG_SBAS = [UBX_CFG, 0x16]
  72 UBX_CFG_GNSS = [UBX_CFG, 0x3e]
  73
  74 UBX_NMEA     = 0xf0
  75 UBX_NMEA_GGA = [UBX_NMEA, 0x00]
  76 UBX_NMEA_GLL = [UBX_NMEA, 0x01]
  77 UBX_NMEA_GSA = [UBX_NMEA, 0x02]
  78 UBX_NMEA_GSV = [UBX_NMEA, 0x03]
  79 UBX_NMEA_RMC = [UBX_NMEA, 0x04]
  80 UBX_NMEA_VTG = [UBX_NMEA, 0x05]
  81 UBX_NMEA_GRS = [UBX_NMEA, 0x06]
  82 UBX_NMEA_GST = [UBX_NMEA, 0x07]
  83 UBX_NMEA_ZDA = [UBX_NMEA, 0x08]
  84 UBX_NMEA_GBS = [UBX_NMEA, 0x09]
  85 UBX_NMEA_TXT = [UBX_NMEA, 0x41]
  86
  87
  88 # Defines the desired rate per UBX message type.
  89 # NMEA messages are disabled by setting their rate to 0.
  90 #
  91 # Eventually, this could be made configurable / controllable via the extcap
  92 # config interface.
  93 UBX_MSG_RATES = [
  94     (UBX_NAV_POSECEF, 0x01),
  95     (UBX_NAV_DOP,     0x01),
  96     (UBX_NAV_PVT,     0x01),
  97     (UBX_NAV_ODO,     0x01),
  98     (UBX_NAV_VELECEF, 0x01),
  99     (UBX_NAV_TIMEGPS, 0x01),
 100     (UBX_NAV_TIMEUTC, 0x01),
 101     (UBX_NAV_TIMELS,  0xff),
 102     (UBX_NAV_SBAS,    0x01),
 103     (UBX_NAV_SAT,     0x01),
 104     (UBX_NAV_EOE,     0x01),
 105     (UBX_RXM_SFRBX,   0x01),
 106     (UBX_RXM_MEASX,   0x01),
 107     (UBX_RXM_RAWX,    0x01),
 108     (UBX_NMEA_GGA,    0x00),
 109     (UBX_NMEA_GLL,    0x00),
 110     (UBX_NMEA_GSA,    0x00),
 111     (UBX_NMEA_GSV,    0x00),
 112     (UBX_NMEA_RMC,    0x00),
 113     (UBX_NMEA_VTG,    0x00),
 114     (UBX_NMEA_GRS,    0x00),
 115     (UBX_NMEA_GST,    0x00),
 116     (UBX_NMEA_ZDA,    0x00),
 117     (UBX_NMEA_GBS,    0x00),
 118     (UBX_NMEA_TXT,    0x00),
 119         ]
 120
 121 # Defines the desired GNSS config.
 122 # Format is (GNSS ID, resTrkCh, maxTrkCh, enable, sigCfgMask).
 123 #
 124 # Eventually, this could be made configurable / controllable via the extcap
 125 # config interface.
 126 UBX_GNSS_CONFIGS = [
 127         (UBX_GNSS_ID_GPS,     8, 14, True,  0x01),
 128         (UBX_GNSS_ID_GLONASS, 0,  0, False, 0x00),
 129         (UBX_GNSS_ID_SBAS,    2,  4, True,  0x01),
 130         (UBX_GNSS_ID_GALILEO, 8, 14, True,  0x01)
 131         ]
 132
 133 ########################
 134 # PCAP-related constants
 135 ########################
 136
 137 DLT      = "147"
 138 DLT_NAME = "DLT_USER0"
 139
 140 PCAP_MAGIC         = 0xa1b2c3d4
 141 PCAP_VERSION_MAJOR = 2
 142 PCAP_VERSION_MINOR = 4
 143 PCAP_THISZONE      = 0
 144 PCAP_SIGFIGS       = 0
 145 PCAP_SNAPLEN       = 0xffffffff
 146
 147 ##########################
 148 # extcap-related constants
 149 ##########################
 150
 151 ERROR_USAGE          = 0
 152 ERROR_ARG            = 1
 153 ERROR_INTERFACE      = 2
 154 ERROR_FIFO           = 3
 155
 156 CTRL_CMD_INITIALIZED   = 0
 157 CTRL_CMD_SET           = 1
 158 CTRL_CMD_ADD           = 2
 159 CTRL_CMD_REMOVE        = 3
 160 CTRL_CMD_ENABLE        = 4
 161 CTRL_CMD_DISABLE       = 5
 162 CTRL_CMD_STATUSBAR_MSG = 6
 163 CTRL_CMD_INFO_MSG      = 7
 164 CTRL_CMD_WARN_MSG      = 8
 165 CTRL_CMD_ERROR_MSG     = 9
 166
 167 CTRL_ARG_LOGGER        = 0
 168
 169 initialized = False
 170 fn_out = None
 171
 172 def extcap_config(option):
 173     # not options implemented for the moment
 174     return
 175
 176 def extcap_version():
 177     print(f"extcap {{version={VERSION}}}{{help=https://www.wireshark.org}}{{display=u-blox UBX extcap interface}}")
 178
 179 def extcap_interfaces():
 180     extcap_version()
 181     for i in serial.tools.list_ports.grep(UBLOX_DEV_DESCRIPTION):
 182         print(f"interface {{value={i.device}}}{{display=u-blox UBX capture}}")
 183
 184     print(f"control {{number={CTRL_ARG_LOGGER}}}{{type=button}}{{role=logger}}{{display=Log}}{{tooltip=Show capture log}}")
 185
 186 def extcap_dlts():
 187     print(f"dlt {{number={DLT}}}{{name={DLT_NAME}}}{{display=UBX DLT ({DLT_NAME})}}")
 188
 189 def log(msg):
 190     control_write(CTRL_ARG_LOGGER, CTRL_CMD_ADD, msg)
 191
 192 def pcap_header():
 193     return struct.pack(
 194             "!IHHiIII",
 195             PCAP_MAGIC,
 196             PCAP_VERSION_MAJOR,
 197             PCAP_VERSION_MINOR,
 198             PCAP_THISZONE,
 199             PCAP_SIGFIGS,
 200             PCAP_SNAPLEN,
 201             int(DLT))
 202
 203 def pcap_packet(ubx_msg):
 204     pcap = bytearray()
 205
 206     caplength = len(ubx_msg)
 207     timestamp = int(time.time())
 208
 209     pcap += struct.pack("!IIII", int(timestamp), 0, caplength, caplength)
 210     pcap += ubx_msg
 211
 212     return pcap
 213
 214 def ubxChecksum(msg):
 215     ck_a = 0
 216     ck_b = 0
 217
 218     for b in msg:
 219         ck_a += b
 220         ck_b += ck_a
 221
 222     return [ck_a & 0xff, ck_b & 0xff]
 223
 224 def ubxMsg(ubxClassId, payload):
 225
 226     payloadLength = len(payload)
 227
 228     msg = bytearray(UBX_HEADER_SIZE + payloadLength + UBX_CHKSUM_SIZE)
 229
 230     # add preamble
 231     msg[0:2] = [UBX_PREAMBLE_1, UBX_PREAMBLE_2]
 232
 233     # add class/id
 234     msg[2:4] = ubxClassId
 235
 236     # add payload length
 237     struct.pack_into('<H', msg, UBX_PAYLOAD_LEN_OFFSET, payloadLength)
 238
 239     # add payload
 240     msg[UBX_HEADER_SIZE:-UBX_CHKSUM_SIZE] = payload
 241
 242     # add checksum
 243     msg[-UBX_CHKSUM_SIZE:] = ubxChecksum(msg[2:-UBX_CHKSUM_SIZE])
 244
 245     return msg
 246
 247 def sendUbxMsg(receiver, msg):
 248     log("Sending UBX message: " + msg.hex() + "\n")
 249     receiver.write(msg)
 250
 251 def ubxCfgMsg(ubxMsgClassId, rate):
 252     return ubxMsg(UBX_CFG_MSG, ubxMsgClassId + [rate])
 253
 254 def ubxCfgGnss(gnssId, resTrkCh, maxTrkCh, enable, sigCfgMask):
 255
 256     msgVer          = 0x00
 257     numTrkChHw      = 0x00 # read only
 258     numTrkChUse     = 0xff
 259     numConfigBlocks = 0x01 # one config block only
 260
 261     payload = bytearray(12)
 262
 263     payload[0]  = msgVer
 264     payload[1]  = numTrkChHw
 265     payload[2]  = numTrkChUse
 266     payload[3]  = numConfigBlocks
 267
 268     payload[4]  = gnssId
 269     payload[5]  = resTrkCh
 270     payload[6]  = maxTrkCh
 271     payload[7]  = 0 # reserved1
 272     payload[8]  = 1 if enable else 0
 273     payload[9]  = 0 # flags, reserved
 274     payload[10] = sigCfgMask
 275     payload[11] = 0 # flags, reserved
 276
 277     return ubxMsg(UBX_CFG_GNSS, payload)
 278
 279
 280 def control_read(fn):
 281     try:
 282         header = fn.read(6)
 283         sp, _, length, arg, typ = struct.unpack('>sBHBB', header)
 284         if length > 2:
 285             payload = fn.read(length - 2).decode('utf-8', 'replace')
 286         else:
 287             payload = ''
 288         return arg, typ, payload
 289     except Exception:
 290         return None, None, None
 291
 292 def control_read_thread(control_in):
 293     global initialized
 294     with open(control_in, 'rb', 0) as fn:
 295         arg = 0
 296         while arg is not None:
 297             arg, typ, payload = control_read(fn)
 298
 299             if typ == CTRL_CMD_INITIALIZED:
 300                 initialized = True
 301
 302 def control_write(arg, typ, payload):
 303     global fn_out
 304
 305     if fn_out is not None:
 306         packet = bytearray()
 307         packet += struct.pack('>sBHBB', b'T', 0, len(payload) + 2, arg, typ)
 308         if sys.version_info[0] >= 3 and isinstance(payload, str):
 309             packet += payload.encode('utf-8')
 310         else:
 311             packet += payload
 312
 313         fn_out.write(packet)
 314
 315 def extcap_capture(interface, fifo, control_in, control_out):
 316     global fn_out
 317
 318     counter = 1
 319
 320     with open(fifo, 'wb', 0) as fh:
 321
 322         fh.write(pcap_header())
 323
 324         if control_out is not None:
 325             fn_out = open(control_out, 'wb', 0)
 326             control_write(CTRL_ARG_LOGGER, CTRL_CMD_SET, "Log started at " + time.strftime("%c") + "\n")
 327
 328         if control_in is not None:
 329             # Start reading thread
 330             thread = Thread(target=control_read_thread, args=(control_in,))
 331             thread.start()
 332
 333         with serial.Serial(baudrate=9600,
 334                         bytesize=serial.EIGHTBITS,
 335                         parity=serial.PARITY_NONE,
 336                         port=interface,
 337                         stopbits=serial.STOPBITS_ONE,
 338                         timeout = 0.1) as receiver:
 339
 340             # set GNSS config
 341             log("Configuring GNSS constellations:\n")
 342             for (gnssId, resTrkCh, maxTrkCh, enable, sigCfgMask) in UBX_GNSS_CONFIGS:
 343                 sendUbxMsg(receiver, ubxCfgGnss(gnssId, resTrkCh, maxTrkCh, enable, sigCfgMask))
 344
 345             # query GNSS config
 346             log("Querying GNSS constellation config:\n")
 347             sendUbxMsg(receiver, ubxMsg(UBX_CFG_GNSS, []))
 348
 349             # query SBAS config
 350             log("Querying SBAS config:\n")
 351             sendUbxMsg(receiver, ubxMsg(UBX_CFG_SBAS, []))
 352
 353             # set the message rates
 354             log("Setting UBX msg rates:\n")
 355             for (ubxClassId, rate) in UBX_MSG_RATES:
 356                 sendUbxMsg(receiver, ubxCfgMsg(ubxClassId, rate))
 357
 358             ubx_in_data = bytearray()
 359
 360             while True:
 361                 ubx_in_data += receiver.read(8192)
 362
 363                 i = 0
 364
 365                 # Is there enough data remaining for a packet of min. possible size?
 366                 while i < len(ubx_in_data) - UBX_HEADER_SIZE - UBX_CHKSUM_SIZE + 1:
 367
 368                     if ubx_in_data[i] == UBX_PREAMBLE_1 and ubx_in_data[i+1] == UBX_PREAMBLE_2:
 369
 370                         (payload_len,) = struct.unpack("<H", ubx_in_data[i + UBX_PAYLOAD_LEN_OFFSET : i + UBX_PAYLOAD_LEN_OFFSET + 2])
 371
 372                         # Is there enough data remaining for the complete message?
 373                         if i + UBX_HEADER_SIZE + payload_len + UBX_CHKSUM_SIZE <= len(ubx_in_data):
 374                             ubx_frame = ubx_in_data[i : i + UBX_HEADER_SIZE + payload_len + UBX_CHKSUM_SIZE]
 375
 376                             log("Emitting UBX PCAP packet with header " + ubx_frame[0:6].hex() + "\n")
 377
 378                             fh.write(pcap_packet(ubx_frame))
 379
 380                             i = i + UBX_HEADER_SIZE + payload_len + UBX_CHKSUM_SIZE
 381
 382                         else:
 383                             break
 384
 385                     else:
 386                         i += 1
 387
 388                 ubx_in_data = ubx_in_data[i:]
 389
 390     thread.join()
 391     if fn_out is not None:
 392         fn_out.close()
 393
 394 def extcap_close_fifo(fifo):
 395     # This is apparently needed to workaround an issue on Windows/macOS
 396     # where the message cannot be read. (really?)
 397     fh = open(fifo, 'wb', 0)
 398     fh.close()
 399
 400 def usage():
 401     print("Usage: %s <--extcap-interfaces | --extcap-dlts | --extcap-interface | --extcap-config | --capture | --extcap-capture-filter | --fifo>" % sys.argv[0] )
 402
 403 if __name__ == '__main__':
 404     option = ""
 405
 406     parser = argparse.ArgumentParser(description="u-blox UBX extcap")
 407
 408     # Extcap Arguments
 409     parser.add_argument("--capture", help="Start the capture routine", action="store_true" )
 410     parser.add_argument("--extcap-interfaces", help="Provide a list of interfaces to capture from", action="store_true")
 411     parser.add_argument("--extcap-interface", help="Provide the interface to capture from")
 412     parser.add_argument("--extcap-dlts", help="Provide a list of dlts for the given interface", action="store_true")
 413     parser.add_argument("--extcap-config", help="Provide a list of configurations for the given interface", action="store_true")
 414     parser.add_argument("--extcap-capture-filter", help="Used together with capture to provide a capture filter")
 415     parser.add_argument("--fifo", help="Use together with capture to provide the fifo to dump data to")
 416     parser.add_argument("--extcap-control-in", help="Used to get control messages from toolbar")
 417     parser.add_argument("--extcap-control-out", help="Used to send control messages to toolbar")
 418     parser.add_argument("--extcap-version", help="Shows the version of this utility", nargs='?', default="")
 419     parser.add_argument("--extcap-reload-option", help="Reload elements for the given option")
 420
 421     try:
 422         args, unknown = parser.parse_known_args()
 423     except argparse.ArgumentError as exc:
 424         print("%s: %s" % (exc.argument.dest, exc.message), file=sys.stderr)
 425         fifo_found = 0
 426         fifo = ""
 427         for arg in sys.argv:
 428             if arg == "--fifo" or arg == "--extcap-fifo":
 429                 fifo_found = 1
 430             elif fifo_found == 1:
 431                 fifo = arg
 432                 break
 433         extcap_close_fifo(fifo)
 434         sys.exit(ERROR_ARG)
 435
 436     if len(sys.argv) <= 1:
 437         parser.exit("No arguments given!")
 438
 439     if args.extcap_version and not args.extcap_interfaces:
 440         extcap_version()
 441         sys.exit(0)
 442
 443     if not args.extcap_interfaces and args.extcap_interface is None:
 444         parser.exit("An interface must be provided or the selection must be displayed")
 445
 446     if args.extcap_interfaces or args.extcap_interface is None:
 447         extcap_interfaces()
 448         sys.exit(0)
 449
 450     if len(unknown) > 1:
 451         print(f"{len(unknown)} unknown arguments given")
 452
 453     if args.extcap_reload_option and len(args.extcap_reload_option) > 0:
 454         option = args.extcap_reload_option
 455
 456     if args.extcap_config:
 457         extcap_config(option)
 458     elif args.extcap_dlts:
 459         extcap_dlts()
 460     elif args.capture:
 461         if args.fifo is None:
 462             sys.exit(ERROR_FIFO)
 463         try:
 464             extcap_capture(args.extcap_interface, args.fifo, args.extcap_control_in, args.extcap_control_out)
 465         except KeyboardInterrupt:
 466             pass
 467     else:
 468         usage()
 469         sys.exit(ERROR_USAGE)