From 1df3c869994f733579be4ff6df7e8bddf8c46c56 Mon Sep 17 00:00:00 2001
From: George Hotz <72895+geohot@users.noreply.github.com>
Date: Thu, 24 Mar 2022 23:07:26 -0700
Subject: [PATCH] sensord: add support for Quectel raw gps (#24027)

* connecting to rawgps

* dumping rawfps packets

* this works, but it might be useless

* fix parsing

* parsing 0x1476

* compare

* compare better

* refactoring

* parsing and broadcasting GPS packet

* glonass support

* compare using submaster

* fix compare for glonass

* update cereal

* make linter happy

* other linter issue

* last mypy complaints

* add support for publishing gpsLocationExternal

* set flags to 1 to match ubloxd

* ready to ship

* qcomdiag

* use unused variables

* last one

Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
---
 selfdrive/manager/process_config.py   |   3 +
 selfdrive/sensord/rawgps/compare.py   |  66 ++++++++
 selfdrive/sensord/rawgps/modemdiag.py | 101 ++++++++++++
 selfdrive/sensord/rawgps/rawgpsd.py   | 201 +++++++++++++++++++++++
 selfdrive/sensord/rawgps/structs.py   | 224 ++++++++++++++++++++++++++
 5 files changed, 595 insertions(+)
 create mode 100755 selfdrive/sensord/rawgps/compare.py
 create mode 100644 selfdrive/sensord/rawgps/modemdiag.py
 create mode 100755 selfdrive/sensord/rawgps/rawgpsd.py
 create mode 100644 selfdrive/sensord/rawgps/structs.py

diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
index bc88154c2..da4284571 100644
--- a/selfdrive/manager/process_config.py
+++ b/selfdrive/manager/process_config.py
@@ -42,6 +42,9 @@ procs = [
   PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON),
   PythonProcess("shutdownd", "selfdrive.hardware.eon.shutdownd", enabled=EON),
   PythonProcess("androidd", "selfdrive.hardware.eon.androidd", enabled=EON, persistent=True),
+
+  # Experimental
+  PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=os.path.isfile("/persist/comma/use-quectel-rawgps")),
 ]
 
 managed_processes = {p.name: p for p in procs}
diff --git a/selfdrive/sensord/rawgps/compare.py b/selfdrive/sensord/rawgps/compare.py
new file mode 100755
index 000000000..073647499
--- /dev/null
+++ b/selfdrive/sensord/rawgps/compare.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+import cereal.messaging as messaging
+from laika import constants
+
+if __name__ == "__main__":
+  sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss'])
+
+  meas = None
+  while 1:
+    sm.update()
+    if sm['ubloxGnss'].which() == "measurementReport":
+      meas = sm['ubloxGnss'].measurementReport.measurements
+    if not sm.updated['qcomGnss'] or meas is None:
+      continue
+    report = sm['qcomGnss'].measurementReport
+    if report.source not in [0, 1]:
+      continue
+    GLONASS = report.source == 1
+    recv_time = report.milliseconds / 1000
+
+    car = []
+    print("qcom has ", list(sorted([x.svId for x in report.sv])))
+    print("ublox has", list(sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)])))
+    for i in report.sv:
+      # match to ublox
+      tm = None
+      for m in meas:
+        if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS:
+          tm = m
+        if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS:
+          tm = m
+      if tm is None:
+        continue
+      
+      if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
+        sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000
+        ublox_psuedorange = tm.pseudorange
+        qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
+        if GLONASS:
+          glonass_freq = tm.glonassFrequencyIndex - 7
+          ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler)
+        else:
+          ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler
+        qcom_speed = i.unfilteredSpeed
+        car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno))
+
+    if len(car) == 0:
+      print("nothing to compare")
+      continue
+
+    pr_err, speed_err = 0., 0.
+    for c in car:
+      ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5]
+      pr_err += ublox_psuedorange - qcom_psuedorange
+      speed_err += ublox_speed - qcom_speed
+    pr_err /= len(car)
+    speed_err /= len(car)
+    print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
+    for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)):
+      svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
+      print("svid: %3d  pseudorange: %10.2f m  speed: %8.2f m/s   meas: %12.2f  speed: %10.2f   meas_err: %10.3f speed_err: %8.3f cno: %d" %
+        (svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,
+        ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno))
+
+
+
diff --git a/selfdrive/sensord/rawgps/modemdiag.py b/selfdrive/sensord/rawgps/modemdiag.py
new file mode 100644
index 000000000..21e39f4f3
--- /dev/null
+++ b/selfdrive/sensord/rawgps/modemdiag.py
@@ -0,0 +1,101 @@
+import os
+import time
+from serial import Serial
+from crcmod import mkCrcFun
+from struct import pack, unpack_from, calcsize
+
+class ModemDiag:
+  def __init__(self):
+    self.serial = self.open_serial()
+
+  def open_serial(self):
+    def op():
+      return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True)
+    try:
+      serial = op()
+    except Exception:
+      # TODO: this is a hack to get around modemmanager's exclusive open
+      print("unlocking serial...")
+      os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/unbind\'')
+      os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/bind\'')
+      time.sleep(0.5)
+      os.system("sudo chmod 666 /dev/ttyUSB0")
+      serial = op()
+    serial.flush()
+    return serial
+
+  ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
+  ESCAPE_CHAR = b'\x7d'
+  TRAILER_CHAR = b'\x7e'
+
+  def hdlc_encapsulate(self, payload):
+    payload += pack('<H', ModemDiag.ccitt_crc16(payload))
+    payload = payload.replace(self.ESCAPE_CHAR, bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]))
+    payload = payload.replace(self.TRAILER_CHAR, bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]))
+    payload += self.TRAILER_CHAR
+    return payload
+
+  def hdlc_decapsulate(self, payload):
+    assert len(payload) >= 3
+    assert payload[-1:] == self.TRAILER_CHAR
+    payload = payload[:-1]
+    payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]), self.TRAILER_CHAR)
+    payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]), self.ESCAPE_CHAR)
+    assert payload[-2:] == pack('<H', ModemDiag.ccitt_crc16(payload[:-2]))
+    return payload[:-2]
+
+  def recv(self):
+    raw_payload = []
+    while 1:
+      char_read = self.serial.read()
+      raw_payload.append(char_read)
+      if char_read.endswith(self.TRAILER_CHAR):
+        break
+    raw_payload = b''.join(raw_payload)
+    unframed_message = self.hdlc_decapsulate(raw_payload)
+    return unframed_message[0], unframed_message[1:]
+
+  def send(self, packet_type, packet_payload):
+    self.serial.write(self.hdlc_encapsulate(bytes([packet_type]) + packet_payload))
+
+# *** end class ***
+
+DIAG_LOG_F = 16
+DIAG_LOG_CONFIG_F = 115
+LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1
+LOG_CONFIG_SET_MASK_OP = 3
+LOG_CONFIG_SUCCESS_S = 0
+
+def send_recv(diag, packet_type, packet_payload):
+  diag.send(packet_type, packet_payload)
+  while 1:
+    opcode, payload = diag.recv()
+    if opcode != DIAG_LOG_F:
+      break
+  return opcode, payload
+
+def setup_logs(diag, types_to_log):
+  opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xI', LOG_CONFIG_RETRIEVE_ID_RANGES_OP))
+
+  header_spec = '<3xII'
+  operation, status = unpack_from(header_spec, payload)
+  assert operation == LOG_CONFIG_RETRIEVE_ID_RANGES_OP
+  assert status == LOG_CONFIG_SUCCESS_S
+
+  log_masks = unpack_from('<16I', payload, calcsize(header_spec))
+
+  for log_type, log_mask_bitsize in enumerate(log_masks):
+    if log_mask_bitsize:
+      log_mask = [0] * ((log_mask_bitsize+7)//8)
+      for i in range(log_mask_bitsize):
+        if ((log_type<<12)|i) in types_to_log:
+          log_mask[i//8] |= 1 << (i%8)
+      opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xIII',
+          LOG_CONFIG_SET_MASK_OP,
+          log_type,
+          log_mask_bitsize
+      ) + bytes(log_mask))
+      assert opcode == DIAG_LOG_CONFIG_F
+      operation, status = unpack_from(header_spec, payload)
+      assert operation == LOG_CONFIG_SET_MASK_OP
+      assert status == LOG_CONFIG_SUCCESS_S
diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py
new file mode 100755
index 000000000..bf7ee59a0
--- /dev/null
+++ b/selfdrive/sensord/rawgps/rawgpsd.py
@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+import os
+import sys
+import signal
+import itertools
+import math
+from typing import NoReturn
+from struct import unpack_from, calcsize
+
+import cereal.messaging as messaging
+from cereal import log
+from selfdrive.swaglog import cloudlog
+
+from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs
+from selfdrive.sensord.rawgps.structs import dict_unpacker
+from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv
+from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv
+from selfdrive.sensord.rawgps.structs import LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT
+from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT
+
+miscStatusFields = {
+  "multipathEstimateIsValid": 0,
+  "directionIsValid": 1,
+}
+
+measurementStatusFields = {
+  "subMillisecondIsValid": 0,
+  "subBitTimeIsKnown": 1,
+  "satelliteTimeIsKnown": 2,
+  "bitEdgeConfirmedFromSignal": 3,
+  "measuredVelocity": 4,
+  "fineOrCoarseVelocity": 5,
+  "lockPointValid": 6,
+  "lockPointPositive": 7,
+
+  "lastUpdateFromDifference": 9,
+  "lastUpdateFromVelocityDifference": 10,
+  "strongIndicationOfCrossCorelation": 11,
+  "tentativeMeasurement": 12,
+  "measurementNotUsable": 13,
+  "sirCheckIsNeeded": 14,
+  "probationMode": 15,
+
+  "multipathIndicator": 24,
+  "imdJammingIndicator": 25,
+  "lteB13TxJammingIndicator": 26,
+  "freshMeasurementIndicator": 27,
+}
+
+measurementStatusGPSFields = {
+  "gpsRoundRobinRxDiversity": 18,
+  "gpsRxDiversity": 19,
+  "gpsLowBandwidthRxDiversityCombined": 20,
+  "gpsHighBandwidthNu4": 21,
+  "gpsHighBandwidthNu8": 22,
+  "gpsHighBandwidthUniform": 23,
+}
+
+measurementStatusGlonassFields = {
+  "glonassMeanderBitEdgeValid": 16,
+  "glonassTimeMarkValid": 17
+}
+
+def main() -> NoReturn:
+  unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
+  unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
+
+  unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
+  unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
+
+  log_types = [
+    LOG_GNSS_GPS_MEASUREMENT_REPORT,
+    LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
+  ]
+  pub_types = ['qcomGnss']
+  if int(os.getenv("PUBLISH_EXTERNAL", "0")) == 1:
+    unpack_position, _ = dict_unpacker(position_report)
+    log_types.append(LOG_GNSS_POSITION_REPORT)
+    pub_types.append("gpsLocationExternal")
+
+  os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
+  diag = ModemDiag()
+
+  def try_setup_logs(diag, log_types):
+    for _ in range(5):
+      try:
+        setup_logs(diag, log_types)
+        break
+      except Exception:
+        pass
+
+  def disable_logs(sig, frame):
+    os.system("mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea")
+    cloudlog.warning("rawgpsd: shutting down")
+    try_setup_logs(diag, [])
+    cloudlog.warning("rawgpsd: logs disabled")
+    sys.exit(0)
+  signal.signal(signal.SIGINT, disable_logs)
+  try_setup_logs(diag, log_types)
+  cloudlog.warning("rawgpsd: setup logs done")
+
+  pm = messaging.PubMaster(pub_types)
+
+  while 1:
+    opcode, payload = diag.recv()
+    assert opcode == DIAG_LOG_F
+    (pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
+    if pending_msgs > 0:
+      cloudlog.debug("have %d pending messages" % pending_msgs)
+    assert log_outer_length == len(inner_log_packet)
+    (log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
+    assert log_inner_length == len(inner_log_packet)
+    if log_type not in log_types:
+      continue
+    #print("got log: %x" % log_type)
+
+    if log_type == LOG_GNSS_POSITION_REPORT:
+      report = unpack_position(log_payload)
+      if report["u_PosSource"] != 2:
+        continue
+      vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
+      vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
+
+      msg = messaging.new_message('gpsLocationExternal')
+      gps = msg.gpsLocationExternal
+      gps.flags = 1
+      gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
+      gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
+      gps.altitude = report["q_FltFinalPosAlt"]
+      gps.speed = math.sqrt(sum([x**2 for x in vNED]))
+      gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
+      # TODO: this probably isn't right, use laika for this
+      gps.timestamp = report['w_GpsWeekNumber']*604800*1000 + report['q_GpsFixTimeMs']
+      gps.source = log.GpsLocationData.SensorSource.qcomdiag
+      gps.vNED = vNED
+      gps.verticalAccuracy = report["q_FltVdop"]
+      gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi
+      gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
+
+      pm.send('gpsLocationExternal', msg)
+
+    if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
+      msg = messaging.new_message('qcomGnss')
+
+      gnss = msg.qcomGnss
+      gnss.logTs = log_time
+      gnss.init('measurementReport')
+      report = gnss.measurementReport
+
+      if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
+        dat = unpack_gps_meas(log_payload)
+        sats = log_payload[size_gps_meas:]
+        unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
+        report.source = 0  # gps
+        measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items())
+      elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
+        dat = unpack_glonass_meas(log_payload)
+        sats = log_payload[size_glonass_meas:]
+        unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
+        report.source = 1  # glonass
+        measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items())
+      else:
+        assert False
+
+      for k,v in dat.items():
+        if k == "version":
+          assert v == 0
+        elif k == "week":
+          report.gpsWeek = v
+        elif k == "svCount":
+          pass
+        else:
+          setattr(report, k, v)
+      assert len(sats)//dat['svCount'] == size_meas_sv
+      report.init('sv', dat['svCount'])
+      for i in range(dat['svCount']):
+        sv = report.sv[i]
+        sv.init('measurementStatus')
+        sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)])
+        for k,v in sat.items():
+          if k == "parityErrorCount":
+            sv.gpsParityErrorCount = v
+          elif k == "frequencyIndex":
+            sv.glonassFrequencyIndex = v
+          elif k == "hemmingErrorCount":
+            sv.glonassHemmingErrorCount = v
+          elif k == "measurementStatus":
+            for kk,vv in itertools.chain(*measurement_status_fields):
+              setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
+          elif k == "miscStatus":
+            for kk,vv in miscStatusFields.items():
+              setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
+          elif k == "pad":
+            pass
+          else:
+            setattr(sv, k, v)
+
+      pm.send('qcomGnss', msg)
+
+if __name__ == "__main__":
+  main()
diff --git a/selfdrive/sensord/rawgps/structs.py b/selfdrive/sensord/rawgps/structs.py
new file mode 100644
index 000000000..ed6c49af1
--- /dev/null
+++ b/selfdrive/sensord/rawgps/structs.py
@@ -0,0 +1,224 @@
+from struct import unpack_from, calcsize
+
+LOG_GNSS_POSITION_REPORT = 0x1476
+LOG_GNSS_GPS_MEASUREMENT_REPORT = 0x1477
+LOG_GNSS_CLOCK_REPORT = 0x1478
+LOG_GNSS_GLONASS_MEASUREMENT_REPORT = 0x1480
+LOG_GNSS_BDS_MEASUREMENT_REPORT = 0x1756
+LOG_GNSS_GAL_MEASUREMENT_REPORT = 0x1886
+
+LOG_GNSS_OEMDRE_MEASUREMENT_REPORT = 0x14DE
+LOG_GNSS_OEMDRE_SVPOLY_REPORT = 0x14E1
+
+LOG_GNSS_ME_DPO_STATUS = 0x1838
+LOG_GNSS_CD_DB_REPORT = 0x147B
+LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
+LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
+LOG_GNSS_CONFIGURATION_STATE = 0x1516
+
+glonass_measurement_report = """
+  uint8_t version;
+  uint32_t f_count;
+  uint8_t glonass_cycle_number;
+  uint16_t glonass_number_of_days;
+  uint32_t milliseconds;
+  float time_bias;
+  float clock_time_uncertainty;
+  float clock_frequency_bias;
+  float clock_frequency_uncertainty;
+  uint8_t sv_count;
+"""
+
+glonass_measurement_report_sv = """
+  uint8_t sv_id;
+  int8_t frequency_index;
+  uint8_t observation_state; // SVObservationStates
+  uint8_t observations;
+  uint8_t good_observations;
+  uint8_t hemming_error_count;
+  uint8_t filter_stages;
+  uint16_t carrier_noise;
+  int16_t latency;
+  uint8_t predetect_interval;
+  uint16_t postdetections;
+  uint32_t unfiltered_measurement_integral;
+  float unfiltered_measurement_fraction;
+  float unfiltered_time_uncertainty;
+  float unfiltered_speed;
+  float unfiltered_speed_uncertainty;
+  uint32_t measurement_status;
+  uint8_t misc_status;
+  uint32_t multipath_estimate;
+  float azimuth;
+  float elevation;
+  int32_t carrier_phase_cycles_integral;
+  uint16_t carrier_phase_cycles_fraction;
+  float fine_speed;
+  float fine_speed_uncertainty;
+  uint8_t cycle_slip_count;
+  uint32_t pad;
+"""
+
+gps_measurement_report = """
+  uint8_t version;
+  uint32_t f_count;
+  uint16_t week;
+  uint32_t milliseconds;
+  float time_bias;
+  float clock_time_uncertainty;
+  float clock_frequency_bias;
+  float clock_frequency_uncertainty;
+  uint8_t sv_count;
+"""
+
+gps_measurement_report_sv = """
+  uint8_t sv_id;
+  uint8_t observation_state; // SVObservationStates
+  uint8_t observations;
+  uint8_t good_observations;
+  uint16_t parity_error_count;
+  uint8_t filter_stages;
+  uint16_t carrier_noise;
+  int16_t latency;
+  uint8_t predetect_interval;
+  uint16_t postdetections;
+  uint32_t unfiltered_measurement_integral;
+  float unfiltered_measurement_fraction;
+  float unfiltered_time_uncertainty;
+  float unfiltered_speed;
+  float unfiltered_speed_uncertainty;
+  uint32_t measurement_status;
+  uint8_t misc_status;
+  uint32_t multipath_estimate;
+  float azimuth;
+  float elevation;
+  int32_t carrier_phase_cycles_integral;
+  uint16_t carrier_phase_cycles_fraction;
+  float fine_speed;
+  float fine_speed_uncertainty;
+  uint8_t cycle_slip_count;
+  uint32_t pad;
+"""
+
+position_report = """
+  uint8       u_Version;                /* Version number of DM log */
+  uint32      q_Fcount;                 /* Local millisecond counter */
+  uint8       u_PosSource;              /* Source of position information */ /*  0: None 1: Weighted least-squares 2: Kalman filter 3: Externally injected 4: Internal database    */
+  uint32      q_Reserved1;              /* Reserved memory field */
+  uint16      w_PosVelFlag;             /* Position velocity bit field: (see DM log 0x1476 documentation) */
+  uint32      q_PosVelFlag2;            /* Position velocity 2 bit field: (see DM log 0x1476 documentation) */
+  uint8       u_FailureCode;            /* Failure code: (see DM log 0x1476 documentation) */
+  uint16      w_FixEvents;              /* Fix events bit field: (see DM log 0x1476 documentation) */
+  uint32 _fake_align_week_number;
+  uint16      w_GpsWeekNumber;          /* GPS week number of position */
+  uint32      q_GpsFixTimeMs;           /* GPS fix time of week of in milliseconds */
+  uint8       u_GloNumFourYear;         /* Number of Glonass four year cycles */
+  uint16      w_GloNumDaysInFourYear;   /* Glonass calendar day in four year cycle */
+  uint32      q_GloFixTimeMs;           /* Glonass fix time of day in milliseconds */
+  uint32      q_PosCount;               /* Integer count of the number of unique positions reported */
+  uint64      t_DblFinalPosLatLon[2];   /* Final latitude and longitude of position in radians */
+  uint32      q_FltFinalPosAlt;         /* Final height-above-ellipsoid altitude of position */
+  uint32      q_FltHeadingRad;          /* User heading in radians */
+  uint32      q_FltHeadingUncRad;       /* User heading uncertainty in radians */
+  uint32      q_FltVelEnuMps[3];        /* User velocity in east, north, up coordinate frame. In meters per second. */
+  uint32      q_FltVelSigmaMps[3];      /* Gaussian 1-sigma value for east, north, up components of user velocity */
+  uint32      q_FltClockBiasMeters;     /* Receiver clock bias in meters */
+  uint32      q_FltClockBiasSigmaMeters;  /* Gaussian 1-sigma value for receiver clock bias in meters */
+  uint32      q_FltGGTBMeters;          /* GPS to Glonass time bias in meters */
+  uint32      q_FltGGTBSigmaMeters;     /* Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
+  uint32      q_FltGBTBMeters;          /* GPS to BeiDou time bias in meters */
+  uint32      q_FltGBTBSigmaMeters;     /* Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
+  uint32      q_FltBGTBMeters;          /* BeiDou to Glonass time bias in meters */
+  uint32      q_FltBGTBSigmaMeters;     /* Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
+  uint32      q_FltFiltGGTBMeters;      /* Filtered GPS to Glonass time bias in meters */
+  uint32      q_FltFiltGGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
+  uint32      q_FltFiltGBTBMeters;      /* Filtered GPS to BeiDou time bias in meters */
+  uint32      q_FltFiltGBTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
+  uint32      q_FltFiltBGTBMeters;      /* Filtered BeiDou to Glonass time bias in meters */
+  uint32      q_FltFiltBGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
+  uint32      q_FltSftOffsetSec;        /* SFT offset as computed by WLS in seconds */
+  uint32      q_FltSftOffsetSigmaSec;   /* Gaussian 1-sigma value for SFT offset in seconds */
+  uint32      q_FltClockDriftMps;       /* Clock drift (clock frequency bias) in meters per second */
+  uint32      q_FltClockDriftSigmaMps;  /* Gaussian 1-sigma value for clock drift in meters per second */
+  uint32      q_FltFilteredAlt;         /* Filtered height-above-ellipsoid altitude in meters as computed by WLS */
+  uint32      q_FltFilteredAltSigma;    /* Gaussian 1-sigma value for filtered height-above-ellipsoid altitude in meters */
+  uint32      q_FltRawAlt;              /* Raw height-above-ellipsoid altitude in meters as computed by WLS */
+  uint32      q_FltRawAltSigma;         /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */
+  uint32   align_Flt[14];
+  uint32      q_FltPdop;                /* 3D position dilution of precision as computed from the unweighted 
+  uint32      q_FltHdop;                /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */
+  uint32      q_FltVdop;                /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */
+  uint8       u_EllipseConfidence;      /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */
+  uint32      q_FltEllipseAngle;        /* Angle of semimajor axis with respect to true North, with increasing angles moving clockwise from North. In units of degrees. */
+  uint32      q_FltEllipseSemimajorAxis;  /* Semimajor axis of final horizontal position uncertainty error ellipse.  In units of meters. */
+  uint32      q_FltEllipseSemiminorAxis;  /* Semiminor axis of final horizontal position uncertainty error ellipse.  In units of meters. */
+  uint32      q_FltPosSigmaVertical;    /* Gaussian 1-sigma value for final position height-above-ellipsoid altitude in meters */
+  uint8       u_HorizontalReliability;  /* Horizontal position reliability 0: Not set 1: Very Low 2: Low 3: Medium 4: High    */
+  uint8       u_VerticalReliability;    /* Vertical position reliability */
+  uint16      w_Reserved2;              /* Reserved memory field */
+  uint32      q_FltGnssHeadingRad;      /* User heading in radians derived from GNSS only solution  */
+  uint32      q_FltGnssHeadingUncRad;   /* User heading uncertainty in radians derived from GNSS only solution  */
+  uint32      q_SensorDataUsageMask;    /* Denotes which additional sensor data were used to compute this position fix.  BIT[0] 0x00000001 <96> Accelerometer BIT[1] 0x00000002 <96> Gyro 0x0000FFFC - Reserved A bit set to 1 indicates that certain fields as defined by the SENSOR_AIDING_MASK were aided with sensor data*/
+  uint32      q_SensorAidMask;         /* Denotes which component of the position report was assisted with additional sensors defined in SENSOR_DATA_USAGE_MASK BIT[0] 0x00000001 <96> Heading aided with sensor data BIT[1] 0x00000002 <96> Speed aided with sensor data BIT[2] 0x00000004 <96> Position aided with sensor data BIT[3] 0x00000008 <96> Velocity aided with sensor data 0xFFFFFFF0 <96> Reserved */
+  uint8       u_NumGpsSvsUsed;          /* The number of GPS SVs used in the fix */
+  uint8       u_TotalGpsSvs;            /* Total number of GPS SVs detected by searcher, including ones not used in position calculation */
+  uint8       u_NumGloSvsUsed;          /* The number of Glonass SVs used in the fix */
+  uint8       u_TotalGloSvs;            /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */
+  uint8       u_NumBdsSvsUsed;          /* The number of BeiDou SVs used in the fix */
+  uint8       u_TotalBdsSvs;            /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */
+"""
+
+def name_to_camelcase(nam):
+  ret = []
+  i = 0
+  while i < len(nam):
+    if nam[i] == "_":
+      ret.append(nam[i+1].upper())
+      i += 2
+    else:
+      ret.append(nam[i])
+      i += 1
+  return ''.join(ret)
+
+def parse_struct(ss):
+  st = "<"
+  nams = []
+  for l in ss.strip().split("\n"):
+    typ, nam = l.split(";")[0].split()
+    #print(typ, nam)
+    if typ == "float" or '_Flt' in nam:
+      st += "f"
+    elif typ == "double" or '_Dbl' in nam:
+      st += "d"
+    elif typ in ["uint8", "uint8_t"]:
+      st += "B"
+    elif typ in ["int8", "int8_t"]:
+      st += "b"
+    elif typ in ["uint32", "uint32_t"]:
+      st += "I"
+    elif typ in ["int32", "int32_t"]:
+      st += "i"
+    elif typ in ["uint16", "uint16_t"]:
+      st += "H"
+    elif typ in ["int16", "int16_t"]:
+      st += "h"
+    elif typ == "uint64":
+      st += "Q"
+    else:
+      print("unknown type", typ)
+      assert False
+    if '[' in nam:
+      cnt = int(nam.split("[")[1].split("]")[0])
+      st += st[-1]*(cnt-1)
+      for i in range(cnt):
+        nams.append("%s[%d]" % (nam.split("[")[0], i))
+    else:
+      nams.append(nam)
+  return st, nams
+
+def dict_unpacker(ss, camelcase = False):
+  st, nams = parse_struct(ss)
+  if camelcase:
+    nams = [name_to_camelcase(x) for x in nams]
+  sz = calcsize(st)
+  return lambda x: dict(zip(nams, unpack_from(st, x))), sz