rawgps: add oemdre support (#24059)

* oemdre works

* fast receive

* parse oemdre

* parsing dre

* parsing oemdre

* unused import

* remove unparsed msgs

* import, don't redefine

Co-authored-by: Comma Device <device@comma.ai>

selfdrive/sensord/rawgps/modemdiag.py

@@ -1,5 +1,6 @@
 import os
 import time
+import select
 from serial import Serial
 from crcmod import mkCrcFun
 from struct import pack, unpack_from, calcsize
@@ -7,10 +8,11 @@ from struct import pack, unpack_from, calcsize
 class ModemDiag:
   def __init__(self):
     self.serial = self.open_serial()
+    self.pend = b''
 
   def open_serial(self):
     def op():
-      return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True)
+      return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0)
     try:
       serial = op()
     except Exception:
@@ -22,6 +24,8 @@ class ModemDiag:
       os.system("sudo chmod 666 /dev/ttyUSB0")
       serial = op()
     serial.flush()
+    serial.reset_input_buffer()
+    serial.reset_output_buffer()
     return serial
 
   ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
@@ -45,13 +49,15 @@ class ModemDiag:
     return payload[:-2]
 
   def recv(self):
-    raw_payload = []
+    # self.serial.read_until makes tons of syscalls!
-    while 1:
+    raw_payload = [self.pend]
-      char_read = self.serial.read()
+    while self.TRAILER_CHAR not in raw_payload[-1]:
-      raw_payload.append(char_read)
+      select.select([self.serial.fd], [], [])
-      if char_read.endswith(self.TRAILER_CHAR):
+      raw = self.serial.read(0x10000)
-        break
+      raw_payload.append(raw)
     raw_payload = b''.join(raw_payload)
+    raw_payload, self.pend = raw_payload.split(self.TRAILER_CHAR, 1)
+    raw_payload += self.TRAILER_CHAR
     unframed_message = self.hdlc_decapsulate(raw_payload)
     return unframed_message[0], unframed_message[1:]
 

selfdrive/sensord/rawgps/rawgpsd.py

@@ -6,18 +6,19 @@ import itertools
 import math
 import time
 from typing import NoReturn
-from struct import unpack_from, calcsize
+from struct import unpack_from, calcsize, pack
 
 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.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
 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 oemdre_measurement_report, oemdre_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
+from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT
 
 DEBUG = int(os.getenv("DEBUG", "0"))==1
 
@@ -71,9 +72,13 @@ def main() -> NoReturn:
   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)
 
+  unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
+  unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
+
   log_types = [
     LOG_GNSS_GPS_MEASUREMENT_REPORT,
     LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
+    LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
   ]
   pub_types = ['qcomGnss']
   if int(os.getenv("PUBLISH_EXTERNAL", "0")) == 1:
@@ -81,11 +86,18 @@ def main() -> NoReturn:
     log_types.append(LOG_GNSS_POSITION_REPORT)
     pub_types.append("gpsLocationExternal")
 
-  # disable DPO power savings for more accuracy
+  # connect to modem
-  os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'")
-  os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
   diag = ModemDiag()
 
+  # NV enable OEMDRE
+  # TODO: it has to reboot for this to take effect
+  DIAG_NV_READ_F = 38
+  DIAG_NV_WRITE_F = 39
+  NV_GNSS_OEM_FEATURE_MASK = 7165
+
+  opcode, payload = send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
+  opcode, payload = send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
+
   def try_setup_logs(diag, log_types):
     for _ in range(5):
       try:
@@ -104,6 +116,29 @@ def main() -> NoReturn:
   try_setup_logs(diag, log_types)
   cloudlog.warning("rawgpsd: setup logs done")
 
+  # disable DPO power savings for more accuracy
+  os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'")
+  os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
+
+  # enable OEMDRE mode
+  DIAG_SUBSYS_CMD_F = 75
+  DIAG_SUBSYS_GPS = 13
+  CGPS_DIAG_PDAPI_CMD = 0x64
+  CGPS_OEM_CONTROL = 202
+  GPSDIAG_OEMFEATURE_DRE = 1
+  GPSDIAG_OEM_DRE_ON = 1
+
+  # gpsdiag_OemControlReqType
+  opcode, payload = send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
+      DIAG_SUBSYS_GPS,           # Subsystem Id
+      CGPS_DIAG_PDAPI_CMD,       # Subsystem Command Code
+      CGPS_OEM_CONTROL,          # CGPS Command Code
+      0,                         # Version
+      GPSDIAG_OEMFEATURE_DRE,
+      GPSDIAG_OEM_DRE_ON,
+      0,0
+  ))
+
   pm = messaging.PubMaster(pub_types)
 
   while 1:
@@ -118,8 +153,51 @@ def main() -> NoReturn:
     if log_type not in log_types:
       continue
     if DEBUG:
-      print("%.2f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
+      print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
-    if log_type == LOG_GNSS_POSITION_REPORT:
+    if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
+      msg = messaging.new_message('qcomGnss')
+
+      gnss = msg.qcomGnss
+      gnss.logTs = log_time
+      gnss.init('drMeasurementReport')
+      report = gnss.drMeasurementReport
+
+      dat = unpack_oemdre_meas(log_payload)
+      for k,v in dat.items():
+        if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
+          k += "Ms"
+        if k == "version":
+          assert v == 2
+        elif k == "svCount" or k.startswith("cdmaClockInfo["):
+          # TODO: should we save cdmaClockInfo?
+          pass
+        elif k == "systemRtcValid":
+          setattr(report, k, bool(v))
+        else:
+          setattr(report, k, v)
+
+      report.init('sv', dat['svCount'])
+      sats = log_payload[size_oemdre_meas:]
+      for i in range(dat['svCount']):
+        sat = unpack_oemdre_meas_sv(sats[size_oemdre_meas_sv*i:size_oemdre_meas_sv*(i+1)])
+        sv = report.sv[i]
+        sv.init('measurementStatus')
+        for k,v in sat.items():
+          if k in ["unkn", "measurementStatus2"]:
+            pass
+          elif k == "multipathEstimateValid":
+            sv.measurementStatus.multipathEstimateIsValid = bool(v)
+          elif k == "directionValid":
+            sv.measurementStatus.directionIsValid = bool(v)
+          elif k == "goodParity":
+            setattr(sv, k, bool(v))
+          elif k == "measurementStatus":
+            for kk,vv in measurementStatusFields.items():
+              setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
+          else:
+            setattr(sv, k, v)
+      pm.send('qcomGnss', msg)
+    elif log_type == LOG_GNSS_POSITION_REPORT:
       report = unpack_position(log_payload)
       if report["u_PosSource"] != 2:
         continue

selfdrive/sensord/rawgps/structs.py

@@ -16,6 +16,93 @@ LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
 LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
 LOG_GNSS_CONFIGURATION_STATE = 0x1516
 
+oemdre_measurement_report = """
+  uint8_t version;
+  uint8_t reason;
+  uint8_t sv_count;
+  uint8_t seq_num;
+  uint8_t seq_max;
+  uint16_t rf_loss;
+
+  uint8_t system_rtc_valid;
+  uint32_t f_count;
+  uint32_t clock_resets;
+  uint64_t system_rtc_time;
+
+  uint8_t gps_leap_seconds;
+  uint8_t gps_leap_seconds_uncertainty;
+  float gps_to_glonass_time_bias_milliseconds;
+  float gps_to_glonass_time_bias_milliseconds_uncertainty;
+
+  uint16_t gps_week;
+  uint32_t gps_milliseconds;
+  uint32_t gps_time_bias;
+  uint32_t gps_clock_time_uncertainty;
+  uint8_t gps_clock_source;
+
+  uint8_t glonass_clock_source;
+  uint8_t glonass_year;
+  uint16_t glonass_day;
+  uint32_t glonass_milliseconds;
+  float glonass_time_bias;
+  float glonass_clock_time_uncertainty;
+
+  float clock_frequency_bias;
+  float clock_frequency_uncertainty;
+  uint8_t frequency_source;
+
+  uint32_t cdma_clock_info[5];
+
+  uint8_t source;
+"""
+
+oemdre_measurement_report_sv = """
+  uint8_t sv_id;
+  uint8_t unkn;
+  int8_t glonass_frequency_index;
+  uint32_t observation_state;
+  uint8_t observations;
+  uint8_t good_observations;
+  uint8_t filter_stages;
+  uint8_t predetect_interval;
+  uint8_t cycle_slip_count;
+  uint16_t postdetections;
+
+  uint32_t measurement_status;
+  uint32_t measurement_status2;
+
+  uint16_t carrier_noise;
+  uint16_t rf_loss;
+  int16_t latency;
+
+  float filtered_measurement_fraction;
+  uint32_t filtered_measurement_integral;
+  float filtered_time_uncertainty;
+  float filtered_speed;
+  float filtered_speed_uncertainty;
+
+  float unfiltered_measurement_fraction;
+  uint32_t unfiltered_measurement_integral;
+  float unfiltered_time_uncertainty;
+  float unfiltered_speed;
+  float unfiltered_speed_uncertainty;
+
+  uint8_t multipath_estimate_valid;
+  uint32_t multipath_estimate;
+  uint8_t direction_valid;
+  float azimuth;
+  float elevation;
+  float doppler_acceleration;
+  float fine_speed;
+  float fine_speed_uncertainty;
+
+  uint64_t carrier_phase;
+  uint32_t f_count;
+
+  uint16_t parity_error_count;
+  uint8_t good_parity;
+"""
+
 glonass_measurement_report = """
   uint8_t version;
   uint32_t f_count;
@@ -184,6 +271,8 @@ def parse_struct(ss):
   st = "<"
   nams = []
   for l in ss.strip().split("\n"):
+    if len(l.strip()) == 0:
+      continue
     typ, nam = l.split(";")[0].split()
     #print(typ, nam)
     if typ == "float" or '_Flt' in nam:
@@ -202,7 +291,7 @@ def parse_struct(ss):
       st += "H"
     elif typ in ["int16", "int16_t"]:
       st += "h"
-    elif typ == "uint64":
+    elif typ in ["uint64", "uint64_t"]:
       st += "Q"
     else:
       print("unknown type", typ)