sensord: add support for Quectel raw gps (commaai#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

@@ -42,6 +42,9 @@
   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}

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))
+
+
+

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

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()