diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml index 0a55843cfd..8b5b165536 100644 --- a/.github/workflows/docs.yaml +++ b/.github/workflows/docs.yaml @@ -54,6 +54,7 @@ jobs: cp -r ../build/docs/html/ docs/ touch docs/.nojekyll + echo -n docs.comma.ai > docs/CNAME git add -f . git commit -m "build docs" diff --git a/README.md b/README.md index 523f459f3b..7d051ada14 100644 --- a/README.md +++ b/README.md @@ -109,7 +109,6 @@ Directory Structure ├── third_party # External libraries └── system # Generic services ├── camerad # Driver to capture images from the camera sensors - ├── clocksd # Broadcasts current time ├── hardware # Hardware abstraction classes ├── logcatd # systemd journal as a service ├── loggerd # Logger and uploader of car data diff --git a/RELEASES.md b/RELEASES.md index 6adc58197b..96028a9787 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -2,6 +2,7 @@ Version 0.9.5 (2023-XX-XX) ======================== * New driving model * Improved navigate on openpilot performance using navigation instructions as an additional model input +* Cadillac Escalade ESV 2019 support thanks to twilsonco! * Hyundai Azera 2022 support thanks to sunnyhaibin! * Hyundai Azera Hybrid 2020 support thanks to chanhojung and haram-KONA! * Hyundai Custin 2023 support thanks to sunnyhaibin and Saber422! diff --git a/SConstruct b/SConstruct index a086f8448a..c0f1a070b2 100644 --- a/SConstruct +++ b/SConstruct @@ -382,7 +382,6 @@ SConscript(['rednose/SConscript']) # Build system services SConscript([ - 'system/clocksd/SConscript', 'system/proclogd/SConscript', 'system/ubloxd/SConscript', 'system/loggerd/SConscript', diff --git a/docs/CARS.md b/docs/CARS.md index fec2f8b840..d6961f6f56 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,7 +4,7 @@ A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. -# 265 Supported Cars +# 267 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
 |Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -20,6 +20,7 @@ A supported vehicle is one that just works when you install a comma device. All |Buick|LaCrosse 2017-19[4](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-II connector
- 1 comma 3X
- 2 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Cadillac|Escalade 2017[4](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-II connector
- 1 comma 3X
- 2 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Cadillac|Escalade ESV 2016[4](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-II connector
- 1 comma 3X
- 2 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Cadillac|Escalade ESV 2019[4](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-II connector
- 1 comma 3X
- 2 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| @@ -74,7 +75,8 @@ A supported vehicle is one that just works when you install a comma device. All |Hyundai|Azera 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Azera Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Custin 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| -|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/release/files_common b/release/files_common index 05bd463463..1e16cb9439 100644 --- a/release/files_common +++ b/release/files_common @@ -132,10 +132,6 @@ selfdrive/car/tesla/*.py selfdrive/car/toyota/*.py selfdrive/car/volkswagen/*.py -system/clocksd/.gitignore -system/clocksd/SConscript -system/clocksd/clocksd.cc - selfdrive/debug/can_printer.py selfdrive/debug/check_freq.py selfdrive/debug/dump.py @@ -230,7 +226,6 @@ system/ubloxd/*.cc selfdrive/locationd/__init__.py selfdrive/locationd/SConscript selfdrive/locationd/.gitignore -selfdrive/locationd/laikad.py selfdrive/locationd/locationd.h selfdrive/locationd/locationd.cc selfdrive/locationd/paramsd.py diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 9b5cae0961..a0defb7cfb 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -198,17 +198,22 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.5 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.ESCALADE_ESV: + elif candidate in (CAR.ESCALADE_ESV, CAR.ESCALADE_ESV_2019): ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 2739. ret.wheelbase = 3.302 ret.steerRatio = 17.3 ret.centerToFront = ret.wheelbase * 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]] - ret.lateralTuning.pid.kf = 0.000045 ret.tireStiffnessFactor = 1.0 + if candidate == CAR.ESCALADE_ESV: + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]] + ret.lateralTuning.pid.kf = 0.000045 + else: + ret.steerActuatorDelay = 0.2 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.BOLT_EUV: ret.mass = 1669. ret.wheelbase = 2.63779 diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 837d664dad..e9ec8de9c5 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -71,6 +71,7 @@ class CAR(StrEnum): BUICK_REGAL = "BUICK REGAL ESSENCE 2018" ESCALADE = "CADILLAC ESCALADE 2017" ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" + ESCALADE_ESV_2019 = "CADILLAC ESCALADE ESV 2019" BOLT_EUV = "CHEVROLET BOLT EUV 2022" SILVERADO = "CHEVROLET SILVERADO 1500 2020" EQUINOX = "CHEVROLET EQUINOX 2019" @@ -106,6 +107,7 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"), CAR.ESCALADE: GMCarInfo("Cadillac Escalade 2017", "Driver Assist Package"), CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"), + CAR.ESCALADE_ESV_2019: GMCarInfo("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS"), CAR.BOLT_EUV: [ GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"), GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), @@ -198,6 +200,10 @@ FINGERPRINTS = { { 309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8 }], + CAR.ESCALADE_ESV_2019: [ + { + 715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7 + }], CAR.BOLT_EUV: [ { 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 8ccc24d105..26b3491358 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -3,8 +3,8 @@ from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \ - EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, UNSUPPORTED_LONGITUDINAL_CAR, \ - Buttons + CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \ + UNSUPPORTED_LONGITUDINAL_CAR, Buttons from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -88,7 +88,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.90 ret.steerRatio = 15.6 * 1.15 ret.tireStiffnessFactor = 0.63 - elif candidate == CAR.ELANTRA: + elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): ret.mass = 1275. ret.wheelbase = 2.7 ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 @@ -260,7 +260,8 @@ class CarInterface(CarInterfaceBase): if candidate in CANFD_CAR: ret.longitudinalTuning.kpV = [0.1] ret.longitudinalTuning.kiV = [0.0] - ret.experimentalLongitudinalAvailable = candidate in (HYBRID_CAR | EV_CAR) and candidate not in CANFD_RADAR_SCC_CAR + ret.experimentalLongitudinalAvailable = (candidate in (HYBRID_CAR | EV_CAR) and candidate not in + (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)) else: ret.longitudinalTuning.kpV = [0.5] ret.longitudinalTuning.kiV = [0.0] diff --git a/selfdrive/car/hyundai/tests/test_hyundai.py b/selfdrive/car/hyundai/tests/test_hyundai.py index 5fa92624af..dbf2b38615 100755 --- a/selfdrive/car/hyundai/tests/test_hyundai.py +++ b/selfdrive/car/hyundai/tests/test_hyundai.py @@ -23,6 +23,7 @@ NO_DATES_PLATFORMS = { CAR.TUCSON_HYBRID_4TH_GEN, # CAN CAR.ELANTRA, + CAR.ELANTRA_GT_I30, CAR.KIA_CEED, CAR.KIA_FORTE, CAR.KIA_OPTIMA_G4, @@ -99,7 +100,7 @@ class TestHyundaiFingerprint(unittest.TestCase): def test_platform_code_ecus_available(self): # TODO: add queries for these non-CAN FD cars to get EPS no_eps_platforms = CANFD_CAR | {CAR.KIA_SORENTO, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, - CAR.KIA_OPTIMA_H_G4_FL, CAR.SONATA_LF, CAR.TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80} + CAR.KIA_OPTIMA_H_G4_FL, CAR.SONATA_LF, CAR.TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80, CAR.ELANTRA} # Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms for car_model, ecus in FW_VERSIONS.items(): diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 69f475e466..209153c4a4 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -37,7 +37,7 @@ class CarControllerParams: # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. - elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.IONIQ, + elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO): self.STEER_MAX = 255 @@ -72,6 +72,7 @@ class CAR(StrEnum): AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN" AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN" ELANTRA = "HYUNDAI ELANTRA 2017" + ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" ELANTRA_2021 = "HYUNDAI ELANTRA 2021" ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021" HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" @@ -160,7 +161,11 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), CAR.AZERA_HEV_6TH_GEN: HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), CAR.ELANTRA: [ - HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), + # TODO: 2017-18 could be Hyundai G + HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), + HyundaiCarInfo("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), + ], + CAR.ELANTRA_GT_I30: [ HyundaiCarInfo("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), HyundaiCarInfo("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), ], @@ -298,9 +303,6 @@ class Buttons: CANCEL = 4 # on newer models, this is a pause/resume button FINGERPRINTS = { - CAR.ELANTRA: [{ - 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], CAR.HYUNDAI_GENESIS: [{ 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 }, @@ -1702,6 +1704,33 @@ FW_VERSIONS = { ], }, CAR.ELANTRA: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00ADP LKAS AT USA LHD 1.00 1.03 99211-F2000 X31', + b'\xf1\x00AD LKAS AT USA LHD 1.01 1.01 95895-F2000 251', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD(\xfcA\x9d', + b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD\x00\x00\x00\x00', + b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\x00\x00\x00\x00', + b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\xc5\x92\x9e\x8a', + b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20SS2.~\x90\x87', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8161698051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x8161657051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816165D051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816165E051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00AD ESC \x11 11 \x18\x05\x06 58910-F2840', + b'\xf1\x00AD ESC \x11 12 \x15\t\t 58920-F2810', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AD__ SCC H-CUP 1.00 1.00 99110-F2100 ', + b'\xf1\x00AD__ SCC H-CUP 1.00 1.01 96400-F2100 ', + ], + }, + CAR.ELANTRA_GT_I30: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00PD LKAS AT USA LHD 1.01 1.01 95740-G3100 A54', b'\xf1\x00PD LKAS AT KOR LHD 1.00 1.02 95740-G3000 A51', @@ -2065,7 +2094,7 @@ CHECKSUM = { CAN_GEARS = { # which message has the gear. hybrid and EV use ELECT_GEAR - "use_cluster_gears": {CAR.ELANTRA, CAR.KONA}, + "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA}, "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, } @@ -2076,7 +2105,11 @@ CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.TUCS # The radar does SCC on these cars when HDA I, rather than the camera CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80, - CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KONA_EV_2ND_GEN, CAR.IONIQ_6} + CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN} + +# These CAN FD cars do not accept communication control to disable the ADAS ECU, +# responds with 0x7F2822 - 'conditions not correct' +CANFD_UNSUPPORTED_LONGITUDINAL_CAR = {CAR.IONIQ_6, CAR.KONA_EV_2ND_GEN} # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } @@ -2094,7 +2127,7 @@ EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR # these cars require a special panda safety mode due to missing counters and checksums in the messages LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.KIA_OPTIMA_G4, CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022, - CAR.KIA_OPTIMA_H} + CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} # these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc. UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4_FL, @@ -2106,6 +2139,7 @@ DBC = { CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None), CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None), CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), + CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None), CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None), CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None), CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None), diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 298e2bfec4..da5d760dfe 100755 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -25,7 +25,6 @@ non_tested_cars = [ HONDA.ODYSSEY_CHN, VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter TOYOTA.RAV4_TSS2_2023, - TOYOTA.RAV4H_TSS2_2023, SUBARU.FORESTER_HYBRID, ] @@ -61,6 +60,7 @@ routes = [ CarTestRoute("75a6bcb9b8b40373|2023-03-11--22-47-33", GM.BUICK_LACROSSE), CarTestRoute("ef8f2185104d862e|2023-02-09--18-37-13", GM.ESCALADE), CarTestRoute("46460f0da08e621e|2021-10-26--07-21-46", GM.ESCALADE_ESV), + CarTestRoute("168f8b3be57f66ae|2023-09-12--21-44-42", GM.ESCALADE_ESV_2019), CarTestRoute("c950e28c26b5b168|2018-05-30--22-03-41", GM.VOLT), CarTestRoute("f08912a233c1584f|2022-08-11--18-02-41", GM.BOLT_EUV, segment=1), CarTestRoute("555d4087cf86aa91|2022-12-02--12-15-07", GM.BOLT_EUV, segment=14), # Bolt EV @@ -159,7 +159,7 @@ routes = [ CarTestRoute("50a2212c41f65c7b|2021-05-24--16-22-06", HYUNDAI.KIA_FORTE), CarTestRoute("192283cdbb7a58c2|2022-10-15--01-43-18", HYUNDAI.KIA_SPORTAGE_5TH_GEN), CarTestRoute("c5ac319aa9583f83|2021-06-01--18-18-31", HYUNDAI.ELANTRA), - CarTestRoute("734ef96182ddf940|2022-10-02--16-41-44", HYUNDAI.ELANTRA), # 2019 Elantra GT + CarTestRoute("734ef96182ddf940|2022-10-02--16-41-44", HYUNDAI.ELANTRA_GT_I30), CarTestRoute("82e9cdd3f43bf83e|2021-05-15--02-42-51", HYUNDAI.ELANTRA_2021), CarTestRoute("715ac05b594e9c59|2021-06-20--16-21-07", HYUNDAI.ELANTRA_HEV_2021), CarTestRoute("7120aa90bbc3add7|2021-08-02--07-12-31", HYUNDAI.SONATA_HYBRID), @@ -167,17 +167,17 @@ routes = [ CarTestRoute("6b0d44d22df18134|2023-05-06--10-36-55", HYUNDAI.GENESIS_GV80), CarTestRoute("00c829b1b7613dea|2021-06-24--09-10-10", TOYOTA.ALPHARD_TSS2), - CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.ALPHARDH_TSS2), + CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.ALPHARD_TSS2), # hybrid CarTestRoute("000cf3730200c71c|2021-05-24--10-42-05", TOYOTA.AVALON), CarTestRoute("0bb588106852abb7|2021-05-26--12-22-01", TOYOTA.AVALON_2019), - CarTestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.AVALONH_2019), + CarTestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.AVALON_2019), # hybrid CarTestRoute("e9966711cfb04ce3|2022-01-11--07-59-43", TOYOTA.AVALON_TSS2), - CarTestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.AVALONH_TSS2), + CarTestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.AVALON_TSS2), # hybrid CarTestRoute("6cdecc4728d4af37|2020-02-23--15-44-18", TOYOTA.CAMRY), CarTestRoute("2f37c007683e85ba|2023-09-02--14-39-44", TOYOTA.CAMRY), # openpilot longitudinal, with radar CAN filter + CarTestRoute("54034823d30962f5|2021-05-24--06-37-34", TOYOTA.CAMRY), # hybrid CarTestRoute("3456ad0cd7281b24|2020-12-13--17-45-56", TOYOTA.CAMRY_TSS2), - CarTestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.CAMRYH_TSS2), - CarTestRoute("54034823d30962f5|2021-05-24--06-37-34", TOYOTA.CAMRYH), + CarTestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.CAMRY_TSS2), # hybrid CarTestRoute("4e45c89c38e8ec4d|2021-05-02--02-49-28", TOYOTA.COROLLA), CarTestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.COROLLA_TSS2), CarTestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.COROLLA_TSS2), # hybrid @@ -186,7 +186,7 @@ routes = [ CarTestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2), CarTestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.RAV4_TSS2_2022), CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4_TSS2), # hybrid - CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022), + CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4_TSS2_2022), # hybrid CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES), CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), CarTestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ES_TSS2), # hybrid @@ -202,7 +202,7 @@ routes = [ CarTestRoute("3fd5305f8b6ca765|2021-04-28--19-26-49", TOYOTA.LEXUS_NX_TSS2), CarTestRoute("09ae96064ed85a14|2022-06-09--12-22-31", TOYOTA.LEXUS_NX_TSS2), # hybrid CarTestRoute("0a302ffddbb3e3d3|2020-02-08--16-19-08", TOYOTA.HIGHLANDER_TSS2), - CarTestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.HIGHLANDERH_TSS2), + CarTestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.HIGHLANDER_TSS2), # hybrid CarTestRoute("3183cd9b021e89ce|2021-05-25--10-34-44", TOYOTA.HIGHLANDER), CarTestRoute("80d16a262e33d57f|2021-05-23--20-01-43", TOYOTA.HIGHLANDERH), CarTestRoute("eb6acd681135480d|2019-06-20--20-00-00", TOYOTA.SIENNA), @@ -214,8 +214,8 @@ routes = [ CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.CHR), # hybrid CarTestRoute("ea8fbe72b96a185c|2023-02-08--15-11-46", TOYOTA.CHR_TSS2), CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.CHR_TSS2), # openpilot longitudinal, with smartDSU - CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.CHRH_TSS2), - # CarTestRoute("6719965b0e1d1737|2023-08-29--06-40-05", TOYOTA.CHRH_TSS2), # openpilot longitudinal, radar disabled + CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.CHR_TSS2), # hybrid + # CarTestRoute("6719965b0e1d1737|2023-08-29--06-40-05", TOYOTA.CHR_TSS2), # hybrid, openpilot longitudinal, radar disabled CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.PRIUS_V), CarTestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.ARTEON_MK1), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 787b5d6316..5fa1a02897 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -133,7 +133,7 @@ class TestCarInterfaces(unittest.TestCase): # Make sure we can combine dicts ret = get_interface_attr('DBC', combine_brands=True) - self.assertGreaterEqual(len(ret), 170) + self.assertGreaterEqual(len(ret), 160) # We don't support combining non-dicts ret = get_interface_attr('CAR', combine_brands=True) diff --git a/selfdrive/car/tests/test_models_segs.txt b/selfdrive/car/tests/test_models_segs.txt index d838c14765..27a2104cf7 100644 --- a/selfdrive/car/tests/test_models_segs.txt +++ b/selfdrive/car/tests/test_models_segs.txt @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2e57357c95937475fe92c13709ff2ee067f036643d5d455383bd9c3335525614 -size 126281 +oid sha256:8674d9a8304d501aee2bfaea2d668d746fd424b5b0872b83ab265d15d3a3d887 +size 125301 diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index b8c9299685..29e616b102 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -14,7 +14,6 @@ SUBARU ASCENT 2023: [.nan, 3.0, .nan] # Toyota LTA also has torque TOYOTA RAV4 2023: [.nan, 3.0, .nan] -TOYOTA RAV4 HYBRID 2023: [.nan, 3.0, .nan] # Tesla has high torque TESLA AP1 MODEL S: [.nan, 2.5, .nan] @@ -37,6 +36,7 @@ RAM 1500 5TH GEN: [2.0, 2.0, 0.05] RAM HD 5TH GEN: [1.4, 1.4, 0.05] SUBARU OUTBACK 6TH GEN: [2.0, 2.0, 0.2] CADILLAC ESCALADE 2017: [1.899999976158142, 1.842270016670227, 0.1120000034570694] +CADILLAC ESCALADE ESV 2019: [1.15, 1.3, 0.2] CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05] CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112] CHEVROLET TRAILBLAZER 2021: [1.33, 1.9, 0.16] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index 86984525e9..2d2dbc3f0b 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -61,16 +61,13 @@ TOYOTA AVALON 2019: [1.7036141952825095, 1.239619084240008, 0.08459830394899492] TOYOTA AVALON 2022: [2.3154403649717357, 2.7777922854327124, 0.11453999639164605] TOYOTA C-HR 2018: [1.5591084333664578, 1.271271459066948, 0.20259087058453193] TOYOTA C-HR 2021: [1.7678810166088303, 1.3742176337919942, 0.2319674583741509] -TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.105192506] -TOYOTA CAMRY 2021: [2.446083, 2.3476510120007434, 0.121615] -TOYOTA CAMRY HYBRID 2018: [1.996333, 1.7996193116697359, 0.112565] -TOYOTA CAMRY HYBRID 2021: [2.263582, 2.3901492458927986, 0.115257] +TOYOTA CAMRY 2018: [2.0568162685952505, 1.7576185169559122, 0.108878753] +TOYOTA CAMRY 2021: [2.3548324999999997, 2.368900128946771, 0.118436] TOYOTA COROLLA 2017: [3.117154369115421, 1.8438132575043773, 0.12289685869250652] TOYOTA COROLLA TSS2 2019: [1.991132339206426, 1.868866242720403, 0.19570063298031432] TOYOTA HIGHLANDER 2017: [1.8696367437248915, 1.626293990451463, 0.17485372210240796] -TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054457] +TOYOTA HIGHLANDER 2020: [1.9617570834136164, 1.8611643317268927, 0.14519673256119725] TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600] -TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993] TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565] TOYOTA PRIUS 2017: [1.60, 1.5023147650693636, 0.151515] TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968] @@ -79,8 +76,8 @@ TOYOTA RAV4 2019: [2.279239424615458, 2.087101966779332, 0.13682208413446817] TOYOTA RAV4 2019 8965: [2.3080951748210854, 2.1189367835820603, 0.12942102328134028] TOYOTA RAV4 2019 x02: [2.762293266024922, 2.243615865975329, 0.11113568178327986] TOYOTA RAV4 HYBRID 2017: [1.9796257271652042, 1.7503987331707576, 0.14628860048885406] -TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.112174] -TOYOTA RAV4 HYBRID 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736] +TOYOTA RAV4 2022: [2.241883248393209, 1.9304407208090029, 0.112174] +TOYOTA RAV4 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736] TOYOTA SIENNA 2018: [1.689726, 1.3208264576110418, 0.140456] VOLKSWAGEN ARTEON 1ST GEN: [1.45136518053819, 1.3639364049316804, 0.23806361745695032] VOLKSWAGEN ATLAS 1ST GEN: [1.4677006726964945, 1.6733266634075656, 0.12959584092073367] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index 9decbbfa07..c7a1566b32 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -4,18 +4,13 @@ MAZDA CX-5: MAZDA CX-9 2021 MAZDA CX-5 2022: MAZDA CX-9 2021 MAZDA CX-9: MAZDA CX-9 2021 -TOYOTA ALPHARD HYBRID 2021 : TOYOTA SIENNA 2018 TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018 TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017 -TOYOTA RAV4 2022: TOYOTA RAV4 HYBRID 2022 -TOYOTA C-HR HYBRID 2022: TOYOTA C-HR 2021 LEXUS IS 2018: LEXUS NX 2018 LEXUS CT HYBRID 2018 : LEXUS NX 2018 -LEXUS ES 2018: TOYOTA CAMRY HYBRID 2018 -LEXUS ES HYBRID 2018: TOYOTA CAMRY HYBRID 2018 +LEXUS ES 2018: TOYOTA CAMRY 2018 +LEXUS ES HYBRID 2018: TOYOTA CAMRY 2018 LEXUS RC 2020: LEXUS NX 2020 -TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019 -TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022 KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020 KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020 @@ -34,6 +29,7 @@ HYUNDAI IONIQ HYBRID 2017-2019: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019 +HYUNDAI I30 N LINE 2019 & GT 2018 DCT: HYUNDAI SONATA 2019 HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020 HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019 HYUNDAI TUCSON 4TH GEN: HYUNDAI TUCSON HYBRID 4TH GEN diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8cc1a95e2e..42ab5cf257 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -85,28 +85,28 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFactor = 0.5533 ret.mass = 4481. * CV.LB_TO_KG # mean between min and max - elif candidate in (CAR.CHR, CAR.CHR_TSS2, CAR.CHRH_TSS2): + elif candidate in (CAR.CHR, CAR.CHR_TSS2): stop_and_go = True ret.wheelbase = 2.63906 ret.steerRatio = 13.6 ret.tireStiffnessFactor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG - elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): + elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2): stop_and_go = True ret.wheelbase = 2.82448 ret.steerRatio = 13.7 ret.tireStiffnessFactor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG # mean between normal and hybrid - elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): + elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2): stop_and_go = True ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in ret.steerRatio = 16.0 ret.tireStiffnessFactor = 0.8 ret.mass = 4516. * CV.LB_TO_KG # mean between normal and hybrid - elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2): + elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALON_TSS2): # starting from 2019, all Avalon variants have stop and go # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf stop_and_go = candidate != CAR.AVALON @@ -115,8 +115,7 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFactor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG # mean between normal and hybrid - elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2022, - CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023): + elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023): ret.wheelbase = 2.68986 ret.steerRatio = 14.3 ret.tireStiffnessFactor = 0.7933 @@ -191,7 +190,7 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFactor = 0.8 ret.mass = 4300. * CV.LB_TO_KG - elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2): + elif candidate == CAR.ALPHARD_TSS2: ret.wheelbase = 3.00 ret.steerRatio = 14.2 ret.tireStiffnessFactor = 0.444 diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index ca05743550..f8fccb8d5b 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -49,26 +49,19 @@ class ToyotaFlags(IntFlag): class CAR(StrEnum): # Toyota ALPHARD_TSS2 = "TOYOTA ALPHARD 2020" - ALPHARDH_TSS2 = "TOYOTA ALPHARD HYBRID 2021" AVALON = "TOYOTA AVALON 2016" AVALON_2019 = "TOYOTA AVALON 2019" - AVALONH_2019 = "TOYOTA AVALON HYBRID 2019" AVALON_TSS2 = "TOYOTA AVALON 2022" # TSS 2.5 - AVALONH_TSS2 = "TOYOTA AVALON HYBRID 2022" CAMRY = "TOYOTA CAMRY 2018" - CAMRYH = "TOYOTA CAMRY HYBRID 2018" CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 - CAMRYH_TSS2 = "TOYOTA CAMRY HYBRID 2021" CHR = "TOYOTA C-HR 2018" CHR_TSS2 = "TOYOTA C-HR 2021" - CHRH_TSS2 = "TOYOTA C-HR HYBRID 2022" COROLLA = "TOYOTA COROLLA 2017" # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" HIGHLANDER = "TOYOTA HIGHLANDER 2017" HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020" HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" - HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020" PRIUS = "TOYOTA PRIUS 2017" PRIUS_V = "TOYOTA PRIUS v 2017" PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021" @@ -77,8 +70,6 @@ class CAR(StrEnum): RAV4_TSS2 = "TOYOTA RAV4 2019" RAV4_TSS2_2022 = "TOYOTA RAV4 2022" RAV4_TSS2_2023 = "TOYOTA RAV4 2023" - RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022" - RAV4H_TSS2_2023 = "TOYOTA RAV4 HYBRID 2023" MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5 SIENNA = "TOYOTA SIENNA 2018" @@ -111,26 +102,38 @@ class ToyotaCarInfo(CarInfo): CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { # Toyota - CAR.ALPHARD_TSS2: ToyotaCarInfo("Toyota Alphard 2019-20"), - CAR.ALPHARDH_TSS2: ToyotaCarInfo("Toyota Alphard Hybrid 2021"), + CAR.ALPHARD_TSS2: [ + ToyotaCarInfo("Toyota Alphard 2019-20"), + ToyotaCarInfo("Toyota Alphard Hybrid 2021"), + ], CAR.AVALON: [ ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"), ToyotaCarInfo("Toyota Avalon 2017-18"), ], - CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21"), - CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"), - CAR.AVALON_TSS2: ToyotaCarInfo("Toyota Avalon 2022"), - CAR.AVALONH_TSS2: ToyotaCarInfo("Toyota Avalon Hybrid 2022"), - CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), - CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), - CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-23", footnotes=[Footnote.CAMRY]), - CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-24"), + CAR.AVALON_2019: [ + ToyotaCarInfo("Toyota Avalon 2019-21"), + ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"), + ], + CAR.AVALON_TSS2: [ + ToyotaCarInfo("Toyota Avalon 2022"), + ToyotaCarInfo("Toyota Avalon Hybrid 2022"), + ], + CAR.CAMRY: [ + ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), + ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), + ], + CAR.CAMRY_TSS2: [ + ToyotaCarInfo("Toyota Camry 2021-23", footnotes=[Footnote.CAMRY]), + ToyotaCarInfo("Toyota Camry Hybrid 2021-24"), + ], CAR.CHR: [ ToyotaCarInfo("Toyota C-HR 2017-20"), ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"), ], - CAR.CHR_TSS2: ToyotaCarInfo("Toyota C-HR 2021"), - CAR.CHRH_TSS2: ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"), + CAR.CHR_TSS2: [ + ToyotaCarInfo("Toyota C-HR 2021"), + ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"), + ], CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"), CAR.COROLLA_TSS2: [ ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), @@ -143,9 +146,11 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { ToyotaCarInfo("Lexus UX Hybrid 2019-23"), ], CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"), - CAR.HIGHLANDER_TSS2: ToyotaCarInfo("Toyota Highlander 2020-23"), + CAR.HIGHLANDER_TSS2: [ + ToyotaCarInfo("Toyota Highlander 2020-23"), + ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"), + ], CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"), - CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"), CAR.PRIUS: [ ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), @@ -168,10 +173,14 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), ], - CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"), - CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"), - CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), - CAR.RAV4H_TSS2_2023: ToyotaCarInfo("Toyota RAV4 Hybrid 2023"), + CAR.RAV4_TSS2_2022: [ + ToyotaCarInfo("Toyota RAV4 2022"), + ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), + ], + CAR.RAV4_TSS2_2023: [ + ToyotaCarInfo("Toyota RAV4 2023"), + ToyotaCarInfo("Toyota RAV4 Hybrid 2023"), + ], CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"), CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED), @@ -381,7 +390,7 @@ FW_QUERY_CONFIG = FwQueryConfig( ], non_essential_ecus={ # FIXME: On some models, abs can sometimes be missing - Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS], + Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS, CAR.ALPHARD_TSS2], # On some models, the engine can show on two different addresses Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, CAR.LEXUS_RC, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX_TSS2], @@ -453,44 +462,26 @@ FW_VERSIONS = { b'F152607171\x00\x00\x00\x00\x00\x00', b'F152607110\x00\x00\x00\x00\x00\x00', b'F152607180\x00\x00\x00\x00\x00\x00', - ], - (Ecu.dsu, 0x791, None): [ - b'881510703200\x00\x00\x00\x00', - ], - (Ecu.eps, 0x7a1, None): [ - b'8965B41080\x00\x00\x00\x00\x00\x00', - b'8965B07010\x00\x00\x00\x00\x00\x00', - b'8965B41090\x00\x00\x00\x00\x00\x00', - ], - (Ecu.engine, 0x700, None): [ - b'\x01896630725200\x00\x00\x00\x00', - b'\x01896630725300\x00\x00\x00\x00', - b'\x01896630735100\x00\x00\x00\x00', - b'\x01896630738000\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 0xf): [ - b'8821F4702300\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 0x6d): [ - b'8646F0702100\x00\x00\x00\x00', - ], - }, - CAR.AVALONH_2019: { - (Ecu.abs, 0x7b0, None): [ b'F152641040\x00\x00\x00\x00\x00\x00', b'F152641061\x00\x00\x00\x00\x00\x00', b'F152641050\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ + b'881510703200\x00\x00\x00\x00', b'881510704200\x00\x00\x00\x00', b'881514107100\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ + b'8965B41080\x00\x00\x00\x00\x00\x00', b'8965B07010\x00\x00\x00\x00\x00\x00', b'8965B41090\x00\x00\x00\x00\x00\x00', b'8965B41070\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ + b'\x01896630725200\x00\x00\x00\x00', + b'\x01896630725300\x00\x00\x00\x00', + b'\x01896630735100\x00\x00\x00\x00', + b'\x01896630738000\x00\x00\x00\x00', b'\x02896630724000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x02896630737000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x02896630728000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', @@ -507,6 +498,7 @@ FW_VERSIONS = { b'\x01F152607240\x00\x00\x00\x00\x00\x00', b'\x01F152607250\x00\x00\x00\x00\x00\x00', b'\x01F152607280\x00\x00\x00\x00\x00\x00', + b'F152641080\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B41110\x00\x00\x00\x00\x00\x00', @@ -514,24 +506,6 @@ FW_VERSIONS = { (Ecu.engine, 0x700, None): [ b'\x01896630742000\x00\x00\x00\x00', b'\x01896630743000\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 0xf): [ - b'\x018821F6201200\x00\x00\x00\x00', - b'\x018821F6201300\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', - b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', - ], - }, - CAR.AVALONH_TSS2: { - (Ecu.abs, 0x7b0, None): [ - b'F152641080\x00\x00\x00\x00\x00\x00', - ], - (Ecu.eps, 0x7a1, None): [ - b'8965B41110\x00\x00\x00\x00\x00\x00', - ], - (Ecu.engine, 0x700, None): [ b'\x018966306Q6000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -567,65 +541,6 @@ FW_VERSIONS = { b'\x018966333Q6300\x00\x00\x00\x00', b'\x018966333Q6500\x00\x00\x00\x00', b'\x018966333W6000\x00\x00\x00\x00', - ], - (Ecu.engine, 0x7e0, None): [ - b'\x02333P1100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.dsu, 0x791, None): [ - b'8821F0601200 ', - b'8821F0601300 ', - b'8821F0602000 ', - b'8821F0603300 ', - b'8821F0604100 ', - b'8821F0605200 ', - b'8821F0607200 ', - b'8821F0608000 ', - b'8821F0608200 ', - b'8821F0609100 ', - ], - (Ecu.abs, 0x7b0, None): [ - b'F152606210\x00\x00\x00\x00\x00\x00', - b'F152606230\x00\x00\x00\x00\x00\x00', - b'F152606270\x00\x00\x00\x00\x00\x00', - b'F152606290\x00\x00\x00\x00\x00\x00', - b'F152606410\x00\x00\x00\x00\x00\x00', - b'F152633540\x00\x00\x00\x00\x00\x00', - b'F152633A10\x00\x00\x00\x00\x00\x00', - b'F152633A20\x00\x00\x00\x00\x00\x00', - ], - (Ecu.eps, 0x7a1, None): [ - b'8965B33540\x00\x00\x00\x00\x00\x00', - b'8965B33542\x00\x00\x00\x00\x00\x00', - b'8965B33580\x00\x00\x00\x00\x00\x00', - b'8965B33581\x00\x00\x00\x00\x00\x00', - b'8965B33621\x00\x00\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 - b'8821F0601200 ', - b'8821F0601300 ', - b'8821F0602000 ', - b'8821F0603300 ', - b'8821F0604100 ', - b'8821F0605200 ', - b'8821F0607200 ', - b'8821F0608000 ', - b'8821F0608200 ', - b'8821F0609100 ', - ], - (Ecu.fwdCamera, 0x750, 0x6d): [ - b'8646F0601200 ', - b'8646F0601300 ', - b'8646F0601400 ', - b'8646F0603400 ', - b'8646F0604100 ', - b'8646F0605000 ', - b'8646F0606000 ', - b'8646F0606100 ', - b'8646F0607100 ', - ], - }, - CAR.CAMRYH: { - (Ecu.engine, 0x700, None): [ b'\x018966306Q6000\x00\x00\x00\x00', b'\x018966333N1100\x00\x00\x00\x00', b'\x018966333N4300\x00\x00\x00\x00', @@ -648,78 +563,97 @@ FW_VERSIONS = { b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966306S1100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', ], - (Ecu.abs, 0x7b0, None): [ - b'F152633214\x00\x00\x00\x00\x00\x00', - b'F152633660\x00\x00\x00\x00\x00\x00', - b'F152633712\x00\x00\x00\x00\x00\x00', - b'F152633713\x00\x00\x00\x00\x00\x00', - b'F152633B51\x00\x00\x00\x00\x00\x00', - b'F152633B60\x00\x00\x00\x00\x00\x00', + (Ecu.engine, 0x7e0, None): [ + b'\x02333P1100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'8821F0601200 ', b'8821F0601300 ', - b'8821F0603400 ', - b'8821F0604000 ', + b'8821F0602000 ', + b'8821F0603300 ', b'8821F0604100 ', - b'8821F0604200 ', b'8821F0605200 ', - b'8821F0606200 ', b'8821F0607200 ', b'8821F0608000 ', b'8821F0608200 ', - b'8821F0609000 ', b'8821F0609100 ', + b'8821F0603400 ', + b'8821F0604000 ', + b'8821F0604200 ', + b'8821F0606200 ', + b'8821F0609000 ', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152606210\x00\x00\x00\x00\x00\x00', + b'F152606230\x00\x00\x00\x00\x00\x00', + b'F152606270\x00\x00\x00\x00\x00\x00', + b'F152606290\x00\x00\x00\x00\x00\x00', + b'F152606410\x00\x00\x00\x00\x00\x00', + b'F152633540\x00\x00\x00\x00\x00\x00', + b'F152633A10\x00\x00\x00\x00\x00\x00', + b'F152633A20\x00\x00\x00\x00\x00\x00', + b'F152633214\x00\x00\x00\x00\x00\x00', + b'F152633660\x00\x00\x00\x00\x00\x00', + b'F152633712\x00\x00\x00\x00\x00\x00', + b'F152633713\x00\x00\x00\x00\x00\x00', + b'F152633B51\x00\x00\x00\x00\x00\x00', + b'F152633B60\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B33540\x00\x00\x00\x00\x00\x00', b'8965B33542\x00\x00\x00\x00\x00\x00', - b'8965B33550\x00\x00\x00\x00\x00\x00', - b'8965B33551\x00\x00\x00\x00\x00\x00', b'8965B33580\x00\x00\x00\x00\x00\x00', b'8965B33581\x00\x00\x00\x00\x00\x00', - b'8965B33611\x00\x00\x00\x00\x00\x00', b'8965B33621\x00\x00\x00\x00\x00\x00', + b'8965B33550\x00\x00\x00\x00\x00\x00', + b'8965B33551\x00\x00\x00\x00\x00\x00', + b'8965B33611\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 b'8821F0601200 ', b'8821F0601300 ', - b'8821F0603400 ', - b'8821F0604000 ', + b'8821F0602000 ', + b'8821F0603300 ', b'8821F0604100 ', - b'8821F0604200 ', b'8821F0605200 ', - b'8821F0606200 ', b'8821F0607200 ', b'8821F0608000 ', b'8821F0608200 ', - b'8821F0609000 ', b'8821F0609100 ', + b'8821F0603400 ', + b'8821F0604000 ', + b'8821F0604200 ', + b'8821F0606200 ', + b'8821F0609000 ', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F0601200 ', b'8646F0601300 ', b'8646F0601400 ', b'8646F0603400 ', - b'8646F0603500 ', b'8646F0604100 ', b'8646F0605000 ', b'8646F0606000 ', b'8646F0606100 ', - b'8646F0607000 ', b'8646F0607100 ', + b'8646F0603500 ', + b'8646F0607000 ', ], }, CAR.CAMRY_TSS2: { (Ecu.eps, 0x7a1, None): [ b'8965B33630\x00\x00\x00\x00\x00\x00', b'8965B33640\x00\x00\x00\x00\x00\x00', + b'8965B33650\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'\x01F152606370\x00\x00\x00\x00\x00\x00', b'\x01F152606390\x00\x00\x00\x00\x00\x00', b'\x01F152606400\x00\x00\x00\x00\x00\x00', b'\x01F152606431\x00\x00\x00\x00\x00\x00', + b'F152633D00\x00\x00\x00\x00\x00\x00', + b'F152633D60\x00\x00\x00\x00\x00\x00', + b'F152633310\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x018966306Q5000\x00\x00\x00\x00', @@ -730,10 +664,16 @@ FW_VERSIONS = { b'\x018966306T3200\x00\x00\x00\x00', b'\x018966306T4000\x00\x00\x00\x00', b'\x018966306T4100\x00\x00\x00\x00', + b'\x018966306Q6000\x00\x00\x00\x00', + b'\x018966306Q7000\x00\x00\x00\x00', + b'\x018966306T0000\x00\x00\x00\x00', + b'\x018966306V1000\x00\x00\x00\x00', + b'\x01896633T20000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F6201200\x00\x00\x00\x00', b'\x018821F6201300\x00\x00\x00\x00', + b'\x018821F6201400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', @@ -743,35 +683,7 @@ FW_VERSIONS = { b'\x028646F3305200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', - ], - }, - CAR.CAMRYH_TSS2: { - (Ecu.eps, 0x7a1, None): [ - b'8965B33630\x00\x00\x00\x00\x00\x00', - b'8965B33650\x00\x00\x00\x00\x00\x00', - ], - (Ecu.abs, 0x7b0, None): [ - b'F152633D00\x00\x00\x00\x00\x00\x00', - b'F152633D60\x00\x00\x00\x00\x00\x00', - b'F152633310\x00\x00\x00\x00\x00\x00', - ], - (Ecu.engine, 0x700, None): [ - b'\x018966306Q6000\x00\x00\x00\x00', - b'\x018966306Q7000\x00\x00\x00\x00', - b'\x018966306T0000\x00\x00\x00\x00', - b'\x018966306V1000\x00\x00\x00\x00', - b'\x01896633T20000\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 15): [ - b'\x018821F6201200\x00\x00\x00\x00', - b'\x018821F6201300\x00\x00\x00\x00', - b'\x018821F6201400\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 109): [ - b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', - b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', - b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.CHR: { @@ -863,13 +775,19 @@ FW_VERSIONS = { (Ecu.abs, 0x7b0, None): [ b'F152610260\x00\x00\x00\x00\x00\x00', b'F1526F4270\x00\x00\x00\x00\x00\x00', + b'F152610041\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B10091\x00\x00\x00\x00\x00\x00', b'8965B10110\x00\x00\x00\x00\x00\x00', + b'8965B10092\x00\x00\x00\x00\x00\x00', + b'8965B10111\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x0189663F459000\x00\x00\x00\x00', + b'\x0189663F438000\x00\x00\x00\x00', + b'\x02896631025000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F453000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x0331014000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', @@ -877,33 +795,12 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821FF410200\x00\x00\x00\x00', b'\x018821FF410300\x00\x00\x00\x00', + b'\x018821FF410500\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646FF410200\x00\x00\x00\x008646GF408200\x00\x00\x00\x00', b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00', - ], - }, - CAR.CHRH_TSS2: { - (Ecu.eps, 0x7a1, None): [ - b'8965B10092\x00\x00\x00\x00\x00\x00', - b'8965B10091\x00\x00\x00\x00\x00\x00', - b'8965B10111\x00\x00\x00\x00\x00\x00', - ], - (Ecu.abs, 0x7b0, None): [ - b'F152610041\x00\x00\x00\x00\x00\x00', - ], - (Ecu.engine, 0x700, None): [ - b'\x0189663F438000\x00\x00\x00\x00', - b'\x02896631025000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', - b'\x0289663F453000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 15): [ - b'\x018821FF410500\x00\x00\x00\x00', - b'\x018821FF410300\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 109): [ b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00', - b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00', ], }, CAR.COROLLA: { @@ -1189,6 +1086,18 @@ FW_VERSIONS = { b'\x01F15260E110\x00\x00\x00\x00\x00\x00', b'\x01F15260E170\x00\x00\x00\x00\x00\x00', b'\x01F15260E05300\x00\x00\x00\x00', + b'\x01F15264872300\x00\x00\x00\x00', + b'\x01F15264872400\x00\x00\x00\x00', + b'\x01F15264872500\x00\x00\x00\x00', + b'\x01F15264872600\x00\x00\x00\x00', + b'\x01F15264872700\x00\x00\x00\x00', + b'\x01F15264873500\x00\x00\x00\x00', + b'\x01F152648C6300\x00\x00\x00\x00', + b'\x01F152648J4000\x00\x00\x00\x00', + b'\x01F152648J5000\x00\x00\x00\x00', + b'\x01F152648J6000\x00\x00\x00\x00', + b'\x01F152648J7000\x00\x00\x00\x00', + b'\x01F152648L5000\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x01896630E62100\x00\x00\x00\x00', @@ -1209,44 +1118,13 @@ FW_VERSIONS = { b'\x01896630EE1100\x00\x00\x00\x00', b'\x01896630EG3000\x00\x00\x00\x00', b'\x01896630EG5000\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 0xf): [ - b'\x018821F3301400\x00\x00\x00\x00', - b'\x018821F6201200\x00\x00\x00\x00', - b'\x018821F6201300\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', - b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', - b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', - b'\x028646F4803200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', - ], - }, - CAR.HIGHLANDERH_TSS2: { - (Ecu.eps, 0x7a1, None): [ - b'8965B48241\x00\x00\x00\x00\x00\x00', - b'8965B48310\x00\x00\x00\x00\x00\x00', - b'8965B48400\x00\x00\x00\x00\x00\x00', - ], - (Ecu.abs, 0x7b0, None): [ - b'\x01F15264872300\x00\x00\x00\x00', - b'\x01F15264872400\x00\x00\x00\x00', - b'\x01F15264872500\x00\x00\x00\x00', - b'\x01F15264872600\x00\x00\x00\x00', - b'\x01F15264873500\x00\x00\x00\x00', - b'\x01F152648C6300\x00\x00\x00\x00', - b'\x01F152648J4000\x00\x00\x00\x00', - b'\x01F152648J5000\x00\x00\x00\x00', - b'\x01F152648J6000\x00\x00\x00\x00', - b'\x01F15264872700\x00\x00\x00\x00', - ], - (Ecu.engine, 0x700, None): [ b'\x01896630E67000\x00\x00\x00\x00', b'\x01896630EA1000\x00\x00\x00\x00', b'\x01896630EE4000\x00\x00\x00\x00', b'\x01896630EE4100\x00\x00\x00\x00', b'\x01896630EE5000\x00\x00\x00\x00', b'\x01896630EE6000\x00\x00\x00\x00', + b'\x01896630EE7000\x00\x00\x00\x00', b'\x01896630EF8000\x00\x00\x00\x00', b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', @@ -1634,46 +1512,6 @@ FW_VERSIONS = { (Ecu.abs, 0x7b0, None): [ b'\x01F15260R350\x00\x00\x00\x00\x00\x00', b'\x01F15260R361\x00\x00\x00\x00\x00\x00', - ], - (Ecu.eps, 0x7a1, None): [ - b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00', - ], - (Ecu.engine, 0x700, None): [ - b'\x01896634AA0000\x00\x00\x00\x00', - b'\x01896634AA0100\x00\x00\x00\x00', - b'\x01896634AA1000\x00\x00\x00\x00', - b'\x01896634A88000\x00\x00\x00\x00', - b'\x01896634A89000\x00\x00\x00\x00', - b'\x01896634A89100\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 0xf): [ - b'\x018821F0R01100\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', - ], - }, - CAR.RAV4_TSS2_2023: { - (Ecu.abs, 0x7b0, None): [ - b'\x01F15260R450\x00\x00\x00\x00\x00\x00', - ], - (Ecu.eps, 0x7a1, None): [ - b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00', - ], - (Ecu.engine, 0x700, None): [ - b'\x01896634A88100\x00\x00\x00\x00', - b'\x01896634AJ2000\x00\x00\x00\x00', - b'\x01896634A89100\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 0xf): [ - b'\x018821F0R03100\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00', - ], - }, - CAR.RAV4H_TSS2_2022: { - (Ecu.abs, 0x7b0, None): [ b'\x01F15264283100\x00\x00\x00\x00', b'\x01F15264286200\x00\x00\x00\x00', b'\x01F15264286100\x00\x00\x00\x00', @@ -1685,6 +1523,12 @@ FW_VERSIONS = { b'8965B42172\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ + b'\x01896634AA0000\x00\x00\x00\x00', + b'\x01896634AA0100\x00\x00\x00\x00', + b'\x01896634AA1000\x00\x00\x00\x00', + b'\x01896634A88000\x00\x00\x00\x00', + b'\x01896634A89000\x00\x00\x00\x00', + b'\x01896634A89100\x00\x00\x00\x00', b'\x01896634A02001\x00\x00\x00\x00', b'\x01896634A03000\x00\x00\x00\x00', b'\x01896634A08000\x00\x00\x00\x00', @@ -1700,8 +1544,9 @@ FW_VERSIONS = { b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', ], }, - CAR.RAV4H_TSS2_2023: { + CAR.RAV4_TSS2_2023: { (Ecu.abs, 0x7b0, None): [ + b'\x01F15260R450\x00\x00\x00\x00\x00\x00', b'\x01F15264283200\x00\x00\x00\x00', b'\x01F15264283300\x00\x00\x00\x00', b'\x01F152642F1000\x00\x00\x00\x00', @@ -1711,15 +1556,12 @@ FW_VERSIONS = { b'8965B42371\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ + b'\x01896634A88100\x00\x00\x00\x00', + b'\x01896634AJ2000\x00\x00\x00\x00', + b'\x01896634A89100\x00\x00\x00\x00', b'\x01896634AE1001\x00\x00\x00\x00', b'\x01896634AF0000\x00\x00\x00\x00', ], - (Ecu.hybrid, 0x7d2, None): [ - b'\x02899830R39000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00', - b'\x02899830R41000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00', - b'\x028998342C0000\x00\x00\x00\x00899854224000\x00\x00\x00\x00', - b'\x028998342C6000\x00\x00\x00\x00899854224000\x00\x00\x00\x00', - ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R03100\x00\x00\x00\x00', ], @@ -2178,34 +2020,22 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.eps, 0x7a1, None): [ - b'8965B58040\x00\x00\x00\x00\x00\x00', - b'8965B58052\x00\x00\x00\x00\x00\x00', - ], - (Ecu.fwdRadar, 0x750, 0xf): [ - b'\x018821F3301200\x00\x00\x00\x00', - b'\x018821F3301400\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', - b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', - ], - }, - CAR.ALPHARDH_TSS2: { - (Ecu.engine, 0x7e0, None): [ b'\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B58040\x00\x00\x00\x00\x00\x00', + b'8965B58052\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152658341\x00\x00\x00\x00\x00\x00' ], (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301200\x00\x00\x00\x00', b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646FV201000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, @@ -2225,20 +2055,14 @@ DBC = { CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CHR_TSS2: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.CHRH_TSS2: dbc_dict('toyota_nodsu_pt_generated', None), CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CAMRYH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None), @@ -2250,14 +2074,11 @@ DBC = { CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_IS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.RAV4H_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None), CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.ALPHARDH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), } # These cars have non-standard EPS torque scale factors. All others are 73 @@ -2265,20 +2086,19 @@ EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_I # Toyota/Lexus Safety Sense 2.0 and 2.5 TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.LEXUS_ES_TSS2, - CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, - CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.LEXUS_IS_TSS2, CAR.MIRAI, CAR.LEXUS_NX_TSS2, - CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2} + CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.LEXUS_IS_TSS2, + CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.CHR_TSS2} -NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY, CAR.CAMRYH} +NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY} # the DSU uses the AEB message for longitudinal on these cars UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC} # these cars have a radar which sends ACC messages instead of the camera -RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2, CAR.CHRH_TSS2} +RADAR_ACC_CAR = {CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2} # these cars use the Lane Tracing Assist (LTA) message for lateral control -ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023} +ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023} # no resume button press required NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 1c4d090194..6439fb0b6b 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -832,6 +832,7 @@ FW_VERSIONS = { b'\xf1\x8783A907115F \xf1\x890002', b'\xf1\x8783A907115G \xf1\x890001', b'\xf1\x8783A907115K \xf1\x890001', + b'\xf1\x8783A907115K \xf1\x890002', b'\xf1\x8704E906024AP\xf1\x891461', b'\xf1\x8783A907115 \xf1\x890007', ], @@ -841,6 +842,7 @@ FW_VERSIONS = { b'\xf1\x8709G927158GC\xf1\x893821', b'\xf1\x8709G927158GD\xf1\x893820', b'\xf1\x8709G927158GM\xf1\x893936', + b'\xf1\x8709G927158GN\xf1\x893938', b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300011N \xf1\x892001', b'\xf1\x870DL300011N \xf1\x892012', @@ -879,6 +881,7 @@ FW_VERSIONS = { b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60604A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1', + b'\xf1\x875QM907144D \xf1\x891063\xf1\x82\x002RA60A2ROM', b'\xf1\x875QM907144D \xf1\x891063\xf1\x82\x002SA6092SOM', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6017A00', b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60804A1', @@ -1071,11 +1074,13 @@ FW_VERSIONS = { b'\xf1\x8783A906259 \xf1\x890001', b'\xf1\x8783A906259 \xf1\x890005', b'\xf1\x8783A906259C \xf1\x890002', + b'\xf1\x8783A906259D \xf1\x890001', b'\xf1\x8783A906259F \xf1\x890001', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158CN\xf1\x893608', b'\xf1\x8709G927158FL\xf1\x893758', + b'\xf1\x8709G927158GG\xf1\x893825', b'\xf1\x8709G927158GP\xf1\x893937', b'\xf1\x870GC300045D \xf1\x892802', b'\xf1\x870GC300046F \xf1\x892701', @@ -1085,6 +1090,7 @@ FW_VERSIONS = { b'\xf1\x875Q0959655BQ\xf1\x890421\xf1\x82\x132121111121120031112124218C219321532111', b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111224118A119321532111', b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111', + b'\xf1\x875Q0959655BQ\xf1\x890421\xf1\x82\x132121111121120031112124218A219321532111', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300', diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py deleted file mode 100755 index 10c0e4a7b0..0000000000 --- a/selfdrive/locationd/laikad.py +++ /dev/null @@ -1,468 +0,0 @@ -#!/usr/bin/env python3 -import math -import os -import time -import shutil -from collections import defaultdict -from concurrent.futures import Future, ProcessPoolExecutor -from enum import IntEnum -from typing import List, Optional, Dict, Any - -import numpy as np - -from cereal import log, messaging -from openpilot.common.params import Params, put_nonblocking -from laika import AstroDog -from laika.constants import SECS_IN_HR, SECS_IN_MIN -from laika.downloader import DownloadFailed -from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem -from laika.gps_time import GPSTime -from laika.helpers import ConstellationId, get_sv_id -from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox -from laika.raw_gnss import gps_time_from_qcom_report, get_measurements_from_qcom_reports -from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func -from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind -from openpilot.selfdrive.locationd.models.gnss_kf import GNSSKalman -from openpilot.selfdrive.locationd.models.gnss_kf import States as GStates -from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import cloudlog - -MAX_TIME_GAP = 10 -EPHEMERIS_CACHE = 'LaikadEphemerisV3' -CACHE_VERSION = 0.2 -POS_FIX_RESIDUAL_THRESHOLD = 100.0 - - -class LogEphemerisType(IntEnum): - nav = 0 - nasaUltraRapid = 1 - glonassIacUltraRapid = 2 - qcom = 3 - -class EphemerisSource(IntEnum): - gnssChip = 0 - internet = 1 - cache = 2 - unknown = 3 - -def get_log_eph_type(ephem): - if ephem.eph_type == EphemerisType.NAV: - source_type = LogEphemerisType.nav - elif ephem.eph_type == EphemerisType.QCOM_POLY: - source_type = LogEphemerisType.qcom - else: - assert ephem.file_epoch is not None - file_src = ephem.file_source - if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z' - source_type = LogEphemerisType.nasaUltraRapid - elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3' - source_type = LogEphemerisType.glonassIacUltraRapid - else: - raise Exception(f"Didn't expect file source {file_src}") - return source_type - -def get_log_eph_source(ephem): - if ephem.file_name == 'qcom' or ephem.file_name == 'ublox': - source = EphemerisSource.gnssChip - elif ephem.file_name == EPHEMERIS_CACHE: - source = EphemerisSource.cache - else: - source = EphemerisSource.internet - return source - - -class Laikad: - def __init__(self, valid_const=(ConstellationId.GPS, ConstellationId.GLONASS), auto_fetch_navs=True, auto_update=False, - valid_ephem_types=(EphemerisType.NAV, EphemerisType.QCOM_POLY), - save_ephemeris=False, use_qcom=False): - """ - valid_const: GNSS constellation which can be used - auto_fetch_navs: If true fetch navs from internet when needed - auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere. - valid_ephem_types: Valid ephemeris types to be used by AstroDog - save_ephemeris: If true saves and loads nav and orbit ephemeris to cache. - """ - self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, - clear_old_ephemeris=True, cache_dir=Paths.download_cache_root()) - self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom) - - self.auto_fetch_navs = auto_fetch_navs - self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None - self.orbit_fetch_future: Optional[Future] = None - - self.last_report_time = GPSTime(0, 0) - self.last_fetch_navs_t = GPSTime(0, 0) - self.last_cached_t = GPSTime(0, 0) - self.save_ephemeris = save_ephemeris - self.load_cache() - - self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} - self.velfix_function = get_velfix_sympy_func() - self.last_fix_pos = None - self.last_fix_t = None - self.use_qcom = use_qcom - self.first_log_time = None - self.ttff = -1 - self.measurement_lag = 0.630 if self.use_qcom else 0.095 - - # qcom specific stuff - self.qcom_reports_received = 4 - self.qcom_reports = [] - - def load_cache(self): - if not self.save_ephemeris: - return - - cache_bytes = Params().get(EPHEMERIS_CACHE) - if not cache_bytes: - return - - nav_dict = {} - try: - with ephemeris_structs.EphemerisCache.from_bytes(cache_bytes) as ephem_cache: - glonass_navs = [GLONASSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.glonassEphemerides] - gps_navs = [GPSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.gpsEphemerides] - for e in sum([glonass_navs, gps_navs], []): - if e.prn not in nav_dict: - nav_dict[e.prn] = [] - nav_dict[e.prn].append(e) - self.astro_dog.add_navs(nav_dict) - except Exception: - cloudlog.exception("Error parsing cache") - cloudlog.debug( - f"Loaded navs ({sum([len(nav_dict[prn]) for prn in nav_dict.keys()])}). Unique orbit and nav sats: {list(nav_dict.keys())} ") - - def cache_ephemeris(self): - - if self.save_ephemeris and (self.last_report_time - self.last_cached_t > SECS_IN_MIN): - nav_list: List = sum([v for k,v in self.astro_dog.navs.items()], []) - #TODO this only saves currently valid ephems, when we download future ephems we should save them too - valid_navs = [e for e in nav_list if e.valid(self.last_report_time)] - if len(valid_navs) > 0: - ephem_cache = ephemeris_structs.EphemerisCache(glonassEphemerides=[e.data for e in valid_navs if e.prn[0]=='R'], - gpsEphemerides=[e.data for e in valid_navs if e.prn[0]=='G']) - put_nonblocking(EPHEMERIS_CACHE, ephem_cache.to_bytes()) - cloudlog.debug("Cache saved") - self.last_cached_t = self.last_report_time - - def create_ephem_statuses(self): - ephemeris_statuses = [] - eph_list: List = sum([v for k,v in self.astro_dog.navs.items()], []) + sum([v for k,v in self.astro_dog.qcom_polys.items()], []) - for eph in eph_list: - status = log.GnssMeasurements.EphemerisStatus.new_message() - status.constellationId = ConstellationId.from_rinex_char(eph.prn[0]).value - status.svId = get_sv_id(eph.prn) - status.type = get_log_eph_type(eph).value - status.source = get_log_eph_source(eph).value - status.tow = eph.epoch.tow - status.gpsWeek = eph.epoch.week - ephemeris_statuses.append(status) - return ephemeris_statuses - - - def get_lsq_fix(self, t, measurements): - if self.last_fix_t is None or abs(self.last_fix_t - t) > 0: - min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4 - - position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) - if len(position_solution) < 3: - return None - position_estimate = position_solution[:3] - position_std = pos_std[:3] - - velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements) - if len(velocity_solution) < 3: - return None - velocity_estimate = velocity_solution[:3] - velocity_std = vel_std[:3] - - return position_estimate, position_std, velocity_estimate, velocity_std - - def is_good_report(self, gnss_msg): - if gnss_msg.which() in ['drMeasurementReport', 'measurementReport'] and self.use_qcom: - # TODO: Understand and use remaining unknown constellations - try: - if gnss_msg.which() == 'drMeasurementReport': - constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) - else: - constellation_id = ConstellationId.from_qcom_source(gnss_msg.measurementReport.source) - good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS] - report_time = gps_time_from_qcom_report(gnss_msg) - except NotImplementedError: - return False - # Garbage timestamps with week > 32767 are sometimes sent by module. - # This is an issue with gpsTime and GLONASS time. - good_week = report_time.week < np.iinfo(np.int16).max - return good_constellation and good_week - elif gnss_msg.which() == 'measurementReport' and not self.use_qcom: - return True - else: - return False - - def read_report(self, gnss_msg): - if self.use_qcom: - # QCOM reports are per constellation, so we need to aggregate them - # Additionally, the pseudoranges are broken in the measurementReports - # and the doppler filteredSpeed is broken in the drMeasurementReports - report_time = gps_time_from_qcom_report(gnss_msg) - if report_time - self.last_report_time == 0: - self.qcom_reports.append(gnss_msg) - self.last_report_time = report_time - elif report_time - self.last_report_time > 0: - self.qcom_reports_received = max(1, len(self.qcom_reports)) - self.qcom_reports = [gnss_msg] - self.last_report_time = report_time - else: - # Sometimes DR reports get sent one iteration late (1second), they need to be ignored - cloudlog.warning(f"Received report with time {report_time} before last report time {self.last_report_time}") - - if len(self.qcom_reports) == self.qcom_reports_received: - new_meas = get_measurements_from_qcom_reports(self.qcom_reports) - else: - new_meas = [] - else: - report = gnss_msg.measurementReport - self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow) - new_meas = read_raw_ublox(report) - return self.last_report_time, new_meas - - def is_ephemeris(self, gnss_msg): - if self.use_qcom: - return gnss_msg.which() == 'drSvPoly' - else: - return gnss_msg.which() in ('ephemeris', 'glonassEphemeris') - - def read_ephemeris(self, gnss_msg): - if self.use_qcom: - try: - ephem = parse_qcom_ephem(gnss_msg.drSvPoly) - self.astro_dog.add_qcom_polys({ephem.prn: [ephem]}) - except Exception: - cloudlog.exception("Error parsing qcom svPoly ephemeris from qcom module") - return - - else: - if gnss_msg.which() == 'ephemeris': - data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict()) - try: - ephem = GPSEphemeris(data_struct, file_name='ublox') - except Exception: - cloudlog.exception("Error parsing GPS ephemeris from ublox") - return - elif gnss_msg.which() == 'glonassEphemeris': - data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict()) - try: - ephem = GLONASSEphemeris(data_struct, file_name='ublox') - except Exception: - cloudlog.exception("Error parsing GLONASS ephemeris from ublox") - return - else: - cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}") - return - self.astro_dog.add_navs({ephem.prn: [ephem]}) - self.cache_ephemeris() - - def process_report(self, new_meas, t): - # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites - new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7] - processed_measurements = process_measurements(new_meas, self.astro_dog) - if self.last_fix_pos is not None: - est_pos = self.last_fix_pos - correct_delay = True - else: - est_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() - correct_delay = False - corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog, correct_delay=correct_delay) - # If many measurements weren't corrected, position may be garbage, so reset - if len(processed_measurements) >= 8 and len(corrected_measurements) < 5: - cloudlog.error("Didn't correct enough measurements, resetting estimate position") - self.last_fix_pos = None - self.last_fix_t = None - return corrected_measurements - - def calc_fix(self, t, measurements): - instant_fix = self.get_lsq_fix(t, measurements) - if instant_fix is None: - return None - else: - position_estimate, position_std, velocity_estimate, velocity_std = instant_fix - self.last_fix_t = t - self.last_fix_pos = position_estimate - self.lat_fix_pos_std = position_std - return position_estimate, position_std, velocity_estimate, velocity_std - - def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): - out_msg = messaging.new_message("gnssMeasurements") - t = gnss_mono_time * 1e-9 - msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time - int(1e9 * self.measurement_lag)} - if self.first_log_time is None: - self.first_log_time = 1e-9 * gnss_mono_time - if self.is_ephemeris(gnss_msg): - self.read_ephemeris(gnss_msg) - elif self.is_good_report(gnss_msg): - report_t, new_meas = self.read_report(gnss_msg) - if report_t.week > 0: - if self.auto_fetch_navs: - self.fetch_navs(report_t, block) - - corrected_measurements = self.process_report(new_meas, t) - msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements] - - fix = self.calc_fix(t, corrected_measurements) - measurement_msg = log.LiveLocationKalman.Measurement.new_message - if fix is not None: - position_estimate, position_std, velocity_estimate, velocity_std = fix - if self.ttff <= 0: - self.ttff = max(1e-3, t - self.first_log_time) - msg_dict["positionECEF"] = measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t)) - msg_dict["velocityECEF"] = measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)) - - self.update_localizer(self.last_fix_pos, t, corrected_measurements) - P_diag = self.gnss_kf.P.diagonal() - kf_valid = all(self.kf_valid(t)) - msg_dict["kalmanPositionECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(), - std=np.sqrt(P_diag[GStates.ECEF_POS]).tolist(), - valid=kf_valid) - msg_dict["kalmanVelocityECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(), - std=np.sqrt(P_diag[GStates.ECEF_VELOCITY]).tolist(), - valid=kf_valid) - - msg_dict['gpsWeek'] = self.last_report_time.week - msg_dict['gpsTimeOfWeek'] = self.last_report_time.tow - msg_dict['timeToFirstFix'] = self.ttff - msg_dict['ephemerisStatuses'] = self.create_ephem_statuses() - out_msg.gnssMeasurements = msg_dict - return out_msg - - def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): - # Check time and outputs are valid - valid = self.kf_valid(t) - if not all(valid): - if not valid[0]: # Filter not initialized - pass - elif not valid[1]: - cloudlog.error("Time gap of over 10s detected, gnss kalman reset") - elif not valid[2]: - cloudlog.error("Gnss kalman filter state is nan") - if est_pos is not None and len(est_pos) > 0: - cloudlog.info(f"Reset kalman filter with {est_pos}") - self.init_gnss_localizer(est_pos) - else: - return - if len(measurements) > 0: - kf_add_observations(self.gnss_kf, t, measurements) - else: - # Ensure gnss filter is updated even with no new measurements - self.gnss_kf.predict(t) - - def kf_valid(self, t: float) -> List[bool]: - filter_time = self.gnss_kf.filter.get_filter_time() - return [not math.isnan(filter_time), - abs(t - filter_time) < MAX_TIME_GAP, - all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))] - - def init_gnss_localizer(self, est_pos): - x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial)) - x_initial[GStates.ECEF_POS] = est_pos - p_initial_diag[GStates.ECEF_POS] = 1000 ** 2 - self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) - - def fetch_navs(self, t: GPSTime, block): - # Download new navs if 1 hour of navs data left - if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (abs(t - self.last_fetch_navs_t) > SECS_IN_MIN): - astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir - ret = None - - if block: # Used for testing purposes - ret = get_orbit_data(t, *astro_dog_vars) - elif self.orbit_fetch_future is None: - self.orbit_fetch_executor = ProcessPoolExecutor(max_workers=1) - self.orbit_fetch_future = self.orbit_fetch_executor.submit(get_orbit_data, t, *astro_dog_vars) - elif self.orbit_fetch_future.done(): - ret = self.orbit_fetch_future.result() - self.orbit_fetch_executor = self.orbit_fetch_future = None - - if ret is not None: - if ret[0] is None: - self.last_fetch_navs_t = ret[2] - else: - self.astro_dog.navs, self.astro_dog.navs_fetched_times, self.last_fetch_navs_t = ret - self.cache_ephemeris() - - -def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir): - astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, cache_dir=cache_dir) - cloudlog.info(f"Start to download/parse navs for time {t.as_datetime()}") - start_time = time.monotonic() - try: - astro_dog.get_navs(t) - cloudlog.info(f"Done parsing navs. Took {time.monotonic() - start_time:.1f}s") - cloudlog.debug(f"Downloaded navs ({sum([len(v) for v in astro_dog.navs])}): {list(astro_dog.navs.keys())}" + - f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}") - return astro_dog.navs, astro_dog.navs_fetched_times, t - except (DownloadFailed, RuntimeError, ValueError, IOError) as e: - cloudlog.warning(f"No orbit data found or parsing failure: {e}") - return None, None, t - - -def create_measurement_msg(meas: GNSSMeasurement): - c = log.GnssMeasurements.CorrectedMeasurement.new_message() - c.constellationId = meas.constellation_id.value - c.svId = meas.sv_id - c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0 - c.pseudorange = float(meas.observables_final['C1C']) - c.pseudorangeStd = float(meas.observables_std['C1C']) - c.pseudorangeRate = float(meas.observables_final['D1C']) - c.pseudorangeRateStd = float(meas.observables_std['D1C']) - c.satPos = meas.sat_pos_final.tolist() - c.satVel = meas.sat_vel.tolist() - c.satVel = meas.sat_vel.tolist() - return c - -def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]): - ekf_data = defaultdict(list) - for m in measurements: - m_arr = m.as_array() - if m.constellation_id == ConstellationId.GPS: - ekf_data[ObservationKind.PSEUDORANGE_GPS].append(m_arr) - elif m.constellation_id == ConstellationId.GLONASS: - ekf_data[ObservationKind.PSEUDORANGE_GLONASS].append(m_arr) - ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = ekf_data[ObservationKind.PSEUDORANGE_GPS] - ekf_data[ObservationKind.PSEUDORANGE_RATE_GLONASS] = ekf_data[ObservationKind.PSEUDORANGE_GLONASS] - for kind, data in ekf_data.items(): - if len(data) > 0: - gnss_kf.predict_and_observe(t, kind, data) - - -def clear_tmp_cache(): - if os.path.exists(Paths.download_cache_root()): - shutil.rmtree(Paths.download_cache_root()) - os.mkdir(Paths.download_cache_root()) - - -def main(): - #clear_tmp_cache() - - use_qcom = not Params().get_bool("UbloxAvailable") - if use_qcom: - raw_name = "qcomGnss" - else: - raw_name = "ubloxGnss" - raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False) - pm = messaging.PubMaster(['gnssMeasurements']) - - # disable until set as main gps source, to better analyze startup time - # TODO ensure low CPU usage before enabling - use_internet = False # "LAIKAD_NO_INTERNET" not in os.environ - - replay = "REPLAY" in os.environ - laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom) - - while True: - for in_msg in messaging.drain_sock(raw_gnss_sock, wait_for_one=True): - out_msg = laikad.process_gnss_msg(getattr(in_msg, raw_name), in_msg.logMonoTime, replay) - pm.send('gnssMeasurements', out_msg) - - -if __name__ == "__main__": - main() diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py deleted file mode 100755 index db8802ed36..0000000000 --- a/selfdrive/locationd/test/test_laikad.py +++ /dev/null @@ -1,319 +0,0 @@ -#!/usr/bin/env python3 -import pytest -import time -import unittest -from cereal import log -from openpilot.common.params import Params -from datetime import datetime -from unittest import mock - - -from laika.constants import SECS_IN_DAY -from laika.downloader import DownloadFailed -from laika.ephemeris import EphemerisType -from laika.gps_time import GPSTime -from laika.helpers import ConstellationId -from laika.raw_gnss import GNSSMeasurement, read_raw_ublox, read_raw_qcom -from openpilot.selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad -from openpilot.selfdrive.test.openpilotci import get_url -from openpilot.tools.lib.logreader import LogReader - -from openpilot.selfdrive.test.process_replay.process_replay import get_process_config, replay_process - -GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC = GPSTime.from_datetime(datetime(2022, month=1, day=29, hour=12)) -UBLOX_TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" -QCOM_TEST_ROUTE = "616dc83ca1f7f11e|2023-07-11--10-52-31" - - -def get_log_ublox(): - logs = LogReader(get_url(UBLOX_TEST_ROUTE, 0)) - - ublox_cfg = get_process_config("ubloxd") - all_logs = replay_process(ublox_cfg, logs) - low_gnss = [] - for m in all_logs: - if m.which() != "ubloxGnss" or m.ubloxGnss.which() != 'measurementReport': - continue - - MAX_MEAS = 7 - if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS: - mb = log.Event.new_message(ubloxGnss=m.ubloxGnss.to_dict()) - mb.logMonoTime = m.logMonoTime - mb.ubloxGnss.measurementReport.numMeas = MAX_MEAS - mb.ubloxGnss.measurementReport.measurements = list(m.ubloxGnss.measurementReport.measurements)[:MAX_MEAS] - mb.ubloxGnss.measurementReport.measurements[0].pseudorange += 1000 - low_gnss.append(mb.as_reader()) - else: - low_gnss.append(m) - return all_logs, low_gnss - - -def get_log_qcom(): - logs = LogReader(get_url(QCOM_TEST_ROUTE, 0)) - all_logs = [m for m in logs if m.which() == 'qcomGnss'] - return all_logs - - -def verify_messages(lr, laikad, return_one_success=False): - good_msgs = [] - for m in lr: - if m.which() == 'ubloxGnss': - gnss_msg = m.ubloxGnss - elif m.which() == 'qcomGnss': - gnss_msg = m.qcomGnss - else: - continue - msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=True) - if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: - good_msgs.append(msg) - if return_one_success: - return msg - return good_msgs - - -def get_first_gps_time(logs): - for m in logs: - if m.which() == 'ubloxGnss': - if m.ubloxGnss.which == 'measurementReport': - new_meas = read_raw_ublox(m.ubloxGnss.measurementReport) - if len(new_meas) > 0: - return new_meas[0].recv_time - elif m.which() == "qcomGnss": - if m.qcomGnss.which == 'measurementReport': - new_meas = read_raw_qcom(m.qcomGnss.measurementReport) - if len(new_meas) > 0: - return new_meas[0].recv_time - - -def get_measurement_mock(gpstime, sat_ephemeris): - meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}) - # Fake measurement being processed - meas.observables_final = meas.observables - meas.sat_ephemeris = sat_ephemeris - return meas - - -@pytest.mark.slow -class TestLaikad(unittest.TestCase): - - @classmethod - def setUpClass(cls): - logs, low_gnss = get_log_ublox() - cls.logs = logs - cls.low_gnss = low_gnss - cls.logs_qcom = get_log_qcom() - first_gps_time = get_first_gps_time(logs) - cls.first_gps_time = first_gps_time - - def setUp(self): - Params().remove(EPHEMERIS_CACHE) - - def test_fetch_navs_non_blocking(self): - gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) - laikad = Laikad() - laikad.fetch_navs(gpstime, block=False) - laikad.orbit_fetch_future.result(30) - - # Get results and save orbits to laikad: - laikad.fetch_navs(gpstime, block=False) - ephem = laikad.astro_dog.navs['G01'][0] - self.assertIsNotNone(ephem) - - laikad.fetch_navs(gpstime+2*SECS_IN_DAY, block=False) - laikad.orbit_fetch_future.result(30) - # Get results and save orbits to laikad: - laikad.fetch_navs(gpstime + 2 * SECS_IN_DAY, block=False) - - ephem2 = laikad.astro_dog.navs['G01'][0] - self.assertIsNotNone(ephem) - self.assertNotEqual(ephem, ephem2) - - - def test_fetch_navs_with_wrong_clocks(self): - laikad = Laikad() - - def check_has_navs(): - self.assertGreater(len(laikad.astro_dog.navs), 0) - ephem = laikad.astro_dog.navs['G01'][0] - self.assertIsNotNone(ephem) - real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1)) - wrong_future_clock_time = real_current_time + SECS_IN_DAY - - laikad.fetch_navs(wrong_future_clock_time, block=True) - check_has_navs() - self.assertEqual(laikad.last_fetch_navs_t, wrong_future_clock_time) - - # Test fetching orbits with earlier time - assert real_current_time < laikad.last_fetch_navs_t - - laikad.astro_dog.orbits = {} - laikad.fetch_navs(real_current_time, block=True) - check_has_navs() - self.assertEqual(laikad.last_fetch_navs_t, real_current_time) - - def test_laika_online(self): - laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT) - correct_msgs = verify_messages(self.logs, laikad) - - correct_msgs_expected = 560 - self.assertEqual(correct_msgs_expected, len(correct_msgs)) - self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - - def test_kf_becomes_valid(self): - laikad = Laikad(auto_update=False) - m = self.logs[0] - self.assertFalse(all(laikad.kf_valid(m.logMonoTime * 1e-9))) - kf_valid = False - for m in self.logs: - if m.which() != 'ubloxGnss': - continue - - laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) - kf_valid = all(laikad.kf_valid(m.logMonoTime * 1e-9)) - if kf_valid: - break - self.assertTrue(kf_valid) - - def test_laika_online_nav_only(self): - for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True): - laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom) - # Disable fetch_orbits to test NAV only - correct_msgs = verify_messages(logs, laikad) - correct_msgs_expected = 55 if use_qcom else 560 - valid_fix_expected = 55 if use_qcom else 560 - - self.assertEqual(correct_msgs_expected, len(correct_msgs)) - self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - - @mock.patch('laika.downloader.download_and_cache_file') - def test_laika_offline(self, downloader_mock): - downloader_mock.side_effect = DownloadFailed("Mock download failed") - laikad = Laikad(auto_update=False) - laikad.fetch_navs(GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC, block=True) - - @mock.patch('laika.downloader.download_and_cache_file') - def test_download_failed_russian_source(self, downloader_mock): - downloader_mock.side_effect = DownloadFailed - laikad = Laikad(auto_update=False) - correct_msgs = verify_messages(self.logs, laikad) - expected_msgs = 376 - self.assertEqual(expected_msgs, len(correct_msgs)) - self.assertEqual(expected_msgs, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - - def test_laika_get_orbits(self): - laikad = Laikad(auto_update=False) - # Pretend process has loaded the orbits on startup by using the time of the first gps message. - laikad.fetch_navs(self.first_gps_time, block=True) - self.dict_has_values(laikad.astro_dog.navs) - - @unittest.skip("Use to debug live data") - def test_laika_get_navs_now(self): - laikad = Laikad(auto_update=False) - laikad.fetch_navs(GPSTime.from_datetime(datetime.utcnow()), block=True) - prn = "G01" - self.assertGreater(len(laikad.astro_dog.navs[prn]), 0) - prn = "R01" - self.assertGreater(len(laikad.astro_dog.navs[prn]), 0) - - def test_get_navs_in_process(self): - for auto_fetch_navs in [True, False]: - for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True): - laikad = Laikad(auto_update=False, use_qcom=use_qcom, auto_fetch_navs=auto_fetch_navs) - has_navs = False - has_fix = False - seen_chip_eph = False - seen_internet_eph = False - - for m in logs: - if m.which() != 'ubloxGnss' and m.which() != 'qcomGnss': - continue - - gnss_msg = m.qcomGnss if use_qcom else m.ubloxGnss - out_msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=False) - if laikad.orbit_fetch_future is not None: - laikad.orbit_fetch_future.result() - vals = laikad.astro_dog.navs.values() - has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0 - vals = laikad.astro_dog.qcom_polys.values() - has_polys = len(vals) > 0 and max([len(v) for v in vals]) > 0 - has_fix = has_fix or out_msg.gnssMeasurements.positionECEF.valid - if len(out_msg.gnssMeasurements.ephemerisStatuses): - seen_chip_eph = seen_chip_eph or any(x.source == 'gnssChip' for x in out_msg.gnssMeasurements.ephemerisStatuses) - seen_internet_eph = seen_internet_eph or any(x.source == 'internet' for x in out_msg.gnssMeasurements.ephemerisStatuses) - - self.assertTrue(has_navs or has_polys) - self.assertTrue(has_fix) - self.assertTrue(seen_chip_eph or auto_fetch_navs) - self.assertTrue(seen_internet_eph or not auto_fetch_navs) - self.assertEqual(len(laikad.astro_dog.navs_fetched_times._ranges), 0) - self.assertEqual(None, laikad.orbit_fetch_future) - - def test_cache(self): - use_qcom = True - for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True): - Params().remove(EPHEMERIS_CACHE) - laikad = Laikad(auto_update=True, save_ephemeris=True, use_qcom=use_qcom) - def wait_for_cache(): - max_time = 2 - while Params().get(EPHEMERIS_CACHE) is None: - time.sleep(0.1) - max_time -= 0.1 - if max_time < 0: - self.fail("Cache has not been written after 2 seconds") - - # Test cache with no ephemeris - laikad.last_report_time = GPSTime(1,0) - laikad.cache_ephemeris() - if Params().get(EPHEMERIS_CACHE) is not None: - self.fail("Cache should not have been written without valid ephem") - - #laikad.astro_dog.get_navs(self.first_gps_time) - msg = verify_messages(logs, laikad, return_one_success=True) - laikad.cache_ephemeris() - wait_for_cache() - - # Check both nav and orbits separate - laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, - save_ephemeris=True, use_qcom=use_qcom, auto_fetch_navs=False) - # Verify navs are loaded from cache - self.dict_has_values(laikad.astro_dog.navs) - # Verify cache is working for only nav by running a segment - msg = verify_messages(logs, laikad, return_one_success=True) - self.assertTrue(len(msg.gnssMeasurements.ephemerisStatuses)) - self.assertTrue(any(x.source=='cache' for x in msg.gnssMeasurements.ephemerisStatuses)) - self.assertIsNotNone(msg) - - #TODO test cache with only orbits - #with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method: - # # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently - # laikad.astro_dog.orbit_fetched_times = TimeRangeHolder() - # laikad.fetch_navs(self.first_gps_time, block=False) - # mock_method.assert_not_called() - - # # Verify cache is working for only orbits by running a segment - # laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT, save_ephemeris=True) - # msg = verify_messages(self.logs, laikad, return_one_success=True) - # self.assertIsNotNone(msg) - # # Verify orbit data is not downloaded - # mock_method.assert_not_called() - #break - - def test_low_gnss_meas(self): - cnt = 0 - laikad = Laikad() - for m in self.low_gnss: - msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) - if msg is None: - continue - gm = msg.gnssMeasurements - if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid: - cnt += 1 - self.assertEqual(cnt, 560) - - def dict_has_values(self, dct): - self.assertGreater(len(dct), 0) - self.assertGreater(min([len(v) for v in dct.values()]), 0) - - -if __name__ == "__main__": - unittest.main() diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index aee4748a26..320958d344 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -45,7 +45,6 @@ procs = [ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), NativeProcess("camerad", "system/camerad", ["./camerad"], driverview), - NativeProcess("clocksd", "system/clocksd", ["./clocksd"], only_onroad), NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad), NativeProcess("proclogd", "system/proclogd", ["./proclogd"], only_onroad), PythonProcess("logmessaged", "system.logmessaged", always_run), @@ -69,7 +68,6 @@ procs = [ PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)), - PythonProcess("laikad", "selfdrive.locationd.laikad", only_onroad), PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", qcomgps, enabled=TICI), PythonProcess("navd", "selfdrive.navd.navd", only_onroad), PythonProcess("pandad", "selfdrive.boardd.pandad", always_run), diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 9303e970a9..4d3af51635 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -26,6 +26,8 @@ class ModelConstants: # model outputs constants FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32) + FCW_5MS2_PROBS_WIDTH = 5 + FCW_3MS2_PROBS_WIDTH = 2 DISENGAGE_WIDTH = 5 POSE_WIDTH = 6 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 9977533e6d..2b8a72b9be 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -1,16 +1,19 @@ +import os import capnp import numpy as np from typing import Dict from cereal import log from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta +SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') + ConfidenceClass = log.ModelDataV2.ConfidenceClass class PublishState: def __init__(self): self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) - self.prev_brake_5ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) - self.prev_brake_3ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) + self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32) + self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32) def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None): builder.t = t @@ -168,6 +171,10 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, else: modelV2.confidence = ConfidenceClass.red + # raw prediction if enabled + if SEND_RAW_PRED: + modelV2.rawPredictions = net_output_data['raw_pred'].tobytes() + def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None: msg.valid = live_calib_seen & (vipc_dropped_frames < 1) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 2508034552..54a95340b4 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import sys +import os import time import pickle import numpy as np @@ -22,6 +23,8 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext +SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') + MODEL_PATHS = { ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed', ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'} @@ -73,7 +76,10 @@ class ModelState: self.model.addInput(k, v) def slice_outputs(self, model_outputs: np.ndarray) -> Dict[str, np.ndarray]: - return {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()} + parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()} + if SEND_RAW_PRED: + parsed_model_outputs['raw_pred'] = model_outputs.copy() + return parsed_model_outputs def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray, inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[Dict[str, np.ndarray]]: diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md index 6992d46f1d..bec3eb9016 100644 --- a/selfdrive/test/process_replay/README.md +++ b/selfdrive/test/process_replay/README.md @@ -17,7 +17,6 @@ Currently the following processes are tested: * locationd * paramsd * ubloxd -* laikad * torqued ### Usage @@ -71,7 +70,7 @@ lr = LogReader(...) output_logs = replay_process_with_name('locationd', lr) # or list of names -output_logs = replay_process_with_name(['ubloxd', 'locationd', 'laikad'], lr) +output_logs = replay_process_with_name(['ubloxd', 'locationd'], lr) ``` Supported processes: @@ -83,7 +82,6 @@ Supported processes: * locationd * paramsd * ubloxd -* laikad * torqued * modeld * dmonitoringmodeld diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 9aab541991..0cb8ac9cac 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -448,16 +448,6 @@ def controlsd_config_callback(params, cfg, lr): params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) -def laikad_config_pubsub_callback(params, cfg, lr): - ublox = params.get_bool("UbloxAvailable") - main_key = "ubloxGnss" if ublox else "qcomGnss" - sub_keys = ({"qcomGnss", } if ublox else {"ubloxGnss", }) - - cfg.pubs = set(cfg.pubs) - sub_keys - cfg.main_pub = main_key - cfg.main_pub_drained = True - - def locationd_config_pubsub_callback(params, cfg, lr): ublox = params.get_bool("UbloxAvailable") sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) @@ -543,17 +533,6 @@ CONFIGS = [ subs=["ubloxGnss", "gpsLocationExternal"], ignore=["logMonoTime"], ), - ProcessConfig( - proc_name="laikad", - pubs=["ubloxGnss", "qcomGnss"], - subs=["gnssMeasurements"], - ignore=["logMonoTime"], - config_callback=laikad_config_pubsub_callback, - tolerance=NUMPY_TOLERANCE, - processing_time=0.002, - timeout=60*10, # first messages are blocked on internet assistance - main_pub="ubloxGnss", # config_callback will switch this to qcom if needed - ), ProcessConfig( proc_name="torqued", pubs=["liveLocationKalman", "carState", "carControl"], diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py index f3f48d0159..4b8629fc7d 100755 --- a/selfdrive/test/process_replay/test_fuzzy.py +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -13,7 +13,7 @@ import openpilot.selfdrive.test.process_replay.process_replay as pr # These processes currently fail because of unrealistic data breaking assumptions # that openpilot makes causing error with NaN, inf, int size, array indexing ... # TODO: Make each one testable -NOT_TESTED = ['controlsd', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'laikad', 'dmonitoringmodeld', 'modeld'] +NOT_TESTED = ['controlsd', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED] class TestFuzzProcesses(unittest.TestCase): diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py index 1d380ba5eb..6d0cc204c5 100755 --- a/selfdrive/test/profiling/profiler.py +++ b/selfdrive/test/profiling/profiler.py @@ -83,14 +83,12 @@ if __name__ == '__main__': from openpilot.selfdrive.controls.radard import radard_thread from openpilot.selfdrive.locationd.paramsd import main as paramsd_thread from openpilot.selfdrive.controls.plannerd import main as plannerd_thread - from openpilot.selfdrive.locationd.laikad import main as laikad_thread procs = { 'radard': radard_thread, 'controlsd': controlsd_thread, 'paramsd': paramsd_thread, 'plannerd': plannerd_thread, - 'laikad': laikad_thread, } proc = sys.argv[1] diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 4a9fc85d2e..e4f6bccf6c 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -47,7 +47,6 @@ PROCS = { "selfdrive.monitoring.dmonitoringd": 4.0, "./proclogd": 1.54, "system.logmessaged": 0.2, - "./clocksd": 0.02, "selfdrive.tombstoned": 0, "./logcatd": 0, "system.micd": 10.0, @@ -57,7 +56,6 @@ PROCS = { "selfdrive.navd.navd": 0.4, "system.loggerd.uploader": 3.0, "system.loggerd.deleter": 0.1, - "selfdrive.locationd.laikad": (1.0, 80.0), # TODO: better GPS setup in testing closet } PROCS.update({ diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py index 8157066334..326cb4fa56 100755 --- a/selfdrive/test/update_ci_routes.py +++ b/selfdrive/test/update_ci_routes.py @@ -8,7 +8,6 @@ from azure.storage.blob import ContainerClient from tqdm import tqdm from openpilot.selfdrive.car.tests.routes import routes as test_car_models_routes -from openpilot.selfdrive.locationd.test.test_laikad import UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE from openpilot.selfdrive.test.process_replay.test_processes import source_segments as replay_segments from openpilot.selfdrive.test.openpilotci import (DATA_CI_ACCOUNT, DATA_CI_ACCOUNT_URL, DATA_CI_CONTAINER, get_azure_credential, get_container_sas) @@ -90,7 +89,6 @@ if __name__ == "__main__": if not len(to_sync): # sync routes from the car tests routes and process replay - to_sync.extend([UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE]) to_sync.extend([rt.route for rt in test_car_models_routes]) to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments]) diff --git a/system/clocksd/.gitignore b/system/clocksd/.gitignore deleted file mode 100644 index a6d841d65e..0000000000 --- a/system/clocksd/.gitignore +++ /dev/null @@ -1 +0,0 @@ -clocksd diff --git a/system/clocksd/SConscript b/system/clocksd/SConscript deleted file mode 100644 index d1cf13e9e8..0000000000 --- a/system/clocksd/SConscript +++ /dev/null @@ -1,2 +0,0 @@ -Import('env', 'common', 'cereal', 'messaging') -env.Program('clocksd.cc', LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj']) diff --git a/system/clocksd/clocksd.cc b/system/clocksd/clocksd.cc deleted file mode 100644 index a5912cf51a..0000000000 --- a/system/clocksd/clocksd.cc +++ /dev/null @@ -1,73 +0,0 @@ -#include -#include -#include - -#include -#include - -// Apple doesn't have timerfd -#ifdef __APPLE__ -#include -#else -#include -#endif - -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/timing.h" -#include "common/util.h" - -ExitHandler do_exit; - -int main() { - setpriority(PRIO_PROCESS, 0, -13); - PubMaster pm({"clocks"}); - -#ifndef __APPLE__ - int timerfd = timerfd_create(CLOCK_BOOTTIME, 0); - assert(timerfd >= 0); - - struct itimerspec spec = {0}; - spec.it_interval.tv_sec = 1; - spec.it_interval.tv_nsec = 0; - spec.it_value.tv_sec = 1; - spec.it_value.tv_nsec = 0; - - int err = timerfd_settime(timerfd, 0, &spec, 0); - assert(err == 0); - - uint64_t expirations = 0; - while (!do_exit && (err = read(timerfd, &expirations, sizeof(expirations)))) { - if (err < 0) { - if (errno == EINTR) continue; - break; - } -#else - // Just run at 1Hz on apple - while (!do_exit) { - util::sleep_for(1000); -#endif - - uint64_t boottime = nanos_since_boot(); - uint64_t monotonic = nanos_monotonic(); - uint64_t monotonic_raw = nanos_monotonic_raw(); - uint64_t wall_time = nanos_since_epoch(); - - MessageBuilder msg; - auto clocks = msg.initEvent().initClocks(); - - clocks.setBootTimeNanos(boottime); - clocks.setMonotonicNanos(monotonic); - clocks.setMonotonicRawNanos(monotonic_raw); - clocks.setWallTimeNanos(wall_time); - - pm.send("clocks", msg); - } - -#ifndef __APPLE__ - close(timerfd); -#endif - return 0; -} diff --git a/system/sensord/rawgps/test_rawgps.py b/system/sensord/rawgps/test_rawgps.py index 02777d5a1d..2a0ee656e2 100755 --- a/system/sensord/rawgps/test_rawgps.py +++ b/system/sensord/rawgps/test_rawgps.py @@ -5,13 +5,11 @@ import time import datetime import unittest import subprocess -import numpy as np import cereal.messaging as messaging from openpilot.system.hardware import TICI from openpilot.system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem from openpilot.selfdrive.manager.process_config import managed_processes -from openpilot.common.transformations.coordinates import ecef_from_geodetic GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0'))) @@ -123,22 +121,5 @@ class TestRawgpsd(unittest.TestCase): managed_processes['rawgpsd'].stop() self.check_assistance(True) - @unittest.skipIf(not GOOD_SIGNAL, "No good GPS signal") - def test_fix(self): - managed_processes['rawgpsd'].start() - managed_processes['laikad'].start() - assert self._wait_for_output(60) - assert self.sm.updated['qcomGnss'] - assert self.sm.updated['gpsLocation'] - assert self.sm['gpsLocation'].flags == 1 - module_fix = ecef_from_geodetic([self.sm['gpsLocation'].latitude, - self.sm['gpsLocation'].longitude, - self.sm['gpsLocation'].altitude]) - assert self.sm['gnssMeasurements'].positionECEF.valid - total_diff = np.array(self.sm['gnssMeasurements'].positionECEF.value) - module_fix - self.assertLess(np.linalg.norm(total_diff), 100) - managed_processes['laikad'].stop() - managed_processes['rawgpsd'].stop() - if __name__ == "__main__": unittest.main(failfast=True) diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 5cd5e70267..9ac88032d5 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -22,12 +22,6 @@ #include "tools/cabana/streamselector.h" #include "tools/cabana/tools/findsignal.h" -static MainWindow *main_win = nullptr; -void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) { - if (type == QtDebugMsg) std::cout << msg.toStdString() << std::endl; - if (main_win) emit main_win->showMessage(msg, 2000); -} - MainWindow::MainWindow() : QMainWindow() { createDockWindows(); setCentralWidget(center_widget = new CenterWidget(this)); @@ -45,20 +39,23 @@ MainWindow::MainWindow() : QMainWindow() { } restoreState(settings.window_state); + // install handlers + static auto static_main_win = this; qRegisterMetaType("uint64_t"); qRegisterMetaType("SourceSet"); qRegisterMetaType("ReplyMsgType"); - installMessageHandler([this](ReplyMsgType type, const std::string msg) { - // use queued connection to recv the log messages from replay. - emit showMessage(QString::fromStdString(msg), 2000); + installDownloadProgressHandler([](uint64_t cur, uint64_t total, bool success) { + emit static_main_win->updateProgressBar(cur, total, success); }); - installDownloadProgressHandler([this](uint64_t cur, uint64_t total, bool success) { - emit updateProgressBar(cur, total, success); + qInstallMessageHandler([](QtMsgType type, const QMessageLogContext &context, const QString &msg) { + if (type == QtDebugMsg) std::cout << msg.toStdString() << std::endl; + emit static_main_win->showMessage(msg, 2000); + }); + installMessageHandler([](ReplyMsgType type, const std::string msg) { + qInfo() << QString::fromStdString(msg); }); - main_win = this; - qInstallMessageHandler(qLogMessageHandler); - + // load fingerprints QFile json_file(QApplication::applicationDirPath() + "/dbc/car_fingerprint_to_dbc.json"); if (json_file.open(QIODevice::ReadOnly)) { fingerprint_to_dbc = QJsonDocument::fromJson(json_file.readAll()); @@ -597,7 +594,9 @@ void MainWindow::closeEvent(QCloseEvent *event) { cleanupAutoSaveFile(); remindSaveChanges(); - main_win = nullptr; + installDownloadProgressHandler(nullptr); + qInstallMessageHandler(nullptr); + if (floating_window) floating_window->deleteLater(); diff --git a/tools/gpstest/fuzzy_testing.py b/tools/gpstest/fuzzy_testing.py index 3bad2770cd..532fd2d34c 100755 --- a/tools/gpstest/fuzzy_testing.py +++ b/tools/gpstest/fuzzy_testing.py @@ -33,8 +33,7 @@ def run_remote_checker(lat, lon, alt, duration, ip_addr): return False, None, None matched, log, info = con.root.exposed_run_checker(lat, lon, alt, - timeout=duration, - use_laikad=True) + timeout=duration) con.close() # TODO: might wanna fetch more logs here con = None @@ -43,7 +42,7 @@ def run_remote_checker(lat, lon, alt, duration, ip_addr): stats = defaultdict(int) # type: ignore -keys = ['success', 'failed', 'ublox_fail', 'laikad_fail', 'proc_crash', 'checker_crash'] +keys = ['success', 'failed', 'ublox_fail', 'proc_crash', 'checker_crash'] def print_report(): print("\nFuzzy testing report summary:") @@ -62,10 +61,7 @@ def update_stats(matched, log, info): if log == "CHECKER CRASHED": stats['checker_crash'] += 1 if log == "TIMEOUT": - if "LAIKAD" in info: - stats['laikad_fail'] += 1 - else: # "UBLOX" in info - stats['ublox_fail'] += 1 + stats['ublox_fail'] += 1 def main(ip_addr, continuous_mode, timeout, pos): diff --git a/tools/gpstest/rpc_server.py b/tools/gpstest/rpc_server.py deleted file mode 100644 index 798237142d..0000000000 --- a/tools/gpstest/rpc_server.py +++ /dev/null @@ -1,185 +0,0 @@ -import os -import time -import shutil -from datetime import datetime -from collections import defaultdict -from openpilot.system.hardware.hw import Paths - -import rpyc -from rpyc.utils.server import ThreadedServer - -#from openpilot.common.params import Params -import cereal.messaging as messaging -from openpilot.selfdrive.manager.process_config import managed_processes -from laika.lib.coordinates import ecef2geodetic - -DELTA = 0.001 -ALT_DELTA = 30 -MATCH_NUM = 10 -REPORT_STATS = 10 - -EPHEM_CACHE = "/data/params/d/LaikadEphemerisV3" - -SERVER_LOG_FILE = "/tmp/fuzzy_server.log" -server_log = open(SERVER_LOG_FILE, "w+") - -def slog(msg): - server_log.write(f"{datetime.now().strftime('%H:%M:%S.%f')} | {msg}\n") - server_log.flush() - -def handle_laikad(msg): - if not hasattr(msg, 'correctedMeasurements'): - return None - - num_corr = len(msg.correctedMeasurements) - pos_ecef = msg.positionECEF.value - pos_geo = [] - if len(pos_ecef) > 0: - pos_geo = ecef2geodetic(pos_ecef) - - pos_std = msg.positionECEF.std - pos_valid = msg.positionECEF.valid - - slog(f"{num_corr} {pos_geo} {pos_ecef} {pos_std} {pos_valid}") - return pos_geo, (num_corr, pos_geo, list(pos_ecef), list(msg.positionECEF.std)) - -hw_msgs = 0 -ephem_msgs: dict = defaultdict(int) -def handle_ublox(msg): - global hw_msgs - - d = msg.to_dict() - - if 'hwStatus2' in d: - hw_msgs += 1 - - if 'ephemeris' in d: - ephem_msgs[msg.ephemeris.svId] += 1 - - num_meas = None - if 'measurementReport' in d: - num_meas = msg.measurementReport.numMeas - - return [hw_msgs, ephem_msgs, num_meas] - - -def start_procs(procs): - for p in procs: - managed_processes[p].start() - time.sleep(1) - -def kill_procs(procs, no_retry=False): - for p in procs: - managed_processes[p].stop() - time.sleep(1) - - if not no_retry: - for p in procs: - mp = managed_processes[p].proc - if mp is not None and mp.is_alive(): - managed_processes[p].stop() - time.sleep(3) - -def check_alive_procs(procs): - for p in procs: - mp = managed_processes[p].proc - if mp is None or not mp.is_alive(): - return False, p - return True, None - - -class RemoteCheckerService(rpyc.Service): - def on_connect(self, conn): - pass - - def on_disconnect(self, conn): - #kill_procs(self.procs, no_retry=False) - # this execution is delayed, it will kill the next run of laikad - # TODO: add polling to wait for everything is killed - pass - - def run_checker(self, slat, slon, salt, sockets, procs, timeout): - global hw_msgs, ephem_msgs - hw_msgs = 0 - ephem_msgs = defaultdict(int) - - slog(f"Run test: {slat} {slon} {salt}") - - # quectel_mod = Params().get_bool("UbloxAvailable") - - match_cnt = 0 - msg_cnt = 0 - stats_laikad = [] - stats_ublox = [] - - self.procs = procs - start_procs(procs) - sm = messaging.SubMaster(sockets) - - start_time = time.monotonic() - while True: - sm.update() - - if sm.updated['ubloxGnss']: - stats_ublox.append(handle_ublox(sm['ubloxGnss'])) - - if sm.updated['gnssMeasurements']: - pos_geo, stats = handle_laikad(sm['gnssMeasurements']) - if pos_geo is None or len(pos_geo) == 0: - continue - - match = all(abs(g-s) < DELTA for g,s in zip(pos_geo[:2], [slat, slon], strict=True)) - match &= abs(pos_geo[2] - salt) < ALT_DELTA - if match: - match_cnt += 1 - if match_cnt >= MATCH_NUM: - return True, "MATCH", f"After: {round(time.monotonic() - start_time, 4)}" - - # keep some stats for error reporting - stats_laikad.append(stats) - - if (msg_cnt % 10) == 0: - a, p = check_alive_procs(procs) - if not a: - return False, "PROC CRASH", f"{p}" - msg_cnt += 1 - - if (time.monotonic() - start_time) > timeout: - h = f"LAIKAD: {stats_laikad[-REPORT_STATS:]}" - if len(h) == 0: - h = f"UBLOX: {stats_ublox[-REPORT_STATS:]}" - return False, "TIMEOUT", h - - - def exposed_run_checker(self, slat, slon, salt, timeout=180, use_laikad=True): - try: - procs = [] - sockets = [] - - if use_laikad: - procs.append("laikad") # pigeond, ubloxd # might wanna keep them running - sockets += ['ubloxGnss', 'gnssMeasurements'] - - if os.path.exists(EPHEM_CACHE): - os.remove(EPHEM_CACHE) - shutil.rmtree(Paths.download_cache_root(), ignore_errors=True) - - ret = self.run_checker(slat, slon, salt, sockets, procs, timeout) - kill_procs(procs) - return ret - - except Exception as e: - # always make sure processes get killed - kill_procs(procs) - return False, "CHECKER CRASHED", f"{str(e)}" - - - def exposed_kill_procs(self): - kill_procs(self.procs, no_retry=True) - - -if __name__ == "__main__": - print(f"Sever Log written to: {SERVER_LOG_FILE}") - t = ThreadedServer(RemoteCheckerService, port=18861) - t.start() - diff --git a/tools/gpstest/test_laikad.py b/tools/gpstest/test_laikad.py deleted file mode 100755 index abb47d4bfc..0000000000 --- a/tools/gpstest/test_laikad.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import unittest - -import cereal.messaging as messaging -import openpilot.system.sensord.pigeond as pd - -from openpilot.common.params import Params -from openpilot.system.hardware import TICI -from openpilot.selfdrive.manager.process_config import managed_processes -from openpilot.selfdrive.test.helpers import with_processes - - -def wait_for_location(sm, timeout, con=10): - cons_meas = 0 - start_time = time.monotonic() - while (time.monotonic() - start_time) < timeout: - sm.update() - if not sm.updated["gnssMeasurements"]: - continue - - msg = sm["gnssMeasurements"] - cons_meas = (cons_meas + 1) if 'positionECEF' in msg.to_dict() else 0 - if cons_meas >= con: - return True - return False - - -class TestLaikad(unittest.TestCase): - @classmethod - def setUpClass(self): - if not TICI: - raise unittest.SkipTest - - ublox_available = Params().get_bool("UbloxAvailable") - if not ublox_available: - raise unittest.SkipTest - - def setUp(self): - # ensure laikad cold start - Params().remove("LaikadEphemerisV3") - os.environ["LAIKAD_NO_INTERNET"] = "1" - managed_processes['laikad'].start() - - def tearDown(self): - managed_processes['laikad'].stop() - - - @with_processes(['pigeond', 'ubloxd']) - def test_laikad_cold_start(self): - time.sleep(5) - - start_time = time.monotonic() - sm = messaging.SubMaster(["gnssMeasurements"]) - - success = wait_for_location(sm, 60*2, con=10) - duration = time.monotonic() - start_time - - assert success, "Waiting for location timed out (2min)!" - assert duration < 60, f"Received Location {duration}!" - - - @with_processes(['ubloxd']) - def test_laikad_ublox_reset_start(self): - time.sleep(2) - - pigeon, pm = pd.create_pigeon() - pd.init_baudrate(pigeon) - assert pigeon.reset_device(), "Could not reset device!" - - laikad_sock = messaging.sub_sock("gnssMeasurements", timeout=0.1) - ublox_gnss_sock = messaging.sub_sock("ubloxGnss", timeout=0.1) - - pd.init_baudrate(pigeon) - pd.initialize_pigeon(pigeon) - pd.run_receiving(pigeon, pm, 180) - - ublox_msgs = messaging.drain_sock(ublox_gnss_sock) - laikad_msgs = messaging.drain_sock(laikad_sock) - - gps_ephem_cnt = 0 - glonass_ephem_cnt = 0 - for um in ublox_msgs: - if um.ubloxGnss.which() == 'ephemeris': - gps_ephem_cnt += 1 - elif um.ubloxGnss.which() == 'glonassEphemeris': - glonass_ephem_cnt += 1 - - assert gps_ephem_cnt > 0, "NO gps ephemeris collected!" - assert glonass_ephem_cnt > 0, "NO glonass ephemeris collected!" - - pos_meas = 0 - duration = -1 - for lm in laikad_msgs: - pos_meas = (pos_meas + 1) if 'positionECEF' in lm.gnssMeasurements.to_dict() else 0 - if pos_meas > 5: - duration = (lm.logMonoTime - laikad_msgs[0].logMonoTime)*1e-9 - break - - assert pos_meas > 5, "NOT enough positions at end of read!" - assert duration < 120, "Laikad took too long to get a Position!" - -if __name__ == "__main__": - unittest.main() diff --git a/tools/replay/util.cc b/tools/replay/util.cc index 2c2de69e78..b4f72d0530 100644 --- a/tools/replay/util.cc +++ b/tools/replay/util.cc @@ -19,10 +19,7 @@ #include "common/util.h" ReplayMessageHandler message_handler = nullptr; -DownloadProgressHandler download_progress_handler = nullptr; - void installMessageHandler(ReplayMessageHandler handler) { message_handler = handler; } -void installDownloadProgressHandler(DownloadProgressHandler handler) { download_progress_handler = handler; } void logMessage(ReplyMsgType type, const char *fmt, ...) { static std::mutex lock; @@ -94,6 +91,11 @@ size_t write_cb(char *data, size_t size, size_t count, void *userp) { size_t dumy_write_cb(char *data, size_t size, size_t count, void *userp) { return size * count; } struct DownloadStats { + void installDownloadProgressHandler(DownloadProgressHandler handler) { + std::lock_guard lk(lock); + download_progress_handler = handler; + } + void add(const std::string &url, uint64_t total_bytes) { std::lock_guard lk(lock); items[url] = {0, total_bytes}; @@ -121,10 +123,17 @@ struct DownloadStats { std::mutex lock; std::map> items; double prev_tm = 0; + DownloadProgressHandler download_progress_handler = nullptr; }; +static DownloadStats download_stats; + } // namespace +void installDownloadProgressHandler(DownloadProgressHandler handler) { + download_stats.installDownloadProgressHandler(handler); +} + std::string formattedDataSize(size_t size) { if (size < 1024) { return std::to_string(size) + " B"; @@ -167,7 +176,6 @@ std::string getUrlWithoutQuery(const std::string &url) { template bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t content_length, std::atomic *abort) { - static DownloadStats download_stats; download_stats.add(url, content_length); int parts = 1;