From 5d710ecf5720fcdf507557b6925a72dd63cc3e66 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 7 Mar 2024 18:30:31 -0500 Subject: [PATCH 1/7] Subaru: log eyesight faults as acc faults (#31716) * log cruise fault * better comment * spacing * backwards * moved * copy the other one * localized Co-authored-by: Shane Smiskol --------- Co-authored-by: Shane Smiskol --- selfdrive/car/subaru/carstate.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ebf0ca9062..821ff2c151 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/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"] From fbe6b2c73ba854de7f73ff99f0d1502cfeb9ef70 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 7 Mar 2024 16:40:13 -0800 Subject: [PATCH 2/7] cgpsd (#31781) * cgpsd * latlong is good * more sentences * little more * cleanup --------- Co-authored-by: Comma Device --- system/qcomgpsd/cgpsd.py | 102 ++++++++++++++++++++++++++++++++++++ system/qcomgpsd/qcomgpsd.py | 4 +- 2 files changed, 104 insertions(+), 2 deletions(-) create mode 100755 system/qcomgpsd/cgpsd.py diff --git a/system/qcomgpsd/cgpsd.py b/system/qcomgpsd/cgpsd.py new file mode 100755 index 0000000000..8c802ef4ef --- /dev/null +++ b/system/qcomgpsd/cgpsd.py @@ -0,0 +1,102 @@ +#!/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 + +""" +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 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 + + 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(nmea['$GNGGA'][9]) + + # TODO: set these from the module + gps.horizontalAccuracy = 3. + gps.verticalAccuracy = 3. + 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() diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index e8c407a627..25b547cf5e 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/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) From 6de71bcddb83510cb48125f4e0440d4328c393a4 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 7 Mar 2024 17:02:11 -0800 Subject: [PATCH 3/7] better comment --- .vscode/settings.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index a7c9b2ac44..811306f399 100644 --- a/.vscode/settings.json +++ b/.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/**", ] -} \ No newline at end of file +} From 428397a18ba734f5ec73c18cfe35d16e2c0b040f Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 7 Mar 2024 17:24:29 -0800 Subject: [PATCH 4/7] cgpsd: check checksums and log some accuracies (#31784) * check checksum * some accuracy --------- Co-authored-by: Comma Device --- system/qcomgpsd/cgpsd.py | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/system/qcomgpsd/cgpsd.py b/system/qcomgpsd/cgpsd.py index 8c802ef4ef..c0edc721fa 100755 --- a/system/qcomgpsd/cgpsd.py +++ b/system/qcomgpsd/cgpsd.py @@ -8,6 +8,7 @@ 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' @@ -32,6 +33,11 @@ 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?") @@ -58,6 +64,13 @@ def main(): 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) @@ -81,11 +94,15 @@ def main(): if len(nmea['$GNGGA']): gngga = nmea['$GNGGA'] if gngga[10] == 'M': - gps.altitude = sfloat(nmea['$GNGGA'][9]) + 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.horizontalAccuracy = 3. - gps.verticalAccuracy = 3. gps.bearingAccuracyDeg = 5. gps.speedAccuracy = 3. From 90442e35971b273d36832bf90188dd010a32507e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 7 Mar 2024 18:57:15 -0800 Subject: [PATCH 5/7] Subaru: make OBD query logging (#31785) * make OBD query logging * Update selfdrive/car/subaru/values.py Co-authored-by: Justin Newberry * wording --------- Co-authored-by: Justin Newberry --- selfdrive/car/subaru/values.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 77b30f89be..a4eb5ba7f9 100644 --- a/selfdrive/car/subaru/values.py +++ b/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], From fd51bfb27bbdd089833810bec49e67e4ce7a1800 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 7 Mar 2024 18:57:27 -0800 Subject: [PATCH 6/7] tools: update replay route parsing for timeless format --- tools/replay/route.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replay/route.cc b/tools/replay/route.cc index d0ddf7f3c8..d5847a94b8 100644 --- a/tools/replay/route.cc +++ b/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(); From 158e36976b8793610ac1f39f51d82f5eeeb099b2 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 7 Mar 2024 19:22:06 -0800 Subject: [PATCH 7/7] fix old route sorting (#31787) * fix old route sorting * cleanup * Update system/loggerd/uploader.py --------- Co-authored-by: Comma Device --- system/loggerd/uploader.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 33ee8c1850..832a227798 100755 --- a/system/loggerd/uploader.py +++ b/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):