Merge remote-tracking branch 'upstream/master' into disp-personality

.vscode/settings.json

@@ -17,10 +17,10 @@
     "**/.git",
     "**/.venv",
     "**/__pycache__",
-    // exclude directories should be using the symlinked version
+    // exclude directories that should be using the symlinked version
     "common/**",
     "selfdrive/**",
     "system/**",
     "tools/**",
   ]
-}
+}

selfdrive/car/subaru/carstate.py

@@ -29,6 +29,16 @@ class CarState(CarStateBase):
       cp_brakes = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
       ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1
 
+    cp_es_distance = cp_body if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam
+    if not (self.CP.flags & SubaruFlags.HYBRID):
+      eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"])
+
+      # if openpilot is controlling long, an eyesight fault is a non-critical fault. otherwise it's an ACC fault
+      if self.CP.openpilotLongitudinalControl:
+        ret.carFaultedNonCritical = eyesight_fault
+      else:
+        ret.accFaulted = eyesight_fault
+
     cp_wheels = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
     ret.wheelSpeeds = self.get_wheel_speeds(
       cp_wheels.vl["Wheel_Speeds"]["FL"],
@@ -84,7 +94,6 @@ class CarState(CarStateBase):
                         cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
     ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
 
-    cp_es_distance = cp_body if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam
     if self.CP.flags & SubaruFlags.PREGLOBAL:
       self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
       self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]

selfdrive/car/subaru/values.py

@@ -234,21 +234,24 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
       whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission],
+      logging=True,
     ),
+    # Non-OBD requests
     # Some Eyesight modules fail on TESTER_PRESENT_REQUEST
     # TODO: check if this resolves the fingerprinting issue for the 2023 Ascent and other new Subaru cars
     Request(
       [SUBARU_VERSION_REQUEST],
       [SUBARU_VERSION_RESPONSE],
       whitelist_ecus=[Ecu.fwdCamera],
+      bus=0,
     ),
-    # Non-OBD requests
     Request(
       [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
       whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission],
       bus=0,
     ),
+    # GEN2 powertrain bus query
     Request(
       [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],

system/loggerd/uploader.py

@@ -44,7 +44,9 @@ class FakeResponse:
 
 
 def get_directory_sort(d: str) -> list[str]:
-  return [s.rjust(10, '0') for s in d.rsplit('--', 1)]
+  # ensure old format is sorted sooner
+  o = ["0", ] if d.startswith("2024-") else ["1", ]
+  return o + [s.rjust(10, '0') for s in d.rsplit('--', 1)]
 
 def listdir_by_creation(d: str) -> list[str]:
   if not os.path.isdir(d):

system/qcomgpsd/cgpsd.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+import time
+import datetime
+from collections import defaultdict
+
+from cereal import log
+import cereal.messaging as messaging
+from openpilot.common.swaglog import cloudlog
+from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem
+
+# https://campar.in.tum.de/twiki/pub/Chair/NaviGpsDemon/nmea.html#RMC
+"""
+AT+CGPSGPOS=1
+response: '$GNGGA,220212.00,3245.09188,N,11711.76362,W,1,06,24.54,0.0,M,,M,,*77'
+
+AT+CGPSGPOS=2
+response: '$GNGSA,A,3,06,17,19,22,,,,,,,,,14.11,8.95,10.91,1*01
+$GNGSA,A,3,29,26,,,,,,,,,,,14.11,8.95,10.91,4*03'
+
+AT+CGPSGPOS=3
+response: '$GPGSV,3,1,11,06,55,047,22,19,29,053,20,22,19,115,14,05,01,177,,0*68
+$GPGSV,3,2,11,11,77,156,23,12,47,322,17,17,08,066,10,20,25,151,,0*6D
+$GPGSV,3,3,11,24,44,232,,25,16,312,,29,02,260,,0*5D'
+
+AT+CGPSGPOS=4
+response: '$GBGSV,1,1,03,26,75,242,20,29,19,049,16,35,,,24,0*7D'
+
+AT+CGPSGPOS=5
+response: '$GNRMC,220216.00,A,3245.09531,N,11711.76043,W,,,070324,,,A,V*20'
+"""
+
+
+def sfloat(n: str):
+  return float(n) if len(n) > 0 else 0
+
+def checksum(s: str):
+  ret = 0
+  for c in s[1:-3]:
+    ret ^= ord(c)
+  return format(ret, '02X')
+
+def main():
+  wait_for_modem("AT+CGPS?")
+
+  cmds = [
+    "AT+GPSPORT=1",
+    "AT+CGPS=1",
+  ]
+  for c in cmds:
+    at_cmd(c)
+
+  nmea = defaultdict(list)
+  pm = messaging.PubMaster(['gpsLocation'])
+  while True:
+    time.sleep(1)
+    try:
+      # TODO: read from streaming AT port instead of polling
+      out = at_cmd("AT+CGPS?")
+
+      sentences = out.split("'")[1].splitlines()
+      new = {l.split(',')[0]: l.split(',') for l in sentences if l.startswith('$G')}
+      nmea.update(new)
+      if '$GNRMC' not in new:
+        print(f"no GNRMC:\n{out}\n")
+        continue
+
+      # validate checksums
+      for s in nmea.values():
+        sent = ','.join(s)
+        if checksum(sent) != s[-1].split('*')[1]:
+          cloudlog.error(f"invalid checksum: {repr(sent)}")
+          continue
+
+      gnrmc = nmea['$GNRMC']
+      #print(gnrmc)
+
+      msg = messaging.new_message('gpsLocation', valid=True)
+      gps = msg.gpsLocation
+      gps.latitude = (sfloat(gnrmc[3][:2]) + (sfloat(gnrmc[3][2:]) / 60)) * (1 if gnrmc[4] == "N" else -2)
+      gps.longitude = (sfloat(gnrmc[5][:3]) + (sfloat(gnrmc[5][3:]) / 60)) * (1 if gnrmc[6] == "E" else -1)
+
+      date = gnrmc[9][:6]
+      dt = datetime.datetime.strptime(f"{date} {gnrmc[1]}", '%d%m%y %H%M%S.%f')
+      gps.unixTimestampMillis = dt.timestamp()*1e3
+
+      gps.flags = 1 if gnrmc[1] == 'A' else 0
+
+      # TODO: make our own source
+      gps.source = log.GpsLocationData.SensorSource.qcomdiag
+
+      gps.speed = sfloat(gnrmc[7])
+      gps.bearingDeg = sfloat(gnrmc[8])
+
+      if len(nmea['$GNGGA']):
+        gngga = nmea['$GNGGA']
+        if gngga[10] == 'M':
+          gps.altitude = sfloat(gngga[9])
+
+      if len(nmea['$GNGSA']):
+        # TODO: this is only for GPS sats
+        gngsa = nmea['$GNGSA']
+        gps.horizontalAccuracy = sfloat(gngsa[4])
+        gps.verticalAccuracy = sfloat(gngsa[5])
+
+      # TODO: set these from the module
+      gps.bearingAccuracyDeg = 5.
+      gps.speedAccuracy = 3.
+
+      # TODO: can we get this from the NMEA sentences?
+      #gps.vNED = vNED
+
+      pm.send('gpsLocation', msg)
+
+    except Exception:
+      cloudlog.exception("gps.issue")
+
+
+if __name__ == "__main__":
+  main()

system/qcomgpsd/qcomgpsd.py

@@ -205,10 +205,10 @@ def teardown_quectel(diag):
   try_setup_logs(diag, [])
 
 
-def wait_for_modem():
+def wait_for_modem(cmd="AT+QGPS?"):
   cloudlog.warning("waiting for modem to come up")
   while True:
-    ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
+    ret = subprocess.call(f"mmcli -m any --timeout 10 --command=\"{cmd}\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
     if ret == 0:
       return
     time.sleep(0.1)

tools/replay/route.cc

@@ -18,7 +18,7 @@ Route::Route(const QString &route, const QString &data_dir) : data_dir_(data_dir
 }
 
 RouteIdentifier Route::parseRoute(const QString &str) {
-  QRegExp rx(R"(^(?:([a-z0-9]{16})([|_/]))?(\d{4}-\d{2}-\d{2}--\d{2}-\d{2}-\d{2})(?:(--|/)(\d*))?$)");
+  QRegExp rx(R"(^(?:([a-z0-9]{16})([|_/]))?(.{20})(?:(--|/)(\d*))?$)");
   if (rx.indexIn(str) == -1) return {};
 
   const QStringList list = rx.capturedTexts();