diff --git a/.gitignore b/.gitignore index 2b283d3b1..be6629f21 100644 --- a/.gitignore +++ b/.gitignore @@ -46,9 +46,6 @@ selfdrive/mapd/default_speeds_by_region.json system/proclogd/proclogd selfdrive/ui/_ui selfdrive/test/longitudinal_maneuvers/out -selfdrive/visiond/visiond -selfdrive/sensord/_gpsd -selfdrive/sensord/_sensord system/camerad/camerad system/camerad/test/ae_gray_test selfdrive/modeld/_modeld diff --git a/Jenkinsfile b/Jenkinsfile index 30c045e41..8a2f83be2 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -19,6 +19,9 @@ source ~/.bash_profile if [ -f /TICI ]; then source /etc/profile fi +if [ -f /data/openpilot/launch_env.sh ]; then + source /data/openpilot/launch_env.sh +fi ln -snf ${env.TEST_DIR} /data/pythonpath @@ -125,6 +128,21 @@ pipeline { } */ + stage('tizi-tests') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tizi", [ + ["build openpilot", "cd selfdrive/manager && ./build.py"], + ["test boardd loopback", "SINGLE_PANDA=1 python selfdrive/boardd/tests/test_boardd_loopback.py"], + ["test pandad", "python selfdrive/boardd/tests/test_pandad.py"], + ["test sensord", "cd system/sensord/tests && python -m unittest test_sensord.py"], + ["test camerad", "python system/camerad/test/test_camerad.py"], + ["test exposure", "python system/camerad/test/test_exposure.py"], + ["test amp", "python system/hardware/tici/tests/test_amplifier.py"], + ]) + } + } + stage('build') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } environment { @@ -156,16 +174,17 @@ pipeline { steps { phone_steps("tici-common", [ ["build", "cd selfdrive/manager && ./build.py"], - ["test power draw", "python system/hardware/tici/test_power_draw.py"], - ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], - ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], - ["test pigeond", "python selfdrive/sensord/tests/test_pigeond.py"], + ["test power draw", "python system/hardware/tici/tests/test_power_draw.py"], + ["test loggerd", "python system/loggerd/tests/test_loggerd.py"], + ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python system/loggerd/tests/test_encoder.py"], + ["test pigeond", "python system/sensord/tests/test_pigeond.py"], ["test manager", "python selfdrive/manager/test/test_manager.py"], + ["test pandad", "python selfdrive/boardd/tests/test_pandad.py"], ]) } } - stage('camerad-ar') { + stage('camerad') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } steps { phone_steps("tici-ar0231", [ @@ -173,12 +192,6 @@ pipeline { ["test camerad", "python system/camerad/test/test_camerad.py"], ["test exposure", "python system/camerad/test/test_exposure.py"], ]) - } - } - - stage('camerad-ox') { - agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } - steps { phone_steps("tici-ox03c10", [ ["build", "cd selfdrive/manager && ./build.py"], ["test camerad", "python system/camerad/test/test_camerad.py"], @@ -192,11 +205,11 @@ pipeline { steps { phone_steps("tici-lsmc", [ ["build", "cd selfdrive/manager && ./build.py"], - ["test sensord", "cd selfdrive/sensord/tests && python -m unittest test_sensord.py"], + ["test sensord", "cd system/sensord/tests && python -m unittest test_sensord.py"], ]) phone_steps("tici-bmx-lsm", [ ["build", "cd selfdrive/manager && ./build.py"], - ["test sensord", "cd selfdrive/sensord/tests && python -m unittest test_sensord.py"], + ["test sensord", "cd system/sensord/tests && python -m unittest test_sensord.py"], ]) } } diff --git a/README.md b/README.md index 4896d9ff7..5428ea7a9 100755 --- a/README.md +++ b/README.md @@ -42,7 +42,7 @@ To use openpilot in a car, you need four things * A supported device to run this software: a [comma three](https://comma.ai/shop/products/three). * This software. The setup procedure of the comma three allows the user to enter a URL for custom software. The URL, openpilot.comma.ai will install the release version of openpilot. To install openpilot master, you can use installer.comma.ai/commaai/master, and replacing commaai with another GitHub username can install a fork. -* One of [the 200+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot. +* One of [the 250+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot. * A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car. We have detailed instructions for [how to mount the device in a car](https://comma.ai/setup). @@ -67,7 +67,7 @@ Documentation related to openpilot development can be found on [docs.comma.ai](h You can add support for your car by following guides we have written for [Brand](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/) and [Model](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) ports. Generally, a car with adaptive cruise control and lane keep assist is a good candidate. [Join our Discord](https://discord.comma.ai) to discuss car ports: most car makes have a dedicated channel. -Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs/). +Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs#open-positions). And [follow us on Twitter](https://twitter.com/comma_ai). @@ -108,7 +108,10 @@ Directory Structure ├── clocksd # Broadcasts current time ├── hardware # Hardware abstraction classes ├── logcatd # systemd journal as a service - └── proclogd # Logs information from /proc + ├── loggerd # Logger and uploader of car data + ├── proclogd # Logs information from /proc + ├── sensord # IMU interface code + └── ubloxd # u-blox GNSS module interface code └── selfdrive # Code needed to drive the car ├── assets # Fonts, images, and sounds for UI ├── athena # Allows communication with the app @@ -117,12 +120,10 @@ Directory Structure ├── controls # Planning and controls ├── debug # Tools to help you debug and do car ports ├── locationd # Precise localization and vehicle parameter estimation - ├── loggerd # Logger and uploader of car data ├── manager # Daemon that starts/stops all other daemons as needed ├── modeld # Driving and monitoring model runners ├── monitoring # Daemon to determine driver attention ├── navd # Turn-by-turn navigation - ├── sensord # IMU interface code ├── test # Unit tests, system tests, and a car simulator └── ui # The UI diff --git a/RELEASES.md b/RELEASES.md index f166e7d1c..a4b2a7687 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,30 @@ +Version 0.9.2 (2023-05-22) +======================== +* New driving model + * Reduced turn diving + * Trained on a new dataset +* UI updates + * New experimental mode visualization + * Draw MPC path instead of model-predicted path +* AGNOS 7 + * Faster boot time + * Fixes rare no sounds bug + * Fixes bootsplash bug at extreme temperatures +* Buick LaCrosse 2017-19 support thanks to koch-cf! +* Chevrolet Trailblazer 2021-22 support thanks to TurboCE! +* Ford Bronco Sport 2021-22 support +* Ford Escape 2020-22 support +* Ford Explorer 2020-22 support +* Ford Kuga 2020-22 support +* Ford Maverick 2022-23 support +* Genesis GV80 2023 support thanks to JWingate80! +* Honda HR-V 2023 support thanks to AlexandreSato and galegozi! +* Kia Niro EV 2023 support thanks to JosselinLecocq! +* Lexus ES 2017-18 support +* Lincoln Aviator 2021 support +* Škoda Fabia 2022-23 support thanks to jyoung8607! + + Version 0.9.1 (2023-02-28) ======================== * New driving model diff --git a/SConstruct b/SConstruct index 007931b7f..45b6fd8fd 100644 --- a/SConstruct +++ b/SConstruct @@ -5,6 +5,10 @@ import sysconfig import platform import numpy as np +import SCons.Errors + +SCons.Warnings.warningAsException(True) + TICI = os.path.isfile('/TICI') AGNOS = TICI @@ -118,7 +122,7 @@ else: f"#third_party/libyuv/{yuv_dir}/lib", f"{brew_prefix}/lib", f"{brew_prefix}/Library", - f"{brew_prefix}/opt/openssl/lib", + f"{brew_prefix}/opt/openssl@3.0/lib", f"{brew_prefix}/Cellar", "/System/Library/Frameworks/OpenGL.framework/Libraries", ] @@ -131,7 +135,7 @@ else: cxxflags += ["-DGL_SILENCE_DEPRECATION"] cpppath += [ f"{brew_prefix}/include", - f"{brew_prefix}/opt/openssl/include", + f"{brew_prefix}/opt/openssl@3.0/include", ] # Linux 86_64 else: @@ -310,8 +314,14 @@ else: qt_env.PrependENVPath('PATH', Dir("#third_party/qt5/larch64/bin/").abspath) elif arch != "Darwin": qt_libs += ["GL"] +qt_env['QT3DIR'] = qt_env['QTDIR'] + +# compatibility for older SCons versions +try: + qt_env.Tool('qt3') +except SCons.Errors.UserError: + qt_env.Tool('qt') -qt_env.Tool('qt') qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"] qt_flags = [ "-D_REENTRANT", @@ -395,12 +405,17 @@ SConscript(['rednose/SConscript']) # Build system services SConscript([ - 'system/camerad/SConscript', 'system/clocksd/SConscript', 'system/proclogd/SConscript', + 'system/ubloxd/SConscript', + 'system/loggerd/SConscript', ]) if arch != "Darwin": - SConscript(['system/logcatd/SConscript']) + SConscript([ + 'system/camerad/SConscript', + 'system/sensord/SConscript', + 'system/logcatd/SConscript', + ]) # Build openpilot @@ -417,25 +432,16 @@ SConscript(['third_party/SConscript']) SConscript(['common/kalman/SConscript']) SConscript(['common/transformations/SConscript']) -SConscript(['selfdrive/modeld/SConscript']) - +SConscript(['selfdrive/boardd/SConscript']) SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript']) SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript']) - -SConscript(['selfdrive/boardd/SConscript']) - -SConscript(['selfdrive/loggerd/SConscript']) - SConscript(['selfdrive/locationd/SConscript']) -SConscript(['selfdrive/sensord/SConscript']) -SConscript(['selfdrive/ui/SConscript']) SConscript(['selfdrive/navd/SConscript']) +SConscript(['selfdrive/modeld/SConscript']) +SConscript(['selfdrive/ui/SConscript']) -if arch in ['x86_64', 'Darwin'] or GetOption('extras'): +if (arch in ['x86_64', 'Darwin'] and Dir('#tools/cabana/').exists()) or GetOption('extras'): SConscript(['tools/replay/SConscript']) - - opendbc = abspath([File('opendbc/can/libdbc.so')]) - Export('opendbc') SConscript(['tools/cabana/SConscript']) external_sconscript = GetOption('external_sconscript') diff --git a/cereal/car.capnp b/cereal/car.capnp index 1af634788..2730888f7 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -43,6 +43,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { overheat @19; calibrationIncomplete @20; calibrationInvalid @21; + calibrationRecalibrating @117; controlsMismatch @22; pcmEnable @23; pcmDisable @24; @@ -65,7 +66,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { lowBattery @48; vehicleModelInvalid @50; accFaulted @51; - accFaultedTemp @115; sensorDataInvalid @52; commIssue @53; commIssueAvgFreq @109; @@ -114,6 +114,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { canBusMissing @111; controlsdLagging @112; resumeBlocked @113; + steerTimeLimit @115; + vehicleSensorsInvalid @116; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; @@ -579,7 +581,7 @@ struct CarParams { noOutput @19; # like silent but without silent CAN TXs hondaBosch @20; volkswagenPq @21; - subaruLegacy @22; # pre-Global platform + subaruPreglobal @22; # pre-Global platform hyundaiLegacy @23; hyundaiCommunity @24; volkswagenMlb @25; @@ -612,6 +614,7 @@ struct CarParams { brand @6 :Text; bus @7 :UInt8; logging @8 :Bool; + obdMultiplexing @9 :Bool; } enum Ecu { @@ -630,10 +633,11 @@ struct CarParams { shiftByWire @16; adas @19; cornerRadar @21; + hvac @20; + parkingAdas @7; # parking assist system ECU, e.g. Toyota's IPAS, Hyundai's RSPA, etc. # Toyota only dsu @6; - apgs @7; # Honda only vsa @13; # Vehicle Stability Assist @@ -642,10 +646,8 @@ struct CarParams { # Chrysler only hcp @18; # Hybrid Control Processor - # Hyundai only - vcu @20; # Vehicle (Motor) Control Unit - debug @17; + unused @22; } enum FingerprintSource { diff --git a/cereal/log.capnp b/cereal/log.capnp index 4e657138c..03aadfba4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -46,6 +46,7 @@ struct InitData { chffrIos @3; tici @4; pc @5; + tizi @6; } struct PandaInfo { @@ -139,16 +140,6 @@ struct FrameData { targetGreyFraction @22 :Float32; exposureValPercent @27 :Float32; - # Focus - lensPos @11 :Int32; - lensSag @12 :Float32; - lensErr @13 :Float32; - lensTruePos @14 :Float32; - focusVal @16 :List(Int16); - focusConf @17 :List(UInt8); - sharpnessScore @18 :List(UInt16); - recoverState @19 :Int32; - transform @10 :List(Float32); image @6 :Data; @@ -171,6 +162,14 @@ struct FrameData { globalGainDEPRECATED @5 :Int32; androidCaptureResultDEPRECATED @9 :AndroidCaptureResult; + lensPosDEPRECATED @11 :Int32; + lensSagDEPRECATED @12 :Float32; + lensErrDEPRECATED @13 :Float32; + lensTruePosDEPRECATED @14 :Float32; + focusValDEPRECATED @16 :List(Int16); + focusConfDEPRECATED @17 :List(UInt8); + sharpnessScoreDEPRECATED @18 :List(UInt16); + recoverStateDEPRECATED @19 :Int32; struct AndroidCaptureResult { sensitivity @0 :Int32; frameDuration @1 :Int64; @@ -409,10 +408,16 @@ struct PandaState @0xa7649e2575e4591e { powerSaveEnabled @16 :Bool; uptime @17 :UInt32; faults @18 :List(FaultType); - harnessStatus @21 :HarnessStatus; heartbeatLost @22 :Bool; interruptLoad @25 :Float32; fanPower @28 :UInt8; + fanStallCount @34 :UInt8; + + spiChecksumErrorCount @33 :UInt16; + + harnessStatus @21 :HarnessStatus; + sbu1Voltage @35 :Float32; + sbu2Voltage @36 :Float32; # can health canState0 @29 :PandaCanState; @@ -428,6 +433,9 @@ struct PandaState @0xa7649e2575e4591e { alternativeExperience @23 :Int16; safetyRxChecksInvalid @32 :Bool; + voltage @0 :UInt32; + current @1 :UInt32; + enum FaultStatus { none @0; faultTemp @1; @@ -460,6 +468,8 @@ struct PandaState @0xa7649e2575e4591e { interruptRateExti @22; interruptRateSpi @23; interruptRateUart7 @24; + sirenMalfunction @25; + heartbeatLoopWatchdog @26; # Update max fault type in boardd when adding faults } @@ -482,15 +492,6 @@ struct PandaState @0xa7649e2575e4591e { flipped @2; } - startedSignalDetectedDEPRECATED @5 :Bool; - voltageDEPRECATED @0 :UInt32; - currentDEPRECATED @1 :UInt32; - hasGpsDEPRECATED @6 :Bool; - fanSpeedRpmDEPRECATED @11 :UInt16; - usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerModeDEPRECATED; - safetyParamDEPRECATED @20 :Int16; - safetyParam2DEPRECATED @26 :UInt32; - struct PandaCanState { busOff @0 :Bool; busOffCnt @1 :UInt32; @@ -525,6 +526,13 @@ struct PandaState @0xa7649e2575e4591e { noChange @7; } } + + startedSignalDetectedDEPRECATED @5 :Bool; + hasGpsDEPRECATED @6 :Bool; + fanSpeedRpmDEPRECATED @11 :UInt16; + usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerModeDEPRECATED; + safetyParamDEPRECATED @20 :Int16; + safetyParam2DEPRECATED @26 :UInt32; } struct PeripheralState { @@ -581,7 +589,7 @@ struct RadarState @0x9a185389d6fdd05f { } struct LiveCalibrationData { - calStatus @1 :Int8; + calStatus @11 :Status; calCycle @2 :Int32; calPerc @3 :Int8; validBlocks @9 :Int32; @@ -595,8 +603,16 @@ struct LiveCalibrationData { wideFromDeviceEuler @10 :List(Float32); warpMatrixDEPRECATED @0 :List(Float32); + calStatusDEPRECATED @1 :Int8; warpMatrix2DEPRECATED @5 :List(Float32); warpMatrixBigDEPRECATED @6 :List(Float32); + + enum Status { + uncalibrated @0; + calibrated @1; + invalid @2; + recalibrating @3; + } } struct LiveTracks { @@ -794,6 +810,17 @@ struct ControlsState @0x97ff69c53601abf1 { canMonoTimesDEPRECATED @21 :List(UInt64); } +# All SI units and in device frame +struct XYZTData @0xc3cbae1fd505ae80 { + x @0 :List(Float32); + y @1 :List(Float32); + z @2 :List(Float32); + t @3 :List(Float32); + xStd @4 :List(Float32); + yStd @5 :List(Float32); + zStd @6 :List(Float32); +} + struct ModelDataV2 { frameId @0 :UInt32; frameIdExtra @20 :UInt32; @@ -827,16 +854,6 @@ struct ModelDataV2 { # Model perceived motion temporalPose @21 :Pose; - # All SI units and in device frame - struct XYZTData { - x @0 :List(Float32); - y @1 :List(Float32); - z @2 :List(Float32); - t @3 :List(Float32); - xStd @4 :List(Float32); - yStd @5 :List(Float32); - zStd @6 :List(Float32); - } struct LeadDataV2 { prob @0 :Float32; # probability that car is your lead at time t @@ -995,6 +1012,11 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { y @1 :List(Float32); } } +struct UiPlan { + frameId @2 :UInt32; + position @0 :XYZTData; + accel @1 :List(Float32); +} struct LateralPlan @0xe1e9318e2ae8b51e { modelMonoTime @31 :UInt64; @@ -1100,6 +1122,7 @@ struct LiveLocationKalman { deviceStable @22 :Bool = true; timeSinceReset @23 :Float64; excessiveResets @24 :Bool; + timeToFirstFix @25 :Float32; enum Status { uninitialized @0; @@ -1172,13 +1195,22 @@ struct GnssMeasurements { gpsTimeOfWeek @2 :Float64; correctedMeasurements @3 :List(CorrectedMeasurement); + ephemerisStatuses @9 :List(EphemerisStatus); kalmanPositionECEF @4 :LiveLocationKalman.Measurement; kalmanVelocityECEF @5 :LiveLocationKalman.Measurement; positionECEF @6 :LiveLocationKalman.Measurement; velocityECEF @7 :LiveLocationKalman.Measurement; + timeToFirstFix @8 :Float32; # Todo sync this with timing pulse of ublox + struct EphemerisStatus { + constellationId @0 :ConstellationId; + svId @1 :UInt8; + type @2 :EphemerisType; + source @3 :EphemerisSource; + } + struct CorrectedMeasurement { constellationId @0 :ConstellationId; svId @1 :UInt8; @@ -1191,11 +1223,11 @@ struct GnssMeasurements { # Satellite position and velocity [x,y,z] satPos @7 :List(Float64); satVel @8 :List(Float64); - ephemerisSource @9 :EphemerisSource; + ephemerisSourceDEPRECATED @9 :EphemerisSourceDEPRECATED; } - struct EphemerisSource { - type @0 :EphemerisSourceType; + struct EphemerisSourceDEPRECATED { + type @0 :EphemerisType; # first epoch in file: gpsWeek @1 :Int16; # -1 if Nav gpsTimeOfWeek @2 :Int32; # -1 if Nav. Integer for seconds is good enough for logs. @@ -1212,13 +1244,20 @@ struct GnssMeasurements { glonass @6; } - enum EphemerisSourceType { + enum EphemerisType { nav @0; # Different ultra-rapid files: nasaUltraRapid @1; glonassIacUltraRapid @2; qcom @3; } + + enum EphemerisSource { + gnssChip @0; + internet @1; + cache @2; + unknown @3; + } } struct UbloxGnss { @@ -1229,6 +1268,19 @@ struct UbloxGnss { hwStatus @3 :HwStatus; hwStatus2 @4 :HwStatus2; glonassEphemeris @5 :GlonassEphemeris; + satReport @6 :SatReport; + } + + struct SatReport { + #received time of week in gps time in seconds and gps week + iTow @0 :UInt32; + svs @1 :List(SatInfo); + + struct SatInfo { + svId @0 :UInt8; + gnssId @1 :UInt8; + flagsBitfield @2 :UInt32; + } } struct MeasurementReport { @@ -1322,7 +1374,7 @@ struct UbloxGnss { iDot @26 :Float64; codesL2 @27 :Float64; - gpsWeek @28 :Float64; + gpsWeekDEPRECATED @28 :Float64; l2 @29 :Float64; svAcc @30 :Float64; @@ -1340,6 +1392,8 @@ struct UbloxGnss { ionoBeta @39 :List(Float64); towCount @40 :UInt32; + toeWeek @41 :UInt16; + tocWeek @42 :UInt16; } struct IonoData { @@ -1418,7 +1472,7 @@ struct UbloxGnss { age @17 :UInt8; svHealth @18 :UInt8; - tk @19 :UInt16; + tkDEPRECATED @19 :UInt16; tb @20 :UInt16; tauN @21 :Float64; @@ -1430,7 +1484,12 @@ struct UbloxGnss { p3 @26 :UInt8; p4 @27 :UInt8; - freqNum @28 :UInt32; + freqNumDEPRECATED @28 :UInt32; + + n4 @29 :UInt8; + nt @30 :UInt16; + freqNum @31 :Int16; + tkSeconds @32 :UInt32; } } @@ -1864,7 +1923,6 @@ struct LiveParametersData { stiffnessFactor @4 :Float32; steerRatio @5 :Float32; sensorValid @6 :Bool; - yawRate @7 :Float32; posenetSpeed @8 :Float32; posenetValid @9 :Bool; angleOffsetFastStd @10 :Float32; @@ -1872,6 +1930,8 @@ struct LiveParametersData { stiffnessFactorStd @12 :Float32; steerRatioStd @13 :Float32; roll @14 :Float32; + + yawRateDEPRECATED @7 :Float32; } struct LiveTorqueParametersData { @@ -2081,6 +2141,7 @@ struct Event { carControl @23 :Car.CarControl; longitudinalPlan @24 :LongitudinalPlan; lateralPlan @64 :LateralPlan; + uiPlan @106 :UiPlan; ubloxGnss @34 :UbloxGnss; ubloxRaw @39 :Data; qcomGnss @31 :QcomGnss; diff --git a/cereal/messaging/messaging.cc b/cereal/messaging/messaging.cc index 48e405634..802662873 100644 --- a/cereal/messaging/messaging.cc +++ b/cereal/messaging/messaging.cc @@ -94,20 +94,3 @@ Poller * Poller::create(std::vector sockets){ } return p; } - -extern "C" Context * messaging_context_create() { - return Context::create(); -} - -extern "C" SubSocket * messaging_subsocket_create(Context* context, const char* endpoint) { - return SubSocket::create(context, std::string(endpoint)); -} - -extern "C" PubSocket * messaging_pubsocket_create(Context* context, const char* endpoint) { - return PubSocket::create(context, std::string(endpoint)); -} - -extern "C" Poller * messaging_poller_create(SubSocket** sockets, int size) { - std::vector socketsVec(sockets, sockets + size); - return Poller::create(socketsVec); -} diff --git a/cereal/services.py b/cereal/services.py index fd1b34312..58f802c6d 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -76,6 +76,7 @@ services = { "navThumbnail": (True, 0.), "navModel": (True, 2., 4.), "mapRenderState": (True, 2., 1.), + "uiPlan": (True, 20., 40.), "qRoadEncodeIdx": (False, 20.), "userFlag": (True, 0., 1), "microphone": (True, 10., 10), diff --git a/cereal/visionipc/visionipc.pxd b/cereal/visionipc/visionipc.pxd index 87999e27b..d508e2e39 100644 --- a/cereal/visionipc/visionipc.pxd +++ b/cereal/visionipc/visionipc.pxd @@ -3,6 +3,7 @@ from libcpp.string cimport string from libcpp.vector cimport vector +from libcpp.set cimport set from libc.stdint cimport uint32_t, uint64_t from libcpp cimport bool @@ -40,3 +41,5 @@ cdef extern from "cereal/visionipc/visionipc_client.h": VisionBuf * recv(VisionIpcBufExtra *, int) bool connect(bool) bool is_connected() + @staticmethod + set[VisionStreamType] getAvailableStreams(string, bool) diff --git a/cereal/visionipc/visionipc_pyx.pyx b/cereal/visionipc/visionipc_pyx.pyx index c36231915..531e7a5ef 100644 --- a/cereal/visionipc/visionipc_pyx.pyx +++ b/cereal/visionipc/visionipc_pyx.pyx @@ -97,3 +97,7 @@ cdef class VisionIpcClient: def is_connected(self): return self.client.is_connected() + + @staticmethod + def available_streams(string name, bool block): + return cppVisionIpcClient.getAvailableStreams(name, block) diff --git a/common/gpio.py b/common/gpio.py index 260f8898a..711fcff85 100644 --- a/common/gpio.py +++ b/common/gpio.py @@ -1,4 +1,5 @@ -from typing import Optional +import glob +from typing import Optional, List def gpio_init(pin: int, output: bool) -> None: try: @@ -23,3 +24,13 @@ def gpio_read(pin: int) -> Optional[bool]: print(f"Failed to set gpio {pin} value: {e}") return val + +def get_irq_for_action(action: str) -> List[int]: + ret = [] + for fn in glob.glob('/sys/kernel/irq/*/actions'): + with open(fn) as f: + actions = f.read().strip().split(',') + if action in actions: + irq = int(fn.split('/')[-2]) + ret.append(irq) + return ret diff --git a/common/i2c.cc b/common/i2c.cc index eb10cd64b..ef788ac9e 100644 --- a/common/i2c.cc +++ b/common/i2c.cc @@ -15,7 +15,7 @@ #define UNUSED(x) (void)(x) #ifdef QCOM2 -// TODO: decide if we want to isntall libi2c-dev everywhere +// TODO: decide if we want to install libi2c-dev everywhere extern "C" { #include #include diff --git a/common/params.cc b/common/params.cc index 2ba4f7e8a..bc1e414fa 100644 --- a/common/params.cc +++ b/common/params.cc @@ -92,15 +92,15 @@ std::unordered_map keys = { {"CameraDebugExpGain", CLEAR_ON_MANAGER_START}, {"CameraDebugExpTime", CLEAR_ON_MANAGER_START}, {"CarBatteryCapacity", PERSISTENT}, - {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CarParamsCache", CLEAR_ON_MANAGER_START}, {"CarParamsPersistent", PERSISTENT}, - {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CompletedTrainingVersion", PERSISTENT}, - {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, - {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, - {"DashcamOverride", PERSISTENT}, - {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, + {"CurrentBootlog", PERSISTENT}, + {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, + {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"DisablePowerDown", PERSISTENT}, {"ExperimentalMode", PERSISTENT}, {"ExperimentalModeConfirmed", PERSISTENT}, @@ -111,7 +111,7 @@ std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"FirmwareObdQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, {"GitCommit", PERSISTENT}, @@ -137,8 +137,8 @@ std::unordered_map keys = { {"IsTestedBranch", CLEAR_ON_MANAGER_START}, {"IsReleaseBranch", CLEAR_ON_MANAGER_START}, {"IsUpdateAvailable", CLEAR_ON_MANAGER_START}, - {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, - {"LaikadEphemerisV2", PERSISTENT | DONT_LOG}, + {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"LaikadEphemerisV3", PERSISTENT | DONT_LOG}, {"LanguageSetting", PERSISTENT}, {"LastAthenaPingTime", CLEAR_ON_MANAGER_START}, {"LastGPSPosition", PERSISTENT}, @@ -150,29 +150,30 @@ std::unordered_map keys = { {"LiveParameters", PERSISTENT}, {"LiveTorqueCarParams", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, - {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, - {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"NavSettingTime24h", PERSISTENT}, {"NavSettingLeftSide", PERSISTENT}, {"NavdRender", PERSISTENT}, - {"ObdMultiplexingDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, + {"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"OpenpilotEnabledToggle", PERSISTENT}, - {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"Passive", PERSISTENT}, {"PrimeType", PERSISTENT}, {"RecordFront", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet - {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"ShouldDoUpdate", CLEAR_ON_MANAGER_START}, - {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"SshEnabled", PERSISTENT}, {"SubscriberInfo", PERSISTENT}, {"TermsVersion", PERSISTENT}, {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, {"UbloxAvailable", PERSISTENT}, - {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"UpdaterState", CLEAR_ON_MANAGER_START}, {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, @@ -190,17 +191,18 @@ std::unordered_map keys = { {"ApiCache_NavDestinations", PERSISTENT}, {"ApiCache_Owner", PERSISTENT}, {"Offroad_BadNvme", CLEAR_ON_MANAGER_START}, - {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START}, {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START}, {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START}, {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START}, {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START}, - {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"Offroad_StorageMissing", CLEAR_ON_MANAGER_START}, {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, + {"Offroad_Recalibration", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, }; } // namespace @@ -304,14 +306,19 @@ std::map Params::readAll() { void Params::clearAll(ParamKeyType key_type) { FileLock file_lock(params_path + "/.lock"); - if (key_type == ALL) { - util::remove_files_in_dir(getParamPath()); - } else { - for (auto &[key, type] : keys) { - if (type & key_type) { - unlink(getParamPath(key).c_str()); + // 1) delete params of key_type + // 2) delete files that are not defined in the keys. + if (DIR *d = opendir(getParamPath().c_str())) { + struct dirent *de = NULL; + while ((de = readdir(d))) { + if (de->d_type != DT_DIR) { + auto it = keys.find(de->d_name); + if (it == keys.end() || (it->second & key_type)) { + unlink(getParamPath(de->d_name).c_str()); + } } } + closedir(d); } fsync_dir(getParamPath()); diff --git a/common/params.h b/common/params.h index aecb3ee47..24b1bffeb 100644 --- a/common/params.h +++ b/common/params.h @@ -7,8 +7,8 @@ enum ParamKeyType { PERSISTENT = 0x02, CLEAR_ON_MANAGER_START = 0x04, - CLEAR_ON_IGNITION_ON = 0x08, - CLEAR_ON_IGNITION_OFF = 0x10, + CLEAR_ON_ONROAD_TRANSITION = 0x08, + CLEAR_ON_OFFROAD_TRANSITION = 0x10, DONT_LOG = 0x20, ALL = 0xFFFFFFFF }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 9d8933609..abb3199d0 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -9,8 +9,8 @@ cdef extern from "common/params.h": cpdef enum ParamKeyType: PERSISTENT CLEAR_ON_MANAGER_START - CLEAR_ON_IGNITION_ON - CLEAR_ON_IGNITION_OFF + CLEAR_ON_ONROAD_TRANSITION + CLEAR_ON_OFFROAD_TRANSITION ALL cdef cppclass c_Params "Params": diff --git a/common/statlog.cc b/common/statlog.cc index 26945882d..587f3e862 100644 --- a/common/statlog.cc +++ b/common/statlog.cc @@ -5,6 +5,7 @@ #include "common/statlog.h" #include "common/util.h" +#include #include #include #include diff --git a/common/swaglog.cc b/common/swaglog.cc index 22682dc54..54f1c3478 100644 --- a/common/swaglog.cc +++ b/common/swaglog.cc @@ -5,6 +5,7 @@ #include "common/swaglog.h" #include +#include #include #include #include diff --git a/common/util.cc b/common/util.cc index 10dff6a9e..ee1080cbd 100644 --- a/common/util.cc +++ b/common/util.cc @@ -99,22 +99,6 @@ std::map read_files_in_dir(const std::string &path) { return ret; } -void remove_files_in_dir(const std::string &path) { - DIR *d = opendir(path.c_str()); - if (!d) return; - - std::string fn; - struct dirent *de = NULL; - while ((de = readdir(d))) { - if (de->d_type != DT_DIR) { - fn = path + "/" + de->d_name; - unlink(fn.c_str()); - } - } - - closedir(d); -} - int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) { int fd = HANDLE_EINTR(open(path, flags, mode)); if (fd == -1) { diff --git a/common/util.h b/common/util.h index 028074384..c80dc234f 100644 --- a/common/util.h +++ b/common/util.h @@ -81,7 +81,6 @@ std::string dir_name(std::string const& path); // **** file fhelpers ***** std::string read_file(const std::string& fn); std::map read_files_in_dir(const std::string& path); -void remove_files_in_dir(const std::string& path); int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664); FILE* safe_fopen(const char* filename, const char* mode); diff --git a/common/version.h b/common/version.h index 7b5764785..5637029cf 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.9.1" +#define COMMA_VERSION "0.9.2" diff --git a/docs/CARS.md b/docs/CARS.md index 886029936..46f8bd278 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -2,249 +2,264 @@ # Supported Cars -A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. +A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. -# 237 Supported Cars +# 252 Supported Cars -|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video| +|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness Kit
 |Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| -|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|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)](##)|GM|| -|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)](##)|GM|| -|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| +|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Buick|LaCrosse 2017-19[3](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None|| -|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| -|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Genesis|GV70 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic Hatchback 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|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)](##)|Hyundai B|| -|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)](##)|Hyundai K|| -|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)](##)|Hyundai E|| -|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)](##)|Hyundai K|| -|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| -|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q|| -|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q|| -|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Hyundai|Ioniq Plug-in Hybrid 2020-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai O|| -|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai I|| -|Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Santa Cruz 2021-22[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| -|Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Hyundai|Sonata 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Tucson Hybrid 2022[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Kia|EV6 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| -|Kia|EV6 (with HDA II) 2022[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| -|Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Kia|Niro Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| -|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Kia|Sorento 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|ES Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|NX Hybrid 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| -|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| -|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B|| -|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| -|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| -|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| -|Ram|1500 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram|| -|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B|| -|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B|| -|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Škoda|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Camry 2021-22|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Polo 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Escape 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Explorer 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Maverick 2022-23|Co-Pilot360 Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai J connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV70 (2.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV70 (3.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai M connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV80 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai M connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic Hatchback 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|HR-V 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Passport 2019-22|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai J connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai Q connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai Q connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai O connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai I connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Santa Cruz 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Sonata 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Tucson Hybrid 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|EV6 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai P connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|EV6 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai P connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|EV6 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX Hybrid 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lincoln|Aviator 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan B connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ram connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Škoda|Fabia 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Škoda|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Škoda|Kodiaq 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry 2021-23|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| 1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.
@@ -275,6 +290,7 @@ If your car has the following packages or features, then it's a good candidate f | Make | Required Package/Features | | ---- | ------------------------- | | Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. | +| Ford | Any car with Lane Centering will likely work. | | Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. | | Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. | | Nissan | Any car with ProPILOT will likely work. | diff --git a/laika/__init__.py b/laika/__init__.py index 661c4584d..787e480e9 100644 --- a/laika/__init__.py +++ b/laika/__init__.py @@ -1,2 +1,9 @@ +import logging +import os + from .astro_dog import AstroDog assert AstroDog + +# setup logging +LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper() +logging.basicConfig(level=LOGLEVEL, format='%(message)s') diff --git a/laika/astro_dog.py b/laika/astro_dog.py index 348c3959a..a2d01946a 100644 --- a/laika/astro_dog.py +++ b/laika/astro_dog.py @@ -9,7 +9,7 @@ from .ephemeris import Ephemeris, EphemerisType, GLONASSEphemeris, GPSEphemeris, from .downloader import download_orbits_gps, download_orbits_russia_src, download_nav, download_ionex, download_dcb, download_prediction_orbits_russia_src from .downloader import download_cors_station from .trop import saast -from .iono import IonexMap, parse_ionex +from .iono import IonexMap, parse_ionex, get_slant_delay from .dcb import DCB, parse_dcbs from .gps_time import GPSTime from .dgps import get_closest_station_names, parse_dgps @@ -33,9 +33,14 @@ class AstroDog: def __init__(self, auto_update=True, cache_dir='/tmp/gnss/', dgps=False, - valid_const=('GPS', 'GLONASS'), + valid_const=(ConstellationId.GPS, ConstellationId.GLONASS), valid_ephem_types=EphemerisType.all_orbits(), clear_old_ephemeris=False): + + for const in valid_const: + if not isinstance(const, ConstellationId): + raise TypeError(f"valid_const must be a list of ConstellationId, got {const}") + self.auto_update = auto_update self.cache_dir = cache_dir self.clear_old_ephemeris = clear_old_ephemeris @@ -44,6 +49,7 @@ class AstroDog: valid_ephem_types = [valid_ephem_types] self.pull_orbit = len(set(EphemerisType.all_orbits()) & set(valid_ephem_types)) > 0 self.pull_nav = EphemerisType.NAV in valid_ephem_types + self.use_qcom_poly = EphemerisType.QCOM_POLY in valid_ephem_types self.valid_const = valid_const self.valid_ephem_types = valid_ephem_types @@ -54,12 +60,14 @@ class AstroDog: self.dgps_delays = [] self.ionex_maps: List[IonexMap] = [] self.orbits: DefaultDict[str, List[PolyEphemeris]] = defaultdict(list) + self.qcom_polys: DefaultDict[str, List[PolyEphemeris]] = defaultdict(list) self.navs: DefaultDict[str, List[Union[GPSEphemeris, GLONASSEphemeris]]] = defaultdict(list) self.dcbs: DefaultDict[str, List[DCB]] = defaultdict(list) self.cached_ionex: Optional[IonexMap] = None self.cached_dgps = None self.cached_orbit: DefaultDict[str, Optional[PolyEphemeris]] = defaultdict(lambda: None) + self.cached_qcom_polys: DefaultDict[str, Optional[PolyEphemeris]] = defaultdict(lambda: None) self.cached_nav: DefaultDict[str, Union[GPSEphemeris, GLONASSEphemeris, None]] = defaultdict(lambda: None) self.cached_dcb: DefaultDict[str, Optional[DCB]] = defaultdict(lambda: None) @@ -96,6 +104,9 @@ class AstroDog: result[prn] = obj return result + def get_all_ephem_prns(self): + return set(self.orbits.keys()).union(set(self.navs.keys())).union(set(self.qcom_polys.keys())) + def get_navs(self, time): if time not in self.navs_fetched_times: self.get_nav_data(time) @@ -108,6 +119,12 @@ class AstroDog: self.cached_orbit[prn] = orbit return orbit + def get_qcom_poly(self, prn: str, time: GPSTime): + poly = self._get_latest_valid_data(self.qcom_polys[prn], self.cached_qcom_polys[prn], None, time, True) + if poly is not None: + self.cached_qcom_polys[prn] = poly + return poly + def get_orbits(self, time): if time not in self.orbit_fetched_times: self.get_orbit_data(time) @@ -129,13 +146,16 @@ class AstroDog: self.cached_dgps = latest_data return latest_data + def add_qcom_polys(self, new_ephems: Dict[str, List[Ephemeris]]): + self._add_ephems(new_ephems, self.qcom_polys) + def add_orbits(self, new_ephems: Dict[str, List[Ephemeris]]): - self._add_ephems(new_ephems, self.orbits, self.orbit_fetched_times) + self._add_ephems(new_ephems, self.orbits) def add_navs(self, new_ephems: Dict[str, List[Ephemeris]]): - self._add_ephems(new_ephems, self.navs, self.navs_fetched_times) + self._add_ephems(new_ephems, self.navs) - def _add_ephems(self, new_ephems: Dict[str, List[Ephemeris]], ephems_dict, fetched_times): + def _add_ephems(self, new_ephems: Dict[str, List[Ephemeris]], ephems_dict): for k, v in new_ephems.items(): if len(v) > 0: if self.clear_old_ephemeris: @@ -143,9 +163,10 @@ class AstroDog: else: ephems_dict[k].extend(v) + def add_ephem_fetched_time(self, ephems, fetched_times): min_epochs = [] max_epochs = [] - for v in new_ephems.values(): + for v in ephems.values(): if len(v) > 0: min_ephem, max_ephem = self.get_epoch_range(v) min_epochs.append(min_ephem) @@ -162,9 +183,9 @@ class AstroDog: fetched_ephems = {} - if 'GPS' in self.valid_const: + if ConstellationId.GPS in self.valid_const: fetched_ephems = download_and_parse(ConstellationId.GPS, parse_rinex_nav_msg_gps) - if 'GLONASS' in self.valid_const: + if ConstellationId.GLONASS in self.valid_const: for k, v in download_and_parse(ConstellationId.GLONASS, parse_rinex_nav_msg_glonass).items(): fetched_ephems.setdefault(k, []).extend(v) self.add_navs(fetched_ephems) @@ -180,7 +201,7 @@ class AstroDog: with ThreadPoolExecutor() as executor: futures_other = [executor.submit(download_orbits_russia_src, t, self.cache_dir, self.valid_ephem_types) for t in time_steps] futures_gps = None - if "GPS" in self.valid_const: + if ConstellationId.GPS in self.valid_const: futures_gps = [executor.submit(download_orbits_gps, t, self.cache_dir, self.valid_ephem_types) for t in time_steps] ephems_other = parse_sp3_orbits([f.result() for f in futures_other if f.result()], self.valid_const, skip_before_epoch) @@ -195,7 +216,7 @@ class AstroDog: result = download_prediction_orbits_russia_src(gps_time, self.cache_dir) if result is not None: result = [result] - elif "GPS" in self.valid_const: + elif ConstellationId.GPS in self.valid_const: # Slower fallback. Russia src prediction orbits are published from 2022 result = [download_orbits_gps(t, self.cache_dir, self.valid_ephem_types) for t in [gps_time - SECS_IN_DAY, gps_time]] if result is None: @@ -209,7 +230,7 @@ class AstroDog: ephems_sp3 = self.download_parse_orbit(time) if sum([len(v) for v in ephems_sp3.values()]) < 5: raise RuntimeError(f'No orbit data found. For Time {time.as_datetime()} constellations {self.valid_const} valid ephem types {self.valid_ephem_types}') - + self.add_ephem_fetched_time(ephems_sp3, self.orbit_fetched_times) self.add_orbits(ephems_sp3) def get_dcb_data(self, time): @@ -257,7 +278,7 @@ class AstroDog: return eph.get_tgd() return None - def get_sat_info(self, prn, time): + def get_eph(self, prn, time): if get_constellation(prn) not in self.valid_const: return None eph = None @@ -265,6 +286,12 @@ class AstroDog: eph = self.get_orbit(prn, time) if not eph and self.pull_nav: eph = self.get_nav(prn, time) + if not eph and self.use_qcom_poly: + eph = self.get_qcom_poly(prn, time) + return eph + + def get_sat_info(self, prn, time): + eph = self.get_eph(prn, time) if eph: return eph.get_sat_info(time) return None @@ -285,7 +312,7 @@ class AstroDog: return None def get_frequency(self, prn, time, signal='C1C'): - if get_constellation(prn) == 'GPS': + if get_constellation(prn) == ConstellationId.GPS: switch = {'1': constants.GPS_L1, '2': constants.GPS_L2, '5': constants.GPS_L5, @@ -296,7 +323,7 @@ class AstroDog: if freq: return freq raise NotImplementedError("Dont know this GPS frequency: ", signal, prn) - elif get_constellation(prn) == 'GLONASS': + elif get_constellation(prn) == ConstellationId.GLONASS: n = self.get_glonass_channel(prn, time) if n is None: return None @@ -319,7 +346,6 @@ class AstroDog: el, az = get_el_az(rcv_pos, sat_pos) if el < 0.2: return None - if self.dgps and not no_dgps: return self._get_delay_dgps(prn, rcv_pos, time) @@ -330,7 +356,11 @@ class AstroDog: # When using internet we expect all data or return None if self.auto_update and (ionex is None or dcb is None or freq is None): return None - iono_delay = ionex.get_delay(rcv_pos, az, el, sat_pos, time, freq) if ionex is not None else 0. + if ionex is not None: + iono_delay = ionex.get_delay(rcv_pos, az, el, sat_pos, time, freq) + else: + # 5m vertical delay is a good default + iono_delay = get_slant_delay(rcv_pos, az, el, sat_pos, time, freq, vertical_delay=5.0) trop_delay = saast(rcv_pos, el) code_bias = dcb.get_delay(signal) if dcb is not None else 0. return iono_delay + trop_delay + code_bias diff --git a/laika/dgps.py b/laika/dgps.py index 53111d90d..60d4393b2 100644 --- a/laika/dgps.py +++ b/laika/dgps.py @@ -8,8 +8,7 @@ from . import raw_gnss as raw from . import opt from .rinex_file import RINEXFile from .downloader import download_cors_coords -from .helpers import get_constellation - +from .helpers import get_constellation, ConstellationId def mean_filter(delay): d2 = delay.copy() @@ -79,7 +78,7 @@ def get_station_position(station_id, cache_dir='/tmp/gnss/', time=GPSTime.from_d return ((time - epoch)/SECS_IN_YEAR)*np.array(vel) + np.array(pos) -def parse_dgps(station_id, station_obs_file_path, dog, max_distance=100000, required_constellations=['GPS']): +def parse_dgps(station_id, station_obs_file_path, dog, max_distance=100000, required_constellations=[ConstellationId.GPS]): station_pos = get_station_position(station_id, cache_dir=dog.cache_dir) obsdata = RINEXFile(station_obs_file_path) measurements = raw.read_rinex_obs(obsdata) @@ -122,7 +121,7 @@ def parse_dgps(station_id, station_obs_file_path, dog, max_distance=100000, requ # could this be biased? Only use GPS for convenience. model_delays = {} for prn in station_delays['C1C']: - if get_constellation(prn) == 'GPS': + if get_constellation(prn) == ConstellationId.GPS: model_delays[prn] = np.nan*np.zeros(n) for i in range(n): model_delays[prn][i] = dog.get_delay(prn, times[i], station_pos, no_dgps=True) diff --git a/laika/downloader.py b/laika/downloader.py index bd456af20..c0bbb584f 100644 --- a/laika/downloader.py +++ b/laika/downloader.py @@ -8,6 +8,7 @@ import pycurl import re import time import socket +import logging from datetime import datetime, timedelta from urllib.parse import urlparse @@ -42,7 +43,7 @@ def retryable(f): try: return f(url_base, *args, **kwargs) except DownloadFailed as e: - print(e) + logging.warning(e) # none of them succeeded raise DownloadFailed("Multiple URL failures attempting to pull file(s)") return wrapped @@ -98,7 +99,7 @@ def ftp_download_files(url_base, folder_path, cacheDir, filenames): if "/" in filename: continue filepath = os.path.join(folder_path_abs, filename) - print("pulling from", url_base, "to", filepath) + logging.debug("pulling from", url_base, "to", filepath) if not os.path.isfile(filepath): os.makedirs(folder_path_abs, exist_ok=True) @@ -141,7 +142,7 @@ def http_download_files(url_base, folder_path, cacheDir, filenames): continue filepath = os.path.join(folder_path_abs, filename) if not os.path.isfile(filepath): - print("pulling from", url_base, "to", filepath) + logging.debug("pulling from", url_base, "to", filepath) os.makedirs(folder_path_abs, exist_ok=True) url_path = url_base + folder_path + filename handle = pycurl.Curl() @@ -170,7 +171,7 @@ def http_download_files(url_base, folder_path, cacheDir, filenames): # if there are downloads left to be done, repeat, and don't overwrite _, requests_processing = fetcher.perform() if requests_processing > 0: - print("some requests stalled, retrying them") + logging.warning("some requests stalled, retrying them") return http_download_files(url_base, folder_path, cacheDir, filenames) return filepaths @@ -229,7 +230,7 @@ def download_files(url_base, folder_path, cacheDir, filenames): @retryable def download_file(url_base, folder_path, filename_zipped): url = url_base + folder_path + filename_zipped - print('Downloading ' + url) + logging.debug('Downloading ' + url) if url.startswith('https://'): return https_download_file(url) elif url.startswith('ftp://'): @@ -478,5 +479,5 @@ def download_cors_station(time, station_name, cache_dir): filepath = download_and_cache_file(url_bases, folder_path, cache_dir+'cors_obs/', filename, compression='.gz') return filepath except DownloadFailed: - print("File not downloaded, check availability on server.") + logging.warning("File not downloaded, check availability on server.") return None diff --git a/laika/ephemeris.capnp b/laika/ephemeris.capnp new file mode 100644 index 000000000..540428639 --- /dev/null +++ b/laika/ephemeris.capnp @@ -0,0 +1,106 @@ +@0xb3ca6d2462778bb1; + +struct Ephemeris { + # This is according to the rinex (2?) format + svId @0 :UInt16; + year @1 :UInt16; + month @2 :UInt16; + day @3 :UInt16; + hour @4 :UInt16; + minute @5 :UInt16; + second @6 :Float32; + af0 @7 :Float64; + af1 @8 :Float64; + af2 @9 :Float64; + + iode @10 :Float64; + crs @11 :Float64; + deltaN @12 :Float64; + m0 @13 :Float64; + + cuc @14 :Float64; + ecc @15 :Float64; + cus @16 :Float64; + a @17 :Float64; # note that this is not the root!! + + toe @18 :Float64; + cic @19 :Float64; + omega0 @20 :Float64; + cis @21 :Float64; + + i0 @22 :Float64; + crc @23 :Float64; + omega @24 :Float64; + omegaDot @25 :Float64; + + iDot @26 :Float64; + codesL2 @27 :Float64; + gpsWeekDEPRECATED @28 :Float64; + l2 @29 :Float64; + + svAcc @30 :Float64; + svHealth @31 :Float64; + tgd @32 :Float64; + iodc @33 :Float64; + + transmissionTime @34 :Float64; + fitInterval @35 :Float64; + + toc @36 :Float64; + + ionoCoeffsValid @37 :Bool; + ionoAlpha @38 :List(Float64); + ionoBeta @39 :List(Float64); + + towCount @40 :UInt32; + toeWeek @41 :UInt16; + tocWeek @42 :UInt16; +} + +struct GlonassEphemeris { + svId @0 :UInt16; + year @1 :UInt16; + dayInYear @2 :UInt16; + hour @3 :UInt16; + minute @4 :UInt16; + second @5 :Float32; + + x @6 :Float64; + xVel @7 :Float64; + xAccel @8 :Float64; + y @9 :Float64; + yVel @10 :Float64; + yAccel @11 :Float64; + z @12 :Float64; + zVel @13 :Float64; + zAccel @14 :Float64; + + svType @15 :UInt8; + svURA @16 :Float32; + age @17 :UInt8; + + svHealth @18 :UInt8; + tkDEPRECATED @19 :UInt16; + tb @20 :UInt16; + + tauN @21 :Float64; + deltaTauN @22 :Float64; + gammaN @23 :Float64; + + p1 @24 :UInt8; + p2 @25 :UInt8; + p3 @26 :UInt8; + p4 @27 :UInt8; + + freqNumDEPRECATED @28 :UInt32; + + n4 @29 :UInt8; + nt @30 :UInt16; + freqNum @31 :Int16; + tkSeconds @32 :UInt32; +} + +struct EphemerisCache { + gpsEphemerides @0 :List(Ephemeris); + glonassEphemerides @1 :List(GlonassEphemeris); +} \ No newline at end of file diff --git a/laika/ephemeris.py b/laika/ephemeris.py index 059fc931c..3615aeec3 100644 --- a/laika/ephemeris.py +++ b/laika/ephemeris.py @@ -1,4 +1,3 @@ -import json import warnings from abc import ABC, abstractmethod from collections import defaultdict @@ -7,7 +6,7 @@ from typing import Dict, List, Optional import numpy as np import numpy.polynomial.polynomial as poly -from datetime import datetime, timedelta +from datetime import datetime from math import sin, cos, sqrt, fabs, atan2 from .gps_time import GPSTime, utc_to_gpst @@ -15,6 +14,12 @@ from .constants import SPEED_OF_LIGHT, SECS_IN_MIN, SECS_IN_HR, SECS_IN_DAY, \ SECS_IN_WEEK, EARTH_ROTATION_RATE, EARTH_GM from .helpers import get_constellation, get_prn_from_nmea_id +import capnp +import os +capnp.remove_import_hook() +capnp_path = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "ephemeris.capnp")) +ephemeris_structs = capnp.load(capnp_path) + def read4(f, rinex_ver): line = f.readline()[:-1] @@ -24,95 +29,6 @@ def read4(f, rinex_ver): return float(line[4:23]), float(line[23:42]), float(line[42:61]), float(line[61:80]) -def convert_ublox_gps_ephem(ublox_ephem, current_time: Optional[datetime] = None): - # Week time of ephemeris gps msg has a roll-over period of 10 bits (19.6 years) - # The latest roll-over was on 2019-04-07 - week = ublox_ephem.gpsWeek - if current_time is None: - # Each message is incremented to be greater or equal than week 1877 (2015-12-27). - # To skip this use the current_time argument - week += 1024 - if week < 1877: - week += 1024 - else: - roll_overs = GPSTime.from_datetime(current_time).week // 1024 - week += (roll_overs - (week // 1024)) * 1024 - - # GPS week refers to current week, the ephemeris can be valid for the next - # if toe equals 0, this can be verified by the TOW count if it is within the - # last 2 hours of the week (gps ephemeris valid for 4hours) - if ublox_ephem.toe == 0 and ublox_ephem.towCount*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR): - week += 1 - - ephem = {} - ephem['sv_id'] = ublox_ephem.svId - ephem['toe'] = GPSTime(week, ublox_ephem.toe) - ephem['toc'] = GPSTime(week, ublox_ephem.toc) - ephem['af0'] = ublox_ephem.af0 - ephem['af1'] = ublox_ephem.af1 - ephem['af2'] = ublox_ephem.af2 - ephem['tgd'] = ublox_ephem.tgd - - ephem['sqrta'] = np.sqrt(ublox_ephem.a) - ephem['dn'] = ublox_ephem.deltaN - ephem['m0'] = ublox_ephem.m0 - - ephem['ecc'] = ublox_ephem.ecc - ephem['w'] = ublox_ephem.omega - ephem['cus'] = ublox_ephem.cus - ephem['cuc'] = ublox_ephem.cuc - ephem['crc'] = ublox_ephem.crc - ephem['crs'] = ublox_ephem.crs - ephem['cic'] = ublox_ephem.cic - ephem['cis'] = ublox_ephem.cis - - ephem['inc'] = ublox_ephem.i0 - ephem['inc_dot'] = ublox_ephem.iDot - ephem['omegadot'] = ublox_ephem.omegaDot - ephem['omega0'] = ublox_ephem.omega0 - - ephem['healthy'] = ublox_ephem.svHealth == 0.0 - - epoch = ephem['toe'] - return GPSEphemeris(ephem, epoch) - - -def convert_ublox_glonass_ephem(ublox_ephem, current_time: Optional[datetime] = None): - ephem = {} - ephem['prn'] = 'R%02i' % ublox_ephem.svId - - etime = datetime.strptime(f"{ublox_ephem.year}-{ublox_ephem.dayInYear}", "%Y-%j") - # glonass time: UTC + 3h - time_in_day = timedelta(hours=ublox_ephem.hour, minutes=ublox_ephem.minute, seconds=ublox_ephem.second) - ephem['toc'] = GPSTime.from_datetime(etime + time_in_day - timedelta(hours=3)) - ephem['toe'] = GPSTime.from_datetime(etime + timedelta(minutes=(ublox_ephem.tb*15 - 180))) - - ephem['x'] = ublox_ephem.x # km - ephem['x_vel'] = ublox_ephem.xVel # km/s - ephem['x_acc'] = ublox_ephem.xAccel # km/s*s - - ephem['y'] = ublox_ephem.y # km - ephem['y_vel'] = ublox_ephem.yVel # km/s - ephem['y_acc'] = ublox_ephem.yAccel # km/s*s - - ephem['z'] = ublox_ephem.z # km - ephem['z_vel'] = ublox_ephem.zVel # km/s - ephem['z_acc'] = ublox_ephem.zAccel # km/s*s - - ephem['healthy'] = ublox_ephem.svHealth == 0.0 - ephem['age'] = ublox_ephem.age # age of information [days] - - # tauN compared to ephemeris from gdc.cddis.eosdis.nasa.gov is times -1 - ephem['min_tauN'] = ublox_ephem.tauN * (-1) # time correction relative to GLONASS tc - ephem['GammaN'] = ublox_ephem.gammaN - - # TODO: channel is in string 7, which is not parsed - ephem['freq_num'] = "1" - - # NOTE: ublox_ephem.tk is in a different format than rinex tk - return GLONASSEphemeris(ephem, ephem['toe']) - - class EphemerisType(IntEnum): # Matches the enum in log.capnp NAV = 0 @@ -138,17 +54,16 @@ class EphemerisType(IntEnum): class Ephemeris(ABC): - def __init__(self, prn: str, data, epoch: GPSTime, eph_type: EphemerisType, healthy: bool, max_time_diff: float, + def __init__(self, prn: str, epoch: GPSTime, eph_type: EphemerisType, healthy: bool, max_time_diff: float, file_epoch: Optional[GPSTime] = None, file_name=None): self.prn = prn - self.data = data self.epoch = epoch self.eph_type = eph_type self.healthy = healthy self.max_time_diff = max_time_diff self.file_epoch = file_epoch + self.file_name = file_name self.file_source = '' if file_name is None else file_name.split('/')[-1][:3] # File source for the ephemeris (e.g. igu, igr, Sta) - self.__json = None def valid(self, time): return abs(time - self.epoch) <= self.max_time_diff @@ -169,54 +84,24 @@ class Ephemeris(ABC): def _get_sat_info(self, time): pass - def to_json(self): - if self.__json is None: - dict = self.__dict__ - dict['ephemeris_class'] = self.__class__.__name__ - self.__json = {'ephemeris': json.dumps(dict, cls=EphemerisSerializer)} - return self.__json - - @classmethod - def from_json(cls, json_dct): - dct = json.loads(json_dct['ephemeris'], object_hook=ephemeris_deserialize_hook) - obj = cls.__new__(globals()[dct['ephemeris_class']]) - obj.__dict__.update(dct) - obj.__json = json_dct - return obj - - -def ephemeris_deserialize_hook(dct): - if 'week' in dct: - return GPSTime(dct['week'], dct['tow']) - return dct - - -class EphemerisSerializer(json.JSONEncoder): - - def default(self, o): - if isinstance(o, GPSTime): - return o.__dict__ - if isinstance(o, np.ndarray): - return o.tolist() - return json.JSONEncoder.default(self, o) - class GLONASSEphemeris(Ephemeris): - def __init__(self, data, epoch, file_name=None): - super().__init__(data['prn'], data, epoch, EphemerisType.NAV, data['healthy'], max_time_diff=25*SECS_IN_MIN, file_name=file_name) - self.channel = data['freq_num'] - self.to_json() + def __init__(self, data, file_name=None): + self.epoch = GPSTime.from_glonass(data.n4, data.nt, data.tb*15*SECS_IN_MIN) + super().__init__('R%02i' % data.svId, self.epoch, EphemerisType.NAV, data.svHealth==0, max_time_diff=25*SECS_IN_MIN, file_name=file_name) + self.data = data + self.epoch = GPSTime.from_glonass(data.n4, data.nt, data.tb*15 * SECS_IN_MIN) + self.channel = data.freqNum def _get_sat_info(self, time: GPSTime): # see the russian doc for this: # http://gauss.gge.unb.ca/GLONASS.ICD.pdf eph = self.data - tdiff = time - utc_to_gpst(eph['toe']) - + tdiff = time - self.epoch # Clock correction (except for general relativity which is applied later) - clock_err = eph['min_tauN'] + tdiff * (eph['GammaN']) - clock_rate_err = eph['GammaN'] + clock_err = -eph.tauN + tdiff * eph.gammaN + clock_rate_err = eph.gammaN def glonass_diff_eq(state, acc): J2 = 1.0826257e-3 @@ -238,14 +123,14 @@ class GLONASSEphemeris(Ephemeris): return ders init_state = np.empty(6) - init_state[0] = eph['x'] - init_state[1] = eph['y'] - init_state[2] = eph['z'] - init_state[3] = eph['x_vel'] - init_state[4] = eph['y_vel'] - init_state[5] = eph['z_vel'] + init_state[0] = eph.x + init_state[1] = eph.y + init_state[2] = eph.z + init_state[3] = eph.xVel + init_state[4] = eph.yVel + init_state[5] = eph.zVel init_state = 1000*init_state - acc = 1000*np.array([eph['x_acc'], eph['y_acc'], eph['z_acc']]) + acc = 1000*np.array([eph.xAccel, eph.yAccel, eph.zAccel]) state = init_state tstep = 90 if tdiff < 0: @@ -271,9 +156,9 @@ class PolyEphemeris(Ephemeris): def __init__(self, prn: str, data, epoch: GPSTime, ephem_type: EphemerisType, file_epoch: GPSTime=None, file_name: str=None, healthy=True, tgd=0, max_time_diff: int=SECS_IN_HR): - super().__init__(prn, data, epoch, ephem_type, healthy, max_time_diff=max_time_diff, file_epoch=file_epoch, file_name=file_name) + super().__init__(prn, epoch, ephem_type, healthy, max_time_diff=max_time_diff, file_epoch=file_epoch, file_name=file_name) + self.data = data self.tgd = tgd - self.to_json() def _get_sat_info(self, time: GPSTime): dt = time - self.data['t0'] @@ -290,27 +175,32 @@ class PolyEphemeris(Ephemeris): class GPSEphemeris(Ephemeris): - def __init__(self, data, epoch, file_name=None): - super().__init__('G%02i' % data['sv_id'], data, epoch, EphemerisType.NAV, data['healthy'], max_time_diff=2*SECS_IN_HR, file_name=file_name) + def __init__(self, data, file_name=None): + self.toe = GPSTime(data.toeWeek, data.toe) + self.toc = GPSTime(data.tocWeek, data.toc) + self.epoch = self.toc + + super().__init__('G%02i' % data.svId, self.epoch, EphemerisType.NAV, data.svHealth==0, max_time_diff=2*SECS_IN_HR, file_name=file_name) self.max_time_diff_tgd = SECS_IN_DAY - self.to_json() + self.data = data + self.sqrta = np.sqrt(data.a) def get_tgd(self): - return self.data['tgd'] + return self.datatgd def _get_sat_info(self, time: GPSTime): eph = self.data - tdiff = time - eph['toc'] # Time of clock - clock_err = eph['af0'] + tdiff * (eph['af1'] + tdiff * eph['af2']) - clock_rate_err = eph['af1'] + 2 * tdiff * eph['af2'] + tdiff = time - self.toc # Time of clock + clock_err = eph.af0 + tdiff * (eph.af1 + tdiff * eph.af2) + clock_rate_err = eph.af1 + 2 * tdiff * eph.af2\ # Orbit propagation - tdiff = time - eph['toe'] # Time of ephemeris (might be different from time of clock) + tdiff = time - self.toe # Time of ephemeris (might be different from time of clock) # Calculate position per IS-GPS-200D p 97 Table 20-IV - a = eph['sqrta'] * eph['sqrta'] # [m] Semi-major axis - ma_dot = sqrt(EARTH_GM / (a * a * a)) + eph['dn'] # [rad/sec] Corrected mean motion - ma = eph['m0'] + ma_dot * tdiff # [rad] Corrected mean anomaly + a = self.sqrta * self.sqrta # [m] Semi-major axis + ma_dot = sqrt(EARTH_GM / (a * a * a)) + eph.deltaN # [rad/sec] Corrected mean motion + ma = eph.m0 + ma_dot * tdiff # [rad] Corrected mean anomaly # Iteratively solve for the Eccentric Anomaly (from Keith Alter and David Johnston) ea = ma # Starting value for E @@ -318,37 +208,37 @@ class GPSEphemeris(Ephemeris): ea_old = 2222 while fabs(ea - ea_old) > 1.0E-14: ea_old = ea - tempd1 = 1.0 - eph['ecc'] * cos(ea_old) - ea = ea + (ma - ea_old + eph['ecc'] * sin(ea_old)) / tempd1 + tempd1 = 1.0 - eph.ecc * cos(ea_old) + ea = ea + (ma - ea_old + eph.ecc * sin(ea_old)) / tempd1 ea_dot = ma_dot / tempd1 # Relativistic correction term - einstein = -4.442807633E-10 * eph['ecc'] * eph['sqrta'] * sin(ea) + einstein = -4.442807633E-10 * eph.ecc * self.sqrta * sin(ea) # Begin calc for True Anomaly and Argument of Latitude - tempd2 = sqrt(1.0 - eph['ecc'] * eph['ecc']) + tempd2 = sqrt(1.0 - eph.ecc * eph.ecc) # [rad] Argument of Latitude = True Anomaly + Argument of Perigee - al = atan2(tempd2 * sin(ea), cos(ea) - eph['ecc']) + eph['w'] + al = atan2(tempd2 * sin(ea), cos(ea) - eph.ecc) + eph.omega al_dot = tempd2 * ea_dot / tempd1 # Calculate corrected argument of latitude based on position - cal = al + eph['cus'] * sin(2.0 * al) + eph['cuc'] * cos(2.0 * al) - cal_dot = al_dot * (1.0 + 2.0 * (eph['cus'] * cos(2.0 * al) - - eph['cuc'] * sin(2.0 * al))) + cal = al + eph.cus * sin(2.0 * al) + eph.cuc * cos(2.0 * al) + cal_dot = al_dot * (1.0 + 2.0 * (eph.cus * cos(2.0 * al) - + eph.cuc * sin(2.0 * al))) # Calculate corrected radius based on argument of latitude - r = a * tempd1 + eph['crc'] * cos(2.0 * al) + eph['crs'] * sin(2.0 * al) - r_dot = (a * eph['ecc'] * sin(ea) * ea_dot + - 2.0 * al_dot * (eph['crs'] * cos(2.0 * al) - - eph['crc'] * sin(2.0 * al))) + r = a * tempd1 + eph.crc * cos(2.0 * al) + eph.crs * sin(2.0 * al) + r_dot = (a * eph.ecc * sin(ea) * ea_dot + + 2.0 * al_dot * (eph.crs * cos(2.0 * al) - + eph.crc * sin(2.0 * al))) # Calculate inclination based on argument of latitude - inc = (eph['inc'] + eph['inc_dot'] * tdiff + - eph['cic'] * cos(2.0 * al) + - eph['cis'] * sin(2.0 * al)) - inc_dot = (eph['inc_dot'] + - 2.0 * al_dot * (eph['cis'] * cos(2.0 * al) - - eph['cic'] * sin(2.0 * al))) + inc = (eph.i0 + eph.iDot * tdiff + + eph.cic * cos(2.0 * al) + + eph.cis * sin(2.0 * al)) + inc_dot = (eph.iDot + + 2.0 * al_dot * (eph.cis * cos(2.0 * al) - + eph.cic * sin(2.0 * al))) # Calculate position and velocity in orbital plane x = r * cos(cal) @@ -357,8 +247,8 @@ class GPSEphemeris(Ephemeris): y_dot = r_dot * sin(cal) + x * cal_dot # Corrected longitude of ascending node - om_dot = eph['omegadot'] - EARTH_ROTATION_RATE - om = eph['omega0'] + tdiff * om_dot - EARTH_ROTATION_RATE * eph['toe'].tow + om_dot = eph.omegaDot - EARTH_ROTATION_RATE + om = eph.omega0 + tdiff * om_dot - EARTH_ROTATION_RATE * self.toe.tow # Compute the satellite's position in Earth-Centered Earth-Fixed coordinates pos = np.empty(3) @@ -507,24 +397,26 @@ def parse_rinex_nav_msg_gps(file_name): line = ' ' + line # Shift 1 char to the right line = line.replace('D', 'E') # Handle bizarro float format - e = {'epoch': epoch, 'sv_id': sv_id} - e['toc'] = epoch + e = {'svId': sv_id} + # TODO are TOC and TOE the same? + e['toc'] = epoch.tow + e['tocWeek'] = epoch.week e['af0'] = float(line[23:42]) e['af1'] = float(line[42:61]) e['af2'] = float(line[61:80]) - e['iode'], e['crs'], e['dn'], e['m0'] = read4(f, rinex_ver) - e['cuc'], e['ecc'], e['cus'], e['sqrta'] = read4(f, rinex_ver) - toe_tow, e['cic'], e['omega0'], e['cis'] = read4(f, rinex_ver) - e['inc'], e['crc'], e['w'], e['omegadot'] = read4(f, rinex_ver) - e['inc_dot'], e['l2_codes'], toe_week, e['l2_pflag'] = read4(f, rinex_ver) - e['sv_accuracy'], e['health'], e['tgd'], e['iodc'] = read4(f, rinex_ver) + e['iode'], e['crs'], e['deltaN'], e['m0'] = read4(f, rinex_ver) + e['cuc'], e['ecc'], e['cus'], sqrta = read4(f, rinex_ver) + e['a'] = sqrta ** 2 + e['toe'], e['cic'], e['omega0'], e['cis'] = read4(f, rinex_ver) + e['i0'], e['crc'], e['omega'], e['omegaDot'] = read4(f, rinex_ver) + e['iDot'], e['codesL2'], e['toeWeek'], l2_pflag = read4(f, rinex_ver) + e['svAcc'], e['svHealth'], e['tgd'], e['iodc'] = read4(f, rinex_ver) f.readline() # Discard last row - e['toe'] = GPSTime(toe_week, toe_tow) - e['healthy'] = (e['health'] == 0.0) + data_struct = ephemeris_structs.Ephemeris.new_message(**e) - ephem = GPSEphemeris(e, epoch, file_name=file_name) + ephem = GPSEphemeris(data_struct, file_name=file_name) ephems[ephem.prn].append(ephem) f.close() return ephems @@ -550,26 +442,39 @@ def parse_rinex_nav_msg_glonass(file_name): got_header = True continue if rinex_ver == 3: - prn = line[:3] - epoch = GPSTime.from_datetime(datetime.strptime(line[4:23], "%y %m %d %H %M %S")) + sv_id = int(line[1:3]) + + epoch = utc_to_gpst(GPSTime.from_datetime(datetime.strptime(line[4:23], "%y %m %d %H %M %S"))) elif rinex_ver == 2: - prn = 'R%02i' % int(line[0:2]) - epoch = GPSTime.from_datetime(datetime.strptime(line[3:20], "%y %m %d %H %M %S")) + sv_id = int(line[0:2]) + epoch = utc_to_gpst(GPSTime.from_datetime(datetime.strptime(line[3:20], "%y %m %d %H %M %S"))) line = ' ' + line # Shift 1 char to the right line = line.replace('D', 'E') # Handle bizarro float format - e = {'epoch': epoch, 'prn': prn} - e['toe'] = epoch - e['min_tauN'] = float(line[23:42]) - e['GammaN'] = float(line[42:61]) - e['tk'] = float(line[61:80]) + e = {'svId': sv_id} + e['n4'], e['nt'], toe_seconds = epoch.as_glonass() + tb = toe_seconds / (15 * SECS_IN_MIN) - e['x'], e['x_vel'], e['x_acc'], e['health'] = read4(f, rinex_ver) - e['y'], e['y_vel'], e['y_acc'], e['freq_num'] = read4(f, rinex_ver) - e['z'], e['z_vel'], e['z_acc'], e['age'] = read4(f, rinex_ver) + + e['tb'] = tb - e['healthy'] = (e['health'] == 0.0) - ephems[prn].append(GLONASSEphemeris(e, epoch, file_name=file_name)) + e['tauN'] = -float(line[23:42]) + e['gammaN'] = float(line[42:61]) + e['tkSeconds'] = float(line[61:80]) + + e['x'], e['xVel'], e['xAccel'], e['svHealth'] = read4(f, rinex_ver) + e['y'], e['yVel'], e['yAccel'], e['freqNum'] = read4(f, rinex_ver) + e['z'], e['zVel'], e['zAccel'], e['age'] = read4(f, rinex_ver) + + # TODO unclear why glonass sometimes has nav messages 3s after correct one + if abs(tb - int(tb)) > 1e-3: + continue + + + data_struct = ephemeris_structs.GlonassEphemeris.new_message(**e) + ephem = GLONASSEphemeris(data_struct, file_name=file_name) + + ephems[ephem.prn].append(ephem) f.close() return ephems @@ -580,9 +485,9 @@ def parse_qcom_ephem(qcom_poly, current_week): t0 = data.t0 # fix glonass time prn = get_prn_from_nmea_id(svId) - if prn == 'GLONASS': + if prn[0] == 'R': # TODO should handle leap seconds better - epoch = GPSTime(current_week, (t0 + 3*SECS_IN_WEEK) % (SECS_IN_WEEK) + 18) + epoch = GPSTime(current_week, (t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + 18) else: epoch = GPSTime(current_week, t0) poly_data = {} @@ -595,4 +500,4 @@ def parse_qcom_ephem(qcom_poly, current_week): poly_data['clock'] = [1e-3*data.other[3], 1e-3*data.other[2], 1e-3*data.other[1], 1e-3*data.other[0]] poly_data['deg'] = 3 poly_data['deg_t'] = 3 - return PolyEphemeris(prn, poly_data, epoch, ephem_type=EphemerisType.QCOM_POLY, max_time_diff=180) + return PolyEphemeris(prn, poly_data, epoch, ephem_type=EphemerisType.QCOM_POLY, max_time_diff=300, file_name='qcom') diff --git a/laika/gps_time.py b/laika/gps_time.py index b9492f52d..293ff4548 100644 --- a/laika/gps_time.py +++ b/laika/gps_time.py @@ -152,6 +152,17 @@ class GPSTime: def as_datetime(self): return tow_to_datetime(self.tow, self.week) + def as_glonass(self): + time_utc = gpst_to_utc(self) + datetime_utc = time_utc.as_datetime() + datetime_glonass = datetime_utc + datetime.timedelta(hours=3) + + year = datetime_glonass.year + cycle = (year - 1992) // 4 + days = (datetime_glonass - datetime.datetime(1992 + cycle*4, 1, 1)).days + 1 + tod = (datetime_glonass - datetime_glonass.replace(hour=0, minute=0, second=0, microsecond=0)).total_seconds() + return cycle, days, tod + def as_unix_timestamp(self): return (gpst_to_utc(self).as_datetime() - datetime.datetime(1970, 1, 1)).total_seconds() diff --git a/laika/helpers.py b/laika/helpers.py index a0480d633..064708156 100644 --- a/laika/helpers.py +++ b/laika/helpers.py @@ -26,7 +26,7 @@ class ConstellationId(IntEnum): if c in RINEX_ID_TO_CONSTELLATION: return RINEX_ID_TO_CONSTELLATION[c] else: - return None + raise ValueError("Unknown rinex constellation id: ", c) @classmethod def from_qcom_source(cls, report_source: int): @@ -97,14 +97,12 @@ def get_closest(time, candidates, recv_pos=None): default=None, ) - def get_constellation(prn: str): identifier = prn[0] - constellation = ConstellationId.from_rinex_char(identifier) - if constellation is not None: - return constellation.name - return None + return ConstellationId.from_rinex_char(identifier) +def get_sv_id(prn: str): + return int(prn[1:]) def get_constellation_and_sv_id(nmea_id): for c, ranges in CONSTELLATION_TO_NMEA_RANGES.items(): @@ -123,11 +121,8 @@ def get_prn_from_nmea_id(nmea_id: int): def get_nmea_id_from_prn(prn: str): constellation = get_constellation(prn) - if constellation is None: - raise ValueError(f"Constellation not found for prn {prn}") - sv_id = int(prn[1:]) # satellite id - return get_nmea_id_from_constellation_and_svid(ConstellationId[constellation], sv_id) + return get_nmea_id_from_constellation_and_svid(constellation, sv_id) def get_nmea_id_from_constellation_and_svid(constellation: ConstellationId, sv_id: int): diff --git a/laika/iono.py b/laika/iono.py index 16bad87fe..0558fdbea 100644 --- a/laika/iono.py +++ b/laika/iono.py @@ -9,6 +9,17 @@ from .gps_time import GPSTime # Altitude of Ionospheric-pierce-point IPP_ALT = 6821000 +def get_alpha_beta(rcv_pos, el): + geocentric_alt = np.linalg.norm(rcv_pos) + alpha = np.pi/2 + el + beta = np.arcsin(geocentric_alt*np.sin(alpha)/IPP_ALT) + return alpha, beta + +def get_slant_delay(rcv_pos, az, el, sat_pos, time, freq, vertical_delay): + alpha, beta = get_alpha_beta(rcv_pos, el) + slant_delay = vertical_delay * ((1 - ((EARTH_RADIUS * np.sin(beta)) / + (EARTH_RADIUS + 3.5e5))**2)**(-0.5)) + return slant_delay def closest_in_list(lst, val, num=2): """ @@ -134,18 +145,16 @@ class IonexMap: # To get a delay from a TEC map, we need to calculate # the ionospheric pierce point, geometry described here # https://en.wikipedia.org/wiki/Ionospheric_pierce_point + alpha, beta = get_alpha_beta(rcv_pos, el) conv = LocalCoord.from_ecef(rcv_pos) - geocentric_alt = np.linalg.norm(rcv_pos) - alpha = np.pi/2 + el - beta = np.arcsin(geocentric_alt*np.sin(alpha)/IPP_ALT) gamma = np.pi - alpha - beta + geocentric_alt = np.linalg.norm(rcv_pos) ipp_dist = geocentric_alt*np.sin(gamma)/np.sin(beta) ipp_ned = conv.ecef2ned(sat_pos)*(ipp_dist)/np.linalg.norm(sat_pos) ipp_geo = conv.ned2geodetic(ipp_ned) factor = 40.30E16 / (freq**2) * 10**(self.exp) vertical_delay = self.get_TEC(ipp_geo, time) * factor - slant_delay = vertical_delay * ((1 - ((EARTH_RADIUS * np.sin(beta)) / - (EARTH_RADIUS + 3.5e5))**2)**(-0.5)) + slant_delay = get_slant_delay(rcv_pos, az, el, sat_pos, time, freq, vertical_delay) return slant_delay @staticmethod diff --git a/laika/opt.py b/laika/opt.py index 1a76f353c..dfa8ae5ca 100644 --- a/laika/opt.py +++ b/laika/opt.py @@ -7,22 +7,28 @@ from .helpers import ConstellationId from .raw_gnss import GNSSMeasurement -def gauss_newton(fun, b, xtol=1e-8, max_n=25): +def gauss_newton(fun, b, M, xtol=1e-8, max_n=25): + + W = np.linalg.inv(M) for _ in range(max_n): # Compute function and jacobian on current estimate r, J = fun(b) - # Update estimate - delta = np.linalg.pinv(J) @ r + # Update estimate, WLS https://en.wikipedia.org/wiki/Weighted_least_squares + delta = np.linalg.pinv(J.T.dot(W).dot(J)).dot(J.T).dot(W) @ r b -= delta # Check step size for stopping condition if np.linalg.norm(delta) < xtol: break - return b + + r, J = fun(b) + Mb = np.linalg.pinv(J.T.dot(W).dot(J)) + x_std = np.sqrt(np.diagonal(Mb)) + return b, r, x_std -def calc_pos_fix(measurements, posfix_functions=None, x0=None, no_weight=False, signal='C1C', min_measurements=6): +def calc_pos_fix(measurements, posfix_functions=None, x0=None, signal='C1C', min_measurements=5): ''' Calculates gps fix using gauss newton method To solve the problem a minimal of 4 measurements are required. @@ -35,15 +41,16 @@ def calc_pos_fix(measurements, posfix_functions=None, x0=None, no_weight=False, x0 = [0, 0, 0, 0, 0] if len(measurements) < min_measurements: - return [],[] + return [],[],[] - Fx_pos = pr_residual(measurements, posfix_functions, signal=signal, no_weight=no_weight, no_nans=True) - x = gauss_newton(Fx_pos, x0) - residual, _ = Fx_pos(x, no_weight=True) - return x.tolist(), residual.tolist() + Fx_pos = pr_residual(measurements, posfix_functions, signal=signal, no_nans=True) + meas_cov = np.diag([meas.observables_std[signal]**2 for meas in measurements]) + + x, residual, x_std = gauss_newton(Fx_pos, x0, meas_cov) + return x.tolist(), residual.tolist(), x_std -def calc_vel_fix(measurements, est_pos, velfix_function=None, v0=None, no_weight=False, signal='D1C', min_measurements=6): +def calc_vel_fix(measurements, est_pos, velfix_function=None, v0=None, signal='D1C', min_measurements=5): ''' Calculates gps velocity fix using gauss newton method returns: @@ -54,12 +61,13 @@ def calc_vel_fix(measurements, est_pos, velfix_function=None, v0=None, no_weight v0 = [0, 0, 0, 0] if len(measurements) < min_measurements: - return [], [] + return [], [], [] - Fx_vel = prr_residual(measurements, est_pos, velfix_function, signal=signal, no_weight=no_weight, no_nans=True) - v = gauss_newton(Fx_vel, v0) - residual, _ = Fx_vel(v, no_weight=True) - return v.tolist(), residual.tolist() + Fx_vel = prr_residual(measurements, est_pos, velfix_function, signal=signal, no_nans=True) + meas_cov = np.diag([meas.observables_std[signal]**2 for meas in measurements]) + + v, residual, x_std = gauss_newton(Fx_vel, v0, meas_cov) + return v.tolist(), residual.tolist(), x_std def get_posfix_sympy_fun(constellation): @@ -73,7 +81,6 @@ def get_posfix_sympy_fun(constellation): # Knowns pr = sympy.Symbol('pr') sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z') - weight = sympy.Symbol('weight') theta = (EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT)*zero_theta val = sympy.sqrt( @@ -83,15 +90,15 @@ def get_posfix_sympy_fun(constellation): ) if constellation == ConstellationId.GLONASS: - res = weight * (val - (pr - bc - bg)) + res = val - (pr - bc - bg) elif constellation == ConstellationId.GPS: - res = weight * (val - (pr - bc)) + res = val - (pr - bc) else: raise NotImplementedError(f"Constellation {constellation} not supported") res = [res] + [sympy.diff(res, v) for v in var] - return sympy.lambdify([x, y, z, bc, bg, pr, zero_theta, sat_x, sat_y, sat_z, weight], res, modules=["numpy"]) + return sympy.lambdify([x, y, z, bc, bg, pr, zero_theta, sat_x, sat_y, sat_z], res, modules=["numpy"]) def get_velfix_sympy_func(): @@ -105,7 +112,6 @@ def get_velfix_sympy_func(): sv_x, sv_y, sv_z = sympy.Symbol('sv_x'), sympy.Symbol('sv_y'), sympy.Symbol('sv_z') sat_vel = np.array([sv_x, sv_y, sv_z]) observables = sympy.Symbol('observables') - weight = sympy.Symbol('weight') # unknown, receiver velocity v_x, v_y, v_z = sympy.Symbol('v_x'), sympy.Symbol('v_y'), sympy.Symbol('v_z') @@ -114,27 +120,24 @@ def get_velfix_sympy_func(): loss = sat_pos - est_pos loss /= sympy.sqrt(loss.dot(loss)) - - nv = loss.dot(sat_vel - vel) - ov = (observables - vel_o) - res = (nv - ov)*weight + res = loss.dot(sat_vel - vel) - (observables - vel_o) res = [res] + [sympy.diff(res, v) for v in [v_x, v_y, v_z, vel_o]] return sympy.lambdify([ ep_x, ep_y, ep_z, sp_x, sp_y, sp_z, - sv_x, sv_y, sv_z, observables, weight, + sv_x, sv_y, sv_z, observables, v_x, v_y, v_z, vel_o ], res, modules=["numpy"]) -def pr_residual(measurements: List[GNSSMeasurement], posfix_functions=None, signal='C1C', no_weight=False, no_nans=False): +def pr_residual(measurements: List[GNSSMeasurement], posfix_functions=None, signal='C1C', no_nans=False): if posfix_functions is None: posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} - def Fx_pos(inp, no_weight=no_weight): + def Fx_pos(inp): vals, gradients = [], [] for meas in measurements: @@ -153,20 +156,19 @@ def pr_residual(measurements: List[GNSSMeasurement], posfix_functions=None, sign gradients.append(np.nan) continue - w = 1.0 if no_weight or meas.observables_std[signal] == 0 else (1 / meas.observables_std[signal]) - val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, zero_theta, *sat_pos, w) + val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, zero_theta, *sat_pos) vals.append(val) gradients.append(gradient) return np.asarray(vals), np.asarray(gradients) return Fx_pos -def prr_residual(measurements: List[GNSSMeasurement], est_pos, velfix_function=None, signal='D1C', no_weight=False, no_nans=False): +def prr_residual(measurements: List[GNSSMeasurement], est_pos, velfix_function=None, signal='D1C', no_nans=False): if velfix_function is None: velfix_function = get_velfix_sympy_func() - def Fx_vel(vel, no_weight=no_weight): + def Fx_vel(vel): vals, gradients = [], [] for meas in measurements: @@ -177,12 +179,11 @@ def prr_residual(measurements: List[GNSSMeasurement], est_pos, velfix_function=N continue sat_pos = meas.sat_pos_final if meas.corrected else meas.sat_pos - weight = 1.0 if no_weight or meas.observables_std[signal] == 0 else (1 / meas.observables_std[signal]) val, *gradient = velfix_function(est_pos[0], est_pos[1], est_pos[2], sat_pos[0], sat_pos[1], sat_pos[2], meas.sat_vel[0], meas.sat_vel[1], meas.sat_vel[2], - meas.observables[signal], weight, + meas.observables[signal], vel[0], vel[1], vel[2], vel[3]) vals.append(val) gradients.append(gradient) diff --git a/laika/raw_gnss.py b/laika/raw_gnss.py index b1f60ab9b..4e3b301c6 100644 --- a/laika/raw_gnss.py +++ b/laika/raw_gnss.py @@ -92,10 +92,13 @@ class GNSSMeasurement: self.processed = True return True - def correct(self, est_pos, dog): + def correct(self, est_pos, dog, correct_delay=True): for obs in self.observables: if obs[0] == 'C': # or obs[0] == 'L': - delay = dog.get_delay(self.prn, self.recv_time, est_pos, signal=obs) + if correct_delay: + delay = dog.get_delay(self.prn, self.recv_time, est_pos, signal=obs) + else: + delay = 0.0 if delay is not None: self.observables_final[obs] = (self.observables[obs] + self.sat_clock_err*constants.SPEED_OF_LIGHT - @@ -147,10 +150,10 @@ def process_measurements(measurements: List[GNSSMeasurement], dog) -> List[GNSSM return proc_measurements -def correct_measurements(measurements: List[GNSSMeasurement], est_pos, dog) -> List[GNSSMeasurement]: +def correct_measurements(measurements: List[GNSSMeasurement], est_pos, dog, correct_delay=True) -> List[GNSSMeasurement]: corrected_measurements = [] for meas in measurements: - if meas.correct(est_pos, dog): + if meas.correct(est_pos, dog, correct_delay=correct_delay): corrected_measurements.append(meas) return corrected_measurements @@ -195,12 +198,12 @@ def read_raw_qcom(report): time_bias_ms = report.timeBias else: raise NotImplementedError('Only GPS (0), SBAS (1) and GLONASS (6) are supported from qcom, not:', {report.source}) - #print(recv_time, report.source, time_bias_ms, dr) + # logging.debug(recv_time, report.source, time_bias_ms, dr) measurements = [] for i in report.sv: nmea_id = i.svId # todo change svId to nmea_id in cereal message. Or better: change the publisher to publish correct svId's, since constellation id is also given if nmea_id == 255: - # todo nmea_id is not valid. Fix publisher + # TODO nmea_id is not valid. Fix publisher continue _, sv_id = get_constellation_and_sv_id(nmea_id) if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown: @@ -217,9 +220,9 @@ def read_raw_qcom(report): observables_std['D1C'] = i.unfilteredSpeedUncertainty observables['S1C'] = (i.carrierNoise/100.) if i.carrierNoise != 0 else np.nan observables['L1C'] = np.nan - #print(" %.5f %3d %10.2f %7.2f %7.2f %.2f %d" % (recv_time.tow, nmea_id, - # observables['C1C'], observables_std['C1C'], - # observables_std['D1C'], observables['S1C'], i.latency), i.observationState, i.measurementStatus.fineOrCoarseVelocity) + # logging.debug(" %.5f %3d %10.2f %7.2f %7.2f %.2f %d" % (recv_time.tow, nmea_id, + # observables['C1C'], observables_std['C1C'], + # observables_std['D1C'], observables['S1C'], i.latency), i.observationState, i.measurementStatus.fineOrCoarseVelocity) glonass_freq = (i.glonassFrequencyIndex - 7) if constellation_id == ConstellationId.GLONASS else np.nan measurements.append(GNSSMeasurement(constellation_id, sv_id, recv_time.week, diff --git a/laika/rinex_file.py b/laika/rinex_file.py index 438506191..f0697edde 100644 --- a/laika/rinex_file.py +++ b/laika/rinex_file.py @@ -11,7 +11,7 @@ import datetime import numpy as np - +import logging def floatornan(x): if x == '' or x[-1] == ' ': @@ -49,10 +49,10 @@ class RINEXFile: self._read_header(f) self._read_data(f) except TypeError: - print("TypeError, file likely not downloaded.") + logging.exception("TypeError, file likely not downloaded.") raise DownloadError("file download failure") except FileNotFoundError: - print("File not found in directory.") + logging.exception("File not found in directory.") raise DownloadError("file missing in download cache") def _read_header(self, f): version_line = padline(f.readline(), 80) diff --git a/launch_env.sh b/launch_env.sh index 3059ec268..c1ecbe3b3 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="6.2" + export AGNOS_VERSION="7.1" fi if [ -z "$PASSIVE" ]; then diff --git a/opendbc/.gitignore b/opendbc/.gitignore index bf8570d8b..d5c2eb4a3 100644 --- a/opendbc/.gitignore +++ b/opendbc/.gitignore @@ -9,6 +9,7 @@ .sconsign.dblite can/*.so +can/*.a can/build/ can/obj/ can/packer_pyx.cpp diff --git a/opendbc/acura_ilx_2016_can_generated.dbc b/opendbc/acura_ilx_2016_can_generated.dbc index 854c36ee0..1afb5bade 100644 --- a/opendbc/acura_ilx_2016_can_generated.dbc +++ b/opendbc/acura_ilx_2016_can_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/acura_rdx_2018_can_generated.dbc b/opendbc/acura_rdx_2018_can_generated.dbc index c9a8dd6d7..6596b4451 100644 --- a/opendbc/acura_rdx_2018_can_generated.dbc +++ b/opendbc/acura_rdx_2018_can_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/acura_rdx_2020_can_generated.dbc b/opendbc/acura_rdx_2020_can_generated.dbc index 0b8162f05..c6149921d 100644 --- a/opendbc/acura_rdx_2020_can_generated.dbc +++ b/opendbc/acura_rdx_2020_can_generated.dbc @@ -357,7 +357,7 @@ BO_ 13274 LKAS_HUD_A: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -378,7 +378,7 @@ BO_ 13275 LKAS_HUD_B: 8 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -436,7 +436,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/can/SConscript b/opendbc/can/SConscript index 9835482d5..f07234c11 100644 --- a/opendbc/can/SConscript +++ b/opendbc/can/SConscript @@ -5,7 +5,14 @@ import os envDBC = env.Clone() dbc_file_path = '-DDBC_FILE_PATH=\'"%s"\'' % (envDBC.Dir("..").abspath) envDBC['CXXFLAGS'] += [dbc_file_path] -libdbc = envDBC.SharedLibrary('libdbc', ["dbc.cc", "parser.cc", "packer.cc", "common.cc"], LIBS=[common, "capnp", "kj", "zmq"]) +src = ["dbc.cc", "parser.cc", "packer.cc", "common.cc"] +libs = [common, "capnp", "kj", "zmq"] + +# shared library for openpilot +libdbc = envDBC.SharedLibrary('libdbc', src, LIBS=libs) + +# static library for tools like cabana +envDBC.Library('libdbc_static', src, LIBS=libs) # Build packer and parser lenv = envCython.Clone() diff --git a/opendbc/can/common.h b/opendbc/can/common.h index ba9ce0234..0031d787f 100644 --- a/opendbc/can/common.h +++ b/opendbc/can/common.h @@ -37,6 +37,7 @@ unsigned int pedal_checksum(uint32_t address, const Signal &sig, const std::vect class MessageState { public: + std::string name; uint32_t address; unsigned int size; @@ -80,11 +81,12 @@ public: CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter); #ifndef DYNAMIC_CAPNP void update_string(const std::string &data, bool sendcan); + void update_strings(const std::vector &data, std::vector &vals, bool sendcan); void UpdateCans(uint64_t sec, const capnp::List::Reader& cans); #endif void UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cans); void UpdateValid(uint64_t sec); - std::vector query_latest(); + void query_latest(std::vector &vals, uint64_t last_ts = 0); }; class CANPacker { diff --git a/opendbc/can/common.pxd b/opendbc/can/common.pxd index c863b65a7..2d3a8eefd 100644 --- a/opendbc/can/common.pxd +++ b/opendbc/can/common.pxd @@ -78,9 +78,8 @@ cdef extern from "common.h": bool can_valid bool bus_timeout CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions]) - void update_string(string, bool) - vector[SignalValue] query_latest() + void update_strings(vector[string]&, vector[SignalValue]&, bool) cdef cppclass CANPacker: CANPacker(string) - vector[uint8_t] pack(uint32_t, vector[SignalPackValue]) + vector[uint8_t] pack(uint32_t, vector[SignalPackValue]&) diff --git a/opendbc/can/common_dbc.h b/opendbc/can/common_dbc.h index 20a8ef3fd..1e0f03452 100644 --- a/opendbc/can/common_dbc.h +++ b/opendbc/can/common_dbc.h @@ -84,6 +84,6 @@ typedef struct ChecksumState { } ChecksumState; DBC* dbc_parse(const std::string& dbc_path); -DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, ChecksumState *checksum = nullptr); +DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, ChecksumState *checksum = nullptr, bool allow_duplicate_msg_name=false); const DBC* dbc_lookup(const std::string& dbc_name); std::vector get_dbc_names(); diff --git a/opendbc/can/dbc.cc b/opendbc/can/dbc.cc index 11231ebd7..22317169b 100644 --- a/opendbc/can/dbc.cc +++ b/opendbc/can/dbc.cc @@ -98,10 +98,11 @@ void set_signal_type(Signal& s, ChecksumState* chk, const std::string& dbc_name, } } -DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, ChecksumState *checksum) { +DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, ChecksumState *checksum, bool allow_duplicate_msg_name) { uint32_t address = 0; std::set address_set; std::set msg_name_set; + std::map> signal_name_sets; std::map> signals; DBC* dbc = new DBC; dbc->name = dbc_name; @@ -134,8 +135,11 @@ DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, Ch // check for duplicates DBC_ASSERT(address_set.find(address) == address_set.end(), "Duplicate message address: " << address << " (" << msg.name << ")"); address_set.insert(address); - DBC_ASSERT(msg_name_set.find(msg.name) == msg_name_set.end(), "Duplicate message name: " << msg.name); - msg_name_set.insert(msg.name); + + if (!allow_duplicate_msg_name) { + DBC_ASSERT(msg_name_set.find(msg.name) == msg_name_set.end(), "Duplicate message name: " << msg.name); + msg_name_set.insert(msg.name); + } } else if (startswith(line, "SG_ ")) { // new signal int offset = 0; @@ -162,6 +166,10 @@ DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, Ch sig.msb = sig.start_bit; } DBC_ASSERT(sig.lsb < (64 * 8) && sig.msb < (64 * 8), "Signal out of bounds: " << line); + + // Check for duplicate signal names + DBC_ASSERT(signal_name_sets[address].find(sig.name) == signal_name_sets[address].end(), "Duplicate signal name: " << sig.name); + signal_name_sets[address].insert(sig.name); } else if (startswith(line, "VAL_ ")) { // new signal value/definition bool ret = std::regex_search(line, match, val_regexp); diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc index 754c09df9..db6d4e65e 100644 --- a/opendbc/can/parser.cc +++ b/opendbc/can/parser.cc @@ -33,7 +33,7 @@ int64_t get_raw_value(const std::vector &msg, const Signal &sig) { bool MessageState::parse(uint64_t sec, const std::vector &dat) { for (int i = 0; i < parse_sigs.size(); i++) { - auto &sig = parse_sigs[i]; + const auto &sig = parse_sigs[i]; int64_t tmp = get_raw_value(dat, sig); if (sig.is_signed) { @@ -124,6 +124,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, assert(false); } + state.name = msg->name; state.size = msg->size; assert(state.size <= 64); // max signal size is 64 bytes @@ -162,6 +163,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum for (const auto& msg : dbc->msgs) { MessageState state = { + .name = msg.name, .address = msg.address, .size = msg.size, .ignore_checksum = ignore_checksum, @@ -202,14 +204,24 @@ void CANParser::update_string(const std::string &data, bool sendcan) { UpdateValid(last_sec); } +void CANParser::update_strings(const std::vector &data, std::vector &vals, bool sendcan) { + uint64_t current_sec = 0; + for (const auto &d : data) { + update_string(d, sendcan); + if (current_sec == 0) { + current_sec = last_sec; + } + } + query_latest(vals, current_sec); +} + void CANParser::UpdateCans(uint64_t sec, const capnp::List::Reader& cans) { //DEBUG("got %d messages\n", cans.size()); bool bus_empty = true; // parse the messages - for (int i = 0; i < cans.size(); i++) { - auto cmsg = cans[i]; + for (const auto cmsg : cans) { if (cmsg.getSrc() != bus) { // DEBUG("skip %d: wrong bus\n", cmsg.getAddress()); continue; @@ -287,9 +299,9 @@ void CANParser::UpdateValid(uint64_t sec) { if (state.check_threshold > 0 && (missing || timed_out)) { if (show_missing && !bus_timeout) { if (missing) { - LOGE("0x%X NOT SEEN", state.address); + LOGE("0x%X '%s' NOT SEEN", state.address, state.name.c_str()); } else if (timed_out) { - LOGE("0x%X TIMED OUT", state.address); + LOGE("0x%X '%s' TIMED OUT", state.address, state.name.c_str()); } } _valid = false; @@ -299,25 +311,25 @@ void CANParser::UpdateValid(uint64_t sec) { can_valid = (can_invalid_cnt < CAN_INVALID_CNT) && _counters_valid; } -std::vector CANParser::query_latest() { - std::vector ret; - +void CANParser::query_latest(std::vector &vals, uint64_t last_ts) { + if (last_ts == 0) { + last_ts = last_sec; + } for (auto& kv : message_states) { auto& state = kv.second; - if (last_sec != 0 && state.last_seen_nanos != last_sec) continue; + if (last_ts != 0 && state.last_seen_nanos < last_ts) { + continue; + } for (int i = 0; i < state.parse_sigs.size(); i++) { const Signal &sig = state.parse_sigs[i]; - ret.push_back((SignalValue){ - .address = state.address, - .ts_nanos = state.last_seen_nanos, - .name = sig.name, - .value = state.vals[i], - .all_values = state.all_vals[i], - }); + SignalValue &v = vals.emplace_back(); + v.address = state.address; + v.ts_nanos = state.last_seen_nanos; + v.name = sig.name; + v.value = state.vals[i]; + v.all_values = state.all_vals[i]; state.all_vals[i].clear(); } } - - return ret; } diff --git a/opendbc/can/parser_pyx.pyx b/opendbc/can/parser_pyx.pyx index 9867305ef..be88859e1 100644 --- a/opendbc/can/parser_pyx.pyx +++ b/opendbc/can/parser_pyx.pyx @@ -1,6 +1,7 @@ # distutils: language = c++ # cython: c_string_encoding=ascii, language_level=3 +from cython.operator cimport dereference as deref, preincrement as preinc from libcpp.string cimport string from libcpp.vector cimport vector from libcpp.unordered_set cimport unordered_set @@ -51,7 +52,7 @@ cdef class CANParser: self.address_to_msg_name[msg.address] = name self.vl[msg.address] = {} self.vl[name] = self.vl[msg.address] - self.vl_all[msg.address] = defaultdict(list) + self.vl_all[msg.address] = {} self.vl_all[name] = self.vl_all[msg.address] self.ts_nanos[msg.address] = {} self.ts_nanos[name] = self.ts_nanos[msg.address] @@ -101,37 +102,29 @@ cdef class CANParser: message_options_v.push_back(mpo) self.can = new cpp_CANParser(bus, dbc_name, message_options_v, signal_options_v) - self.update_vl() - - cdef unordered_set[uint32_t] update_vl(self): - cdef unordered_set[uint32_t] updated_addrs - - new_vals = self.can.query_latest() - for cv in new_vals: - # Cast char * directly to unicode - cv_name = cv.name - self.vl[cv.address][cv_name] = cv.value - self.vl_all[cv.address][cv_name].extend(cv.all_values) - self.ts_nanos[cv.address][cv_name] = cv.ts_nanos - updated_addrs.insert(cv.address) - - return updated_addrs - - def update_string(self, dat, sendcan=False): - for v in self.vl_all.values(): - v.clear() - - self.can.update_string(dat, sendcan) - return self.update_vl() + self.update_strings([]) def update_strings(self, strings, sendcan=False): for v in self.vl_all.values(): - v.clear() + for l in v.values(): + l.clear() + + cdef vector[SignalValue] new_vals + cdef unordered_set[uint32_t] updated_addrs + + self.can.update_strings(strings, new_vals, sendcan) + cdef vector[SignalValue].iterator it = new_vals.begin() + cdef SignalValue* cv + while it != new_vals.end(): + cv = &deref(it) + # Cast char * directly to unicode + cv_name = cv.name + self.vl[cv.address][cv_name] = cv.value + self.vl_all[cv.address][cv_name] = cv.all_values + self.ts_nanos[cv.address][cv_name] = cv.ts_nanos + updated_addrs.insert(cv.address) + preinc(it) - updated_addrs = set() - for s in strings: - self.can.update_string(s, sendcan) - updated_addrs.update(self.update_vl()) return updated_addrs @property diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 916a0a254..d83f035dc 100644 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc/gm_global_a_powertrain_generated.dbc @@ -263,6 +263,11 @@ BO_ 880 ASCMActiveCruiseControlStatus: 6 K124_ASCM SG_ ACCCmdActive : 23|1@0+ (1,0) [0|0] "" NEO SG_ FCWAlert : 41|2@0+ (1,0) [0|3] "" XXX +BO_ 967 EVDriveMode: 4 XXX + SG_ SinglePedalModeActive : 7|1@0+ (1,0) [0|1] "" XXX + SG_ SinglePedalModeRisingEdge : 21|1@0+ (1,0) [0|1] "" XXX + SG_ SinglePedalModeFallingEdge : 22|1@0+ (1,0) [0|1] "" XXX + BO_ 977 ECMCruiseControl: 8 K20_ECM SG_ CruiseActive : 39|1@0+ (1,0) [0|3] "" NEO SG_ CruiseSetSpeed : 19|12@0+ (0.0625,0) [0|0] "km/h" NEO diff --git a/opendbc/honda_accord_2018_can_generated.dbc b/opendbc/honda_accord_2018_can_generated.dbc index d1a84ac80..85947333d 100644 --- a/opendbc/honda_accord_2018_can_generated.dbc +++ b/opendbc/honda_accord_2018_can_generated.dbc @@ -357,7 +357,7 @@ BO_ 13274 LKAS_HUD_A: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -378,7 +378,7 @@ BO_ 13275 LKAS_HUD_B: 8 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -436,7 +436,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/honda_civic_ex_2022_can_generated.dbc b/opendbc/honda_civic_ex_2022_can_generated.dbc index b72f9aa6b..f696d13b4 100644 --- a/opendbc/honda_civic_ex_2022_can_generated.dbc +++ b/opendbc/honda_civic_ex_2022_can_generated.dbc @@ -375,7 +375,7 @@ BO_ 13274 LKAS_HUD_A: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -396,7 +396,7 @@ BO_ 13275 LKAS_HUD_B: 8 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -445,6 +445,12 @@ BO_ 432 STANDSTILL: 7 VSA SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON +BO_ 446 BRAKE_MODULE: 3 VSA + SG_ BRAKE_PRESSED : 4|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_FAULT : 22|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 21|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 19|4@0+ (1,0) [0|15] "" XXX + BO_ 456 ACC_CONTROL: 8 XXX SG_ ACCEL_COMMAND : 7|12@0- (0.01,0) [0|0] "m/s^2" XXX SG_ IDLESTOP_ALLOW : 8|1@0+ (1,0) [0|1] "" XXX @@ -459,6 +465,11 @@ BO_ 456 ACC_CONTROL: 8 XXX SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX +BO_ 467 CRUISE_FAULT_STATUS: 8 XXX + SG_ CRUISE_FAULT : 3|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + BO_ 829 LKAS_HUD: 8 ADAS SG_ CAM_TEMP_HIGH : 7|1@0+ (1,0) [0|255] "" BDY SG_ SET_ME_X41 : 6|7@0+ (1,0) [0|127] "" BDY @@ -470,7 +481,7 @@ BO_ 829 LKAS_HUD: 8 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY @@ -496,6 +507,7 @@ BO_ 254913108 LKAS_HUD_2: 8 ADAS SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX +CM_ 446 "If exists, describes cruise faults and what the PCM uses for brake press detection."; CM_ SG_ 456 IDLESTOP_ALLOW "allows car to turn off engine at a standstill"; CM_ SG_ 456 STANDSTILL "set to 1 when camera requests -4.0 m/s^2"; diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc index 349fd2f9e..75ba176a8 100644 --- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc +++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc @@ -357,7 +357,7 @@ BO_ 13274 LKAS_HUD_A: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -378,7 +378,7 @@ BO_ 13275 LKAS_HUD_B: 8 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -436,7 +436,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc index 9be63fe36..47205cb24 100644 --- a/opendbc/honda_civic_touring_2016_can_generated.dbc +++ b/opendbc/honda_civic_touring_2016_can_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc index 345d94819..9f3dc0fba 100644 --- a/opendbc/honda_crv_ex_2017_can_generated.dbc +++ b/opendbc/honda_crv_ex_2017_can_generated.dbc @@ -357,7 +357,7 @@ BO_ 13274 LKAS_HUD_A: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -378,7 +378,7 @@ BO_ 13275 LKAS_HUD_B: 8 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -436,7 +436,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/honda_crv_executive_2016_can_generated.dbc b/opendbc/honda_crv_executive_2016_can_generated.dbc index a00563215..272cf5dd3 100644 --- a/opendbc/honda_crv_executive_2016_can_generated.dbc +++ b/opendbc/honda_crv_executive_2016_can_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY @@ -306,7 +306,7 @@ BO_ 404 STEERING_CONTROL: 4 EON SG_ STEER_TORQUE : 7|12@0- (1,0) [-768|768] "" EPS SG_ SET_ME_X00 : 11|4@0+ (1,0) [0|15] "" EPS SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS - SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS + SG_ SET_ME_X00_2 : 22|7@0+ (1,0) [0|127] "" EPS SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS diff --git a/opendbc/honda_crv_touring_2016_can_generated.dbc b/opendbc/honda_crv_touring_2016_can_generated.dbc index d56746bc2..164844215 100644 --- a/opendbc/honda_crv_touring_2016_can_generated.dbc +++ b/opendbc/honda_crv_touring_2016_can_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY @@ -308,7 +308,7 @@ BO_ 404 STEERING_CONTROL: 4 EON SG_ STEER_TORQUE : 7|12@0- (1,0) [-768|768] "" EPS SG_ SET_ME_X00 : 11|4@0+ (1,0) [0|15] "" EPS SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS - SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS + SG_ SET_ME_X00_2 : 22|7@0+ (1,0) [0|127] "" EPS SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS diff --git a/opendbc/honda_fit_ex_2018_can_generated.dbc b/opendbc/honda_fit_ex_2018_can_generated.dbc index b1900455c..1a80a9d4e 100644 --- a/opendbc/honda_fit_ex_2018_can_generated.dbc +++ b/opendbc/honda_fit_ex_2018_can_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/honda_insight_ex_2019_can_generated.dbc b/opendbc/honda_insight_ex_2019_can_generated.dbc index 095774203..ccbf25bfc 100644 --- a/opendbc/honda_insight_ex_2019_can_generated.dbc +++ b/opendbc/honda_insight_ex_2019_can_generated.dbc @@ -357,7 +357,7 @@ BO_ 13274 LKAS_HUD_A: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -378,7 +378,7 @@ BO_ 13275 LKAS_HUD_B: 8 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ SET_ME_X01 : 20|1@0+ (1,0) [0|1] "" BDY @@ -436,7 +436,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/honda_odyssey_exl_2018_generated.dbc b/opendbc/honda_odyssey_exl_2018_generated.dbc index 1afefeee1..bb33513b2 100644 --- a/opendbc/honda_odyssey_exl_2018_generated.dbc +++ b/opendbc/honda_odyssey_exl_2018_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc b/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc index 5ee66d2ca..b5664d9e7 100644 --- a/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc +++ b/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc @@ -255,7 +255,7 @@ BO_ 829 LKAS_HUD: 5 ADAS SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY - SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ BOH_2 : 23|2@0+ (1,0) [0|4] "" BDY SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY diff --git a/opendbc/subaru_global_2017_generated.dbc b/opendbc/subaru_global_2017_generated.dbc index d0b47a2c6..b572d1b64 100644 --- a/opendbc/subaru_global_2017_generated.dbc +++ b/opendbc/subaru_global_2017_generated.dbc @@ -316,10 +316,22 @@ BO_ 576 CruiseControl: 8 XXX SG_ Cruise_Activated : 41|1@1+ (1,0) [0|1] "" XXX SG_ Signal2 : 42|22@1+ (1,0) [0|4194303] "" XXX +BO_ 803 INFOTAINMENT_STATUS: 8 XXX + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX + SG_ LKAS_Blue_Lines : 15|4@0+ (1,0) [0|15] "" XXX + SG_ Signal1 : 19|4@0+ (1,0) [0|15] "" XXX + SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX + SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX + CM_ SG_ 545 Cruise_Throttle "RPM-like output signal"; CM_ SG_ 545 Cruise_EPB "1 = Electric Parking Brake set"; CM_ SG_ 545 Distance_Swap "Switch from Close to Far distance"; CM_ SG_ 545 Cruise_Soft_Disable "Eyesight Temporary disable, sets CruiseControl Activated = 0"; CM_ SG_ 546 Cruise_RPM "ES RPM output for ECM and TCM"; CM_ SG_ 546 Signal3 "0 when cruise_activated = 1"; +CM_ SG_ 803 Signal1 "Seems to be static, set only on some cars"; +CM_ SG_ 803 Signal2 "Seems to be static, set only on some cars"; +VAL_ 803 LKAS_Blue_Lines 0 "no modifier" 2 "changes lines to blue"; +VAL_ 803 LKAS_State_Infotainment 0 "none" 2 "Obstacle Detected" 3 "Keep Hands On Wheel" 4 "Keep Hands On Wheel Off"; VAL_ 72 Gear 2 "N" 3 "R" 4 "P" 121 "D" 137 "1" 145 "2" 153 "3" 161 "4" 169 "5" 177 "6"; diff --git a/opendbc/toyota_new_mc_pt_generated.dbc b/opendbc/toyota_new_mc_pt_generated.dbc index d611c1d84..e7702572a 100644 --- a/opendbc/toyota_new_mc_pt_generated.dbc +++ b/opendbc/toyota_new_mc_pt_generated.dbc @@ -127,6 +127,7 @@ BO_ 466 PCM_CRUISE: 8 XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 467 PCM_CRUISE_2: 8 XXX + SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX @@ -547,6 +548,9 @@ BO_ 610 EPS_STATUS: 5 EPS SG_ TYPE : 24|1@0+ (1,0) [0|1] "" XXX SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX +BO_ 1178 BRAKE_RELATED: 8 XXX + SG_ BRAKE_PRESSED : 48|1@0+ (1,0) [0|1] "" XXX + CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index c51b56c05..3c88adc4d 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -127,6 +127,7 @@ BO_ 466 PCM_CRUISE: 8 XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 467 PCM_CRUISE_2: 8 XXX + SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX diff --git a/opendbc/toyota_tnga_k_pt_generated.dbc b/opendbc/toyota_tnga_k_pt_generated.dbc index cf20ec697..ba432a818 100644 --- a/opendbc/toyota_tnga_k_pt_generated.dbc +++ b/opendbc/toyota_tnga_k_pt_generated.dbc @@ -127,6 +127,7 @@ BO_ 466 PCM_CRUISE: 8 XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 467 PCM_CRUISE_2: 8 XXX + SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX diff --git a/opendbc/vw_golf_mk4.dbc b/opendbc/vw_golf_mk4.dbc index b266a7f4c..eb8f6c959 100644 --- a/opendbc/vw_golf_mk4.dbc +++ b/opendbc/vw_golf_mk4.dbc @@ -770,23 +770,36 @@ BO_ 1340 Fahrwerk_1: 1 XXX SG_ Ansteuererung_Fahrzeugniveau : 0|4@1+ (1,0) [0|15] "" XXX BO_ 1472 EPB_1: 8 XXX - SG_ Textmeldungen_EPB : 52|4@1+ (1,0) [0|0] "" XXX - SG_ Aufmerksamkeits_Gtong_EPB : 51|1@1+ (1,0) [0|0] "" XXX - SG_ Fehler_Gong_EBP : 50|1@1+ (1,0) [0|0] "" XXX - SG_ Warnton_EPB : 49|1@1+ (1,0) [0|0] "" XXX - SG_ Funktionslampe_EPB : 48|1@1+ (1,0) [0|0] "" XXX - SG_ Frei_EPB_1_1 : 33|15@1+ (1,0) [0|0] "" XXX - SG_ Fehlerspeichereintrag_EPB : 32|1@1+ (1,0) [0|0] "" XXX - SG_ Neigungswinkel : 24|8@1+ (1,-128) [-128|127] "g" XXX - SG_ Verzoegerungsanforderung__EPB_ : 24|8@1+ (0.5,0) [0|255] "g" XXX - SG_ Status_Neigungswinkelgeber : 15|1@1+ (1,0) [0|0] "" XXX - SG_ Schalterinfo_Parkbremse : 13|2@1+ (1,0) [0|0] "" XXX - SG_ Erreichte_Spannkraft : 8|5@1+ (1,0) [0|30] "kN" XXX - SG_ Checksumme_Parkbremse : 8|8@1+ (1,0) [0|255] "" XXX - SG_ Schalterstatus_Parkbremse : 7|1@1+ (1,0) [0|0] "" XXX - SG_ Status_Parkbremse : 6|1@1+ (1,0) [0|0] "" XXX - SG_ Fehlerstatus_Parkbremse : 4|2@1+ (1,0) [0|0] "" XXX - SG_ Zaehler_Parkbremse : 0|4@1+ (1,0) [0|0] "" XXX + SG_ CHECKSUM : 56|8@1+ (1,0) [0|255] "" XXX + SG_ COUNTER : 0|4@1+ (1,0) [0|15] "" XXX + SG_ EP1__Text : 52|4@1+ (1,0) [0|8] "" Vector__XXX + SG_ EP1_Failure_gelb : 51|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Failure_BKL : 50|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Warnton : 49|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Fkt_Lampe : 48|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ EP1_Freigabe_Ver : 33|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Failureeintr : 32|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Neig_winkel : 16|8@1+ (1,-128) [-128|127] "g" XXX + SG_ EP1_Verzoegerung : 24|8@1+ (0.048,-7.968) [-7.968|4.224] "g" XXX + SG_ EP1_Sta_NWS : 15|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Schalterinfo : 13|2@1+ (1,0) [0|3] "" XXX + SG_ EP1_Spannkraft : 8|5@1+ (1,0) [0|30] "kN" XXX + SG_ EP1_Sta_Schalter : 7|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Sta_EPB : 6|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Failure_Sta : 4|2@1+ (1,0) [0|3] "" XXX + SG_ EP1_AutoHold_zul : 34|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_AutoHold_active : 35|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_SleepInd : 36|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Status_Kl_15 : 37|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Lampe_AutoP : 38|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Bremslicht : 39|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Warnton1 : 40|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_Warnton2 : 41|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_AnfShLock : 42|1@1+ (1,0) [0|1] "" XXX + SG_ EPB_Autoholdlampe : 43|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_KuppModBer : 45|2@1+ (1,0) [0|3] "" XXX + SG_ EP1_HydrHalten : 47|1@1+ (1,0) [0|1] "" XXX + SG_ EP1_QualNeigWi : 44|1@1+ (1,0) [0|1] "" XXX BO_ 1326 Diag_Lenkhilfe: 3 XXX SG_ Werkstattcode__Diag_ : 16|8@1+ (1,0) [0|0] "" XXX @@ -1086,10 +1099,10 @@ BO_ 872 ACC_System: 8 XXX SG_ ACS_Sollbeschl : 24|11@1+ (0.005,-7.22) [-7.22|3.005] "Unit_MeterPerSeconSquar" XXX SG_ ACS_Anhaltewunsch : 38|1@1+ (1,0) [0|1] "" XXX SG_ ACS_Fehler : 39|1@1+ (1,0) [0|1] "" XXX - SG_ ACS_zul_Regelabw : 40|8@1+ (1,0.005) [0|1.265] "Unit_MeterPerSeconSquar" XXX - SG_ ACS_max_AendGrad : 48|8@1+ (1,0.02) [0.02|5.06] "Unit_MeterPerSeconSquar" XXX + SG_ ACS_zul_Regelabw : 40|8@1+ (0.005,0) [0|1.265] "Unit_MeterPerSeconSquar" XXX + SG_ ACS_max_AendGrad : 48|8@1+ (0.02,0) [0.02|5.06] "Unit_MeterPerSeconSquar" XXX -BO_ 1386 ACC_GRA_Anziege: 8 XXX +BO_ 1386 ACC_GRA_Anzeige: 8 XXX SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ ACA_StaACC : 8|3@1+ (1,0) [0|7] "" XXX SG_ ACA_ID_StaACC : 11|5@1+ (1,0) [0|31] "" XXX @@ -1100,7 +1113,7 @@ BO_ 1386 ACC_GRA_Anziege: 8 XXX SG_ ACA_kmh_mph : 32|1@1+ (1,0) [0|1] "" XXX SG_ ACA_Akustik1 : 33|1@1+ (1,0) [0|1] "" XXX SG_ ACA_Akustik2 : 34|1@1+ (1,0) [0|1] "" XXX - SG_ ACA_PrioDisp : 35|1@1+ (1,0) [0|1] "" XXX + SG_ ACA_PrioDisp : 35|2@1+ (1,0) [0|3] "" XXX SG_ ACA_gemZeitl : 40|4@1+ (1,0) [0|15] "" XXX SG_ ACA_ACC_Verz : 44|1@1+ (1,0) [0|1] "" XXX SG_ ACA_StaGRA : 48|3@1+ (1,0) [0|7] "" XXX @@ -1153,8 +1166,8 @@ BO_ 644 Motor_Bremse: 6 XXX SG_ MOB_COUNTER : 8|4@1+ (1,0) [0|15] "" Bremse_MK25AESP SG_ TSK_v_Begrenzung_aktiv : 15|1@0+ (1,0) [0|1] "" XXX SG_ TSK_ax_Getriebe_01 : 40|8@1+ (0.048,0) [0|255] "m/s2" XXX - SG_ MOB_Bremsstgr : 16|11@1+ (0.76,0) [0|100] "Unit_PerCent" Vector__XXX - SG_ MOB_Bremsmom : 27|13@1+ (1,0) [0|8190] "Unit_NewtoMeter" Bremse_MK25AESP + SG_ MOB_Bremsstgr : 16|11@1+ (0.048852,0) [0|100] "Unit_PerCent" Vector__XXX + SG_ MOB_Bremsmom : 27|13@1+ (4,0) [0|32760] "Unit_NewtoMeter" Bremse_MK25AESP BO_ 870 AWV: 5 XXX SG_ AWV_2_Gurtstraffer : 39|1@1+ (1,0) [0|1] "" Bremsbooster @@ -1531,5 +1544,36 @@ CM_ SG_ 1470 XX_DLCORTLC2 "Might be DLC or TLC, might have wrong size"; CM_ SG_ 1550 MFA_v_Signal_02 "0=km/h, 1=mph"; +VAL_ 872 ACS_Sta_ADR 2 "ADR_passiv" 0 "ADR_nicht_aktiv" 1 "ADR_aktiv" 3 "irrev_Fehler" ; +VAL_ 872 ACS_ADR_Schub 1 "Verz_begr_auf_Schub" 0 "Verz_nicht_begr_auf_Schub" ; +VAL_ 872 ACS_Schubabsch 1 "SA_nicht_zulaessig" 0 "SA_zulaessig" ; +VAL_ 872 ACS_StSt_Info 3 "Systemfehler" 0 "Motorlauf_nn" 1 "Stoppverbot_Motoranlauf_nn" 2 "Motoranlauf_notwendig" ; +VAL_ 872 ACS_MomEingriff 1 "MomEingr_verhindern" 0 "keine_Beeinfl_MomEingr_Mot" ; +VAL_ 872 ACS_Typ_ACC 0 "Basis_ACC" 1 "ACC_mit_FollowToStop" 3 "frei" 2 "frei" ; +VAL_ 872 ACS_FreigSollB 0 "Sollbeschl_nicht_freigeg" 1 "Sollbeschl_freigeg" ; +VAL_ 872 ACS_Sollbeschl 2046 "ADR_nicht_aktiv" 2047 "Fehler" ; +VAL_ 872 ACS_Anhaltewunsch 0 "kein_Haltewunsch" 1 "Fzg_haelt_an" ; +VAL_ 872 ACS_Fehler 1 "Fehlerspeichereintrag" 0 "kein_Fehlerspeichereintrag" ; +VAL_ 872 ACS_zul_Regelabw 254 "ADR_nicht_aktiv" 255 "Fehler" ; +VAL_ 872 ACS_max_AendGrad 254 "Neutralwert" 0 "Neutralwert" 255 "Fehler" ; + VAL_ 978 LH2_Sta_HCA 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active"; VAL_ 1088 Waehlhebelposition__Getriebe_1_ 8 "P" 7 "R" 6 "N" 5 "D" 9 "U" 12 "S" 14 "T" 10 "T" 11 "T"; + +VAL_ 1386 ACA_StaACC 6 "ACC_rev_aus" 0 "Hauptschalter_aus" 4 "ACC_im_Hintergrund" 3 "ACC_aktiv" 1 "Reserve" 2 "ACC_passiv" 7 "ACC_irrev_aus" 5 "frei" ; +VAL_ 1386 ACA_ID_StaACC 0 "keine_Anzeige" ; +VAL_ 1386 ACA_Fahrerhinw 1 "Ein" 0 "Aus" ; +VAL_ 1386 ACA_AnzDisplay 1 "Anzeige_erw" 0 "Anzeige_nicht_erw" ; +VAL_ 1386 ACA_Zeitluecke 3 "Zeitluecke3" 10 "Zeitluecke10" 4 "Zeitluecke4" 14 "Zeitluecke14" 11 "Zeitluecke11" 2 "Zeitluecke2" 13 "Zeitluecke13" 9 "Zeitluecke9" 1 "Zeitluecke1" 8 "Zeitluecke8" 5 "Zeitluecke5" 15 "Zeitluecke15" 0 "nicht_definiert" 12 "Zeitluecke12" 6 "Zeitluecke6" 7 "Zeitluecke7" ; +VAL_ 1386 ACA_V_Wunsch 255 "kein_Wert_im_Speicher" ; +VAL_ 1386 ACA_kmh_mph 0 "km_h" 1 "mph" ; +VAL_ 1386 ACA_Akustik1 0 "kein_Gong" 1 "Gong" ; +VAL_ 1386 ACA_Akustik2 0 "kein_Summer" 1 "Summer" ; +VAL_ 1386 ACA_PrioDisp 1 "mittlere_Prio" 3 "keine_Anzeige_Anf" 0 "hohe_Prio" 2 "niedrige_Prio" ; +VAL_ 1386 ACA_gemZeitl 6 "Zeitluecke6" 2 "Zeitluecke2" 7 "Zeitluecke7" 13 "Zeitluecke13" 11 "Zeitluecke11" 4 "Zeitluecke4" 8 "Zeitluecke8" 12 "Zeitluecke12" 10 "Zeitluecke10" 0 "Kein_Objekt_erfasst" 1 "Zeitluecke1" 3 "Zeitluecke3" 9 "Zeitluecke9" 15 "Zeitluecke15" 14 "Zeitluecke14" 5 "Zeitluecke5" ; +VAL_ 1386 ACA_ACC_Verz 0 "ACC_verzoegert_nicht" 1 "ACC_verzoegert" ; +VAL_ 1386 ACA_StaGRA 3 "GRA_aktiv" 4 "GRA_uebertreten" 2 "GRA_passiv" 0 "Hauptschalter_aus" 6 "frei" 7 "GRA_Fehler" 1 "Reserve" 5 "frei" ; +VAL_ 1386 ACA_ID_StaGRA 0 "keine_Anzeige" ; +VAL_ 1386 ACA_Codierung 0 "ACC" 1 "GRA" ; +VAL_ 1386 ACA_Tachokranz 0 "nicht_beleuchtet" 1 "beleuchtet" ; +VAL_ 1386 ACA_Aend_Zeitluecke 1 "Anzeige_angef" 0 "keine_Anzeige" ; diff --git a/opendbc/vw_mqb_2010.dbc b/opendbc/vw_mqb_2010.dbc index f1adeab84..8806e0011 100644 --- a/opendbc/vw_mqb_2010.dbc +++ b/opendbc/vw_mqb_2010.dbc @@ -1211,18 +1211,19 @@ BO_ 1602 WIV_01: 8 Motor_Diesel_MQB SG_ WIV_W_min : 48|7@1+ (1000,0) [0|127000] "Unit_KiloMeter" Gateway_MQB SG_ WIV_W_max : 56|7@1+ (1000,0) [0|127000] "Unit_KiloMeter" Gateway_MQB -BO_ 294 HCA_01: 8 XXX - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX - SG_ COUNTER : 8|4@1+ (1,0) [0|255] "" XXX - SG_ SET_ME_0X3 : 12|4@1+ (1,0) [0|15] "" XXX - SG_ Assist_Torque : 16|14@1+ (1,0) [0|300] "Nm" XXX - SG_ Assist_Requested : 30|1@1+ (1,0) [0|1] "" XXX - SG_ Assist_VZ : 31|1@1+ (1,0) [0|1] "" XXX - SG_ HCA_Available : 32|1@1+ (1,0) [0|1] "" XXX - SG_ HCA_Standby : 33|1@1+ (1,0) [0|1] "" XXX - SG_ HCA_Active : 34|1@1+ (1,0) [0|1] "" XXX - SG_ SET_ME_0XFE : 40|8@1+ (1,0) [0|255] "" XXX - SG_ SET_ME_0X07 : 48|8@1+ (1,0) [0|255] "" XXX +BO_ 294 HCA_01: 8 Frontsensorik + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" Vector__XXX + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" Vector__XXX + SG_ HCA_01_Vib_Freq : 12|4@1+ (1,15) [15|30] "Unit_Hertz" Vector__XXX + SG_ HCA_01_LM_Offset : 16|9@1+ (1,0) [0|511] "Unit_centiNewtoMeter" Vector__XXX + SG_ EA_ACC_Sollstatus : 25|2@1+ (1,0) [0|3] "" Frontradar + SG_ EA_Ruckprofil : 27|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ HCA_01_Sendestatus : 30|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ HCA_01_LM_OffSign : 31|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ HCA_01_Status_HCA : 32|4@1+ (1,0) [0|15] "" Vector__XXX + SG_ HCA_01_Vib_Amp : 36|4@1+ (0.2,0) [0|3] "Unit_NewtoMeter" Vector__XXX + SG_ EA_Ruckfreigabe : 40|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ EA_ACC_Wunschgeschwindigkeit : 41|10@1+ (0.32,0) [0|327.04] "Unit_KiloMeterPerHour" Frontradar BO_ 159 LH_EPS_03: 8 XXX SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX @@ -1420,12 +1421,16 @@ CM_ SG_ 159 EPS_VZ_Lenkmoment "Steering input by driver, direction"; CM_ SG_ 159 EPS_Berechneter_LW "Raw steering angle, degrees"; CM_ SG_ 159 EPS_VZ_BLW "Raw steering angle, direction"; CM_ SG_ 173 COUNTERXX "Message not renamed to COUNTER because J533 rate-limiting makes it look like messages are being lost"; -CM_ SG_ 294 SET_ME_0X3 "May be zero when sent by older cameras"; -CM_ SG_ 294 SET_ME_0X07 "May be zero when sent by older cameras"; -CM_ SG_ 294 SET_ME_0XFE "May be zero when sent by older cameras"; -CM_ SG_ 294 Assist_Torque "Heading control input, torque"; -CM_ SG_ 294 Assist_VZ "Heading control input, direction (sign)"; -CM_ SG_ 294 HCA_Available "Must be 1 for steering rack to accept HCA commands"; +CM_ SG_ 294 HCA_01_Vib_Freq "Frequenz der Lenkradvibration"; +CM_ SG_ 294 HCA_01_LM_Offset "Von HCA angefordertes Lenkmoment (Betrag)"; +CM_ SG_ 294 EA_ACC_Sollstatus "Status-Anforderung ACC von Emergency Alert. Statuswechsel bei Flanke. Solange Wert=1, wird EA_ACC_Wunschgeschwindigkeit übernommen. Wert=2 führt zu Zustand ¿ACC_GRA_passiv¿"; +CM_ SG_ 294 EA_Ruckprofil "Emergency Alert Anforderung an ESP, welcher Ruck verwendet werden soll. Eine Umsetzung der Ruckanforderung im ESP erfolgt nur mit gesetztem Bit 'EA_Ruckfreigabe'."; +CM_ SG_ 294 HCA_01_Sendestatus "Gibt den Sendestatus der HCA_01 an (notwendig für IL-Unterstützung)"; +CM_ SG_ 294 HCA_01_LM_OffSign "Vorzeichen des HCA-Lenkmoments"; +CM_ SG_ 294 HCA_01_Status_HCA "Statusinformation vom HCA und Manoevrierassistent für Handshakemechanismus mit der Lenkung"; +CM_ SG_ 294 HCA_01_Vib_Amp "Momentenamplitude der Lenkradvibration"; +CM_ SG_ 294 EA_Ruckfreigabe "Emergency Alert Freigabit für die Warnruck-Anforderung an das ESP"; +CM_ SG_ 294 EA_ACC_Wunschgeschwindigkeit "Emergency Alert Anforderung neue Wunschgeschwindigkeit"; CM_ SG_ 302 ACC_Hold_Request "Active request for ABS brake hold in ACC_Hold_Type"; CM_ SG_ 302 ACC_Boost_Request "Hybrid engine start related"; CM_ SG_ 302 ACC_Freewheel_Request "Active request for DSG sailing/coasting in ACC_Freewheel_Type"; @@ -1475,6 +1480,13 @@ CM_ SG_ 1720 KBI_Variante_USA "In diesem Signal wird die HW-Variante des Kombis VAL_ 159 EPS_HCA_Status 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active" ; VAL_ 173 GE_Fahrstufe 5 "P" 6 "R" 7 "N" 8 "D" 9 "S" 10 "E" 13 "T" 14 "T" ; VAL_ 288 TSK_Status 0 "init" 1 "disabled" 2 "enabled" 3 "regulating" 4 "accel_pedal_override" 5 "brake_only" 6 "temp_fault" 7 "perm_fault" ; +VAL_ 294 EA_ACC_Sollstatus 0 "Init" 1 "ACC_aktivieren" 2 "ACC_deaktivieren" ; +VAL_ 294 EA_Ruckprofil 0 "Init" 1 "Profil_1" 2 "Profil_2" 3 "Profil_3" 4 "Profil_4" 5 "Profil_5" 6 "Profil_6" 7 "Profil_7" ; +VAL_ 294 HCA_01_Sendestatus 0 "HCA_sendet_mit_1000ms" 1 "HCA_sendet_mit_20ms" ; +VAL_ 294 HCA_01_LM_OffSign 0 "positives_Vorzeichen" 1 "negatives_Vorzeichen" ; +VAL_ 294 HCA_01_Status_HCA 0 "deaktiviert" 1 "reserviert" 2 "reserviert" 3 "funktionsbereit" 4 "reserviert" 5 "HCA_Momenteneingriff_1" 6 "MA_Aktiv" 7 "HCA_Momenteneingriff_2" 8 "reserviert" 9 "reserviert" 10 "reserviert" 11 "reserviert" 12 "reserviert" 13 "reserviert" 14 "reserviert" 15 "reserviert" ; +VAL_ 294 EA_Ruckfreigabe 0 "keine_Freigabe" 1 "Freigabe" ; +VAL_ 294 EA_ACC_Wunschgeschwindigkeit 1023 "Init" ; VAL_ 780 ACC_Wunschgeschw_02 1023 "keine_Anzeige" ; VAL_ 780 ACC_Status_Prim_Anz 0 "Symbol nicht beleuchtet" 1 "Farbe 1 (typisch 'gruen')" 2 "Farbe 2 (typisch 'rot')" 3 "Farbe 3 (typisch 'gelb')" ; VAL_ 780 ACC_Abstandsindex 0 "Sonderanzeige_graue_Fahrbahn" 1022 "Sonderanzeige_graue_Fahrbahn" 1023 "Sonderanzeige_Fahrbahn_mit_gruenem_roten_Bereich" ; diff --git a/panda/board/README.md b/panda/board/README.md index 4434d28af..ad9091411 100644 --- a/panda/board/README.md +++ b/panda/board/README.md @@ -4,18 +4,15 @@ Programming **Panda** ``` -./recover.sh # flash bootstub -``` - -``` -./flash.sh # flash application +./recover.py # flash bootstub +./flash.py # flash application ``` Troubleshooting ---- -If your panda will not flash and green LED is on, use `recover.sh`. -If panda is blinking fast with green LED, use `flash.sh`. +If your panda will not flash and green LED is on, use `recover.py`. +If panda is blinking fast with green LED, use `flash.py`. Otherwise if LED is off and panda can't be seen with `lsusb` command, use [panda paw](https://comma.ai/shop/products/panda-paw) to go into DFU mode. diff --git a/panda/board/SConscript b/panda/board/SConscript index 7c9127904..dc095e712 100644 --- a/panda/board/SConscript +++ b/panda/board/SConscript @@ -102,7 +102,8 @@ def get_key_header(name): from Crypto.PublicKey import RSA public_fn = File(f'../certs/{name}.pub').srcnode().abspath - rsa = RSA.importKey(open(public_fn).read()) + with open(public_fn) as f: + rsa = RSA.importKey(f.read()) assert(rsa.size_in_bits() == 1024) rr = pow(2**1024, 2, rsa.n) diff --git a/panda/board/boards/black.h b/panda/board/boards/black.h index 7c7894874..216def774 100644 --- a/panda/board/boards/black.h +++ b/panda/board/boards/black.h @@ -25,7 +25,7 @@ void black_enable_can_transceiver(uint8_t transceiver, bool enabled) { void black_enable_can_transceivers(bool enabled) { for(uint8_t i=1U; i<=4U; i++){ // Leave main CAN always on for CAN-based ignition detection - if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ + if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ black_enable_can_transceiver(i, true); } else { black_enable_can_transceiver(i, enabled); @@ -83,7 +83,7 @@ void black_set_can_mode(uint8_t mode){ switch (mode) { case CAN_MODE_NORMAL: case CAN_MODE_OBD_CAN2: - if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(car_harness_status == HARNESS_STATUS_FLIPPED)) { + if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { // B12,B13: disable OBD mode set_gpio_mode(GPIOB, 12, MODE_INPUT); set_gpio_mode(GPIOB, 13, MODE_INPUT); @@ -160,7 +160,7 @@ void black_init(void) { black_set_can_mode(CAN_MODE_NORMAL); // flip CAN0 and CAN2 if we are flipped - if (car_harness_status == HARNESS_STATUS_FLIPPED) { + if (harness.status == HARNESS_STATUS_FLIPPED) { can_flip_buses(0, 2); } } @@ -191,6 +191,9 @@ const board board_black = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .avdd_mV = 3300U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = black_init, .enable_can_transceiver = black_enable_can_transceiver, .enable_can_transceivers = black_enable_can_transceivers, @@ -204,4 +207,4 @@ const board board_black = { .set_phone_power = unused_set_phone_power, .set_siren = unused_set_siren, .read_som_gpio = unused_read_som_gpio -}; \ No newline at end of file +}; diff --git a/panda/board/boards/board_declarations.h b/panda/board/boards/board_declarations.h index 4ca1fb4fe..db9bbd402 100644 --- a/panda/board/boards/board_declarations.h +++ b/panda/board/boards/board_declarations.h @@ -11,7 +11,7 @@ typedef void (*board_set_ir_power)(uint8_t percentage); typedef void (*board_set_fan_enabled)(bool enabled); typedef void (*board_set_phone_power)(bool enabled); typedef void (*board_set_siren)(bool enabled); -typedef void (*board_board_tick)(bool ignition, bool usb_enum, bool heartbeat_seen); +typedef void (*board_board_tick)(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted); typedef bool (*board_read_som_gpio)(void); struct board { @@ -25,6 +25,9 @@ struct board { const bool has_canfd; const bool has_rtc_battery; const uint16_t fan_max_rpm; + const uint16_t avdd_mV; + const bool fan_stall_recovery; + const uint8_t fan_enable_cooldown_time; board_init init; board_enable_can_transceiver enable_can_transceiver; board_enable_can_transceivers enable_can_transceivers; diff --git a/panda/board/boards/dos.h b/panda/board/boards/dos.h index d2e8f3df2..d2bf7d3bf 100644 --- a/panda/board/boards/dos.h +++ b/panda/board/boards/dos.h @@ -25,7 +25,7 @@ void dos_enable_can_transceiver(uint8_t transceiver, bool enabled) { void dos_enable_can_transceivers(bool enabled) { for(uint8_t i=1U; i<=4U; i++){ // Leave main CAN always on for CAN-based ignition detection - if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ + if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ dos_enable_can_transceiver(i, true); } else { dos_enable_can_transceiver(i, enabled); @@ -53,9 +53,9 @@ void dos_set_bootkick(bool enabled){ set_gpio_output(GPIOC, 4, !enabled); } -void dos_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen) { - if (ignition && !usb_enum) { - // enable bootkick if ignition seen +void dos_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) { + if ((ignition && !usb_enum) || harness_inserted) { + // enable bootkick if ignition seen or if plugged into a harness dos_set_bootkick(true); } else if (heartbeat_seen) { // disable once openpilot is up @@ -69,7 +69,7 @@ void dos_set_can_mode(uint8_t mode){ switch (mode) { case CAN_MODE_NORMAL: case CAN_MODE_OBD_CAN2: - if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(car_harness_status == HARNESS_STATUS_FLIPPED)) { + if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { // B12,B13: disable OBD mode set_gpio_mode(GPIOB, 12, MODE_INPUT); set_gpio_mode(GPIOB, 13, MODE_INPUT); @@ -140,12 +140,8 @@ void dos_init(void) { set_gpio_output(GPIOC, 11, 1); #ifdef ENABLE_SPI - // A4-A7: SPI - set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1); - set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1); - set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1); - set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1); - register_set_bits(&(GPIOA->OSPEEDR), GPIO_OSPEEDER_OSPEEDR4 | GPIO_OSPEEDER_OSPEEDR5 | GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7); + // SPI init + gpio_spi_init(); #endif // C8: FAN PWM aka TIM3_CH3 @@ -181,7 +177,7 @@ void dos_init(void) { dos_set_can_mode(CAN_MODE_NORMAL); // flip CAN0 and CAN2 if we are flipped - if (car_harness_status == HARNESS_STATUS_FLIPPED) { + if (harness.status == HARNESS_STATUS_FLIPPED) { can_flip_buses(0, 2); } @@ -219,6 +215,9 @@ const board board_dos = { .has_canfd = false, .has_rtc_battery = true, .fan_max_rpm = 6500U, + .avdd_mV = 3300U, + .fan_stall_recovery = true, + .fan_enable_cooldown_time = 3U, .init = dos_init, .enable_can_transceiver = dos_enable_can_transceiver, .enable_can_transceivers = dos_enable_can_transceivers, diff --git a/panda/board/boards/grey.h b/panda/board/boards/grey.h index 1ee5efb0a..0adb5e2a1 100644 --- a/panda/board/boards/grey.h +++ b/panda/board/boards/grey.h @@ -45,6 +45,9 @@ const board board_grey = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .avdd_mV = 3300U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = grey_init, .enable_can_transceiver = white_enable_can_transceiver, .enable_can_transceivers = white_enable_can_transceivers, diff --git a/panda/board/boards/pedal.h b/panda/board/boards/pedal.h index a2c51f3d5..d545a9faf 100644 --- a/panda/board/boards/pedal.h +++ b/panda/board/boards/pedal.h @@ -83,6 +83,9 @@ const board board_pedal = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .avdd_mV = 3300U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = pedal_init, .enable_can_transceiver = pedal_enable_can_transceiver, .enable_can_transceivers = pedal_enable_can_transceivers, diff --git a/panda/board/boards/red.h b/panda/board/boards/red.h index 0a333cdf4..e7dccc00e 100644 --- a/panda/board/boards/red.h +++ b/panda/board/boards/red.h @@ -22,7 +22,7 @@ void red_enable_can_transceiver(uint8_t transceiver, bool enabled) { } void red_enable_can_transceivers(bool enabled) { - uint8_t main_bus = (car_harness_status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; + uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; for (uint8_t i=1U; i<=4U; i++) { // Leave main CAN always on for CAN-based ignition detection if (i == main_bus) { @@ -53,7 +53,7 @@ void red_set_can_mode(uint8_t mode) { switch (mode) { case CAN_MODE_NORMAL: case CAN_MODE_OBD_CAN2: - if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(car_harness_status == HARNESS_STATUS_FLIPPED)) { + if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { // B12,B13: disable normal mode set_gpio_pullup(GPIOB, 12, PULL_NONE); set_gpio_mode(GPIOB, 12, MODE_ANALOG); @@ -147,7 +147,7 @@ void red_init(void) { red_set_can_mode(CAN_MODE_NORMAL); // flip CAN0 and CAN2 if we are flipped - if (car_harness_status == HARNESS_STATUS_FLIPPED) { + if (harness.status == HARNESS_STATUS_FLIPPED) { can_flip_buses(0, 2); } } @@ -178,6 +178,9 @@ const board board_red = { .has_canfd = true, .has_rtc_battery = false, .fan_max_rpm = 0U, + .avdd_mV = 3300U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = red_init, .enable_can_transceiver = red_enable_can_transceiver, .enable_can_transceivers = red_enable_can_transceivers, diff --git a/panda/board/boards/red_chiplet.h b/panda/board/boards/red_chiplet.h index 52e52c338..8a3f8dbd9 100644 --- a/panda/board/boards/red_chiplet.h +++ b/panda/board/boards/red_chiplet.h @@ -24,7 +24,7 @@ void red_chiplet_enable_can_transceiver(uint8_t transceiver, bool enabled) { } void red_chiplet_enable_can_transceivers(bool enabled) { - uint8_t main_bus = (car_harness_status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; + uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; for (uint8_t i=1U; i<=4U; i++) { // Leave main CAN always on for CAN-based ignition detection if (i == main_bus) { @@ -45,13 +45,13 @@ void red_chiplet_init(void) { // A8, A3: OBD_SBU1_RELAY, OBD_SBU2_RELAY set_gpio_output_type(GPIOA, 8, OUTPUT_TYPE_OPEN_DRAIN); set_gpio_pullup(GPIOA, 8, PULL_NONE); - set_gpio_mode(GPIOA, 8, MODE_OUTPUT); set_gpio_output(GPIOA, 8, 1); + set_gpio_mode(GPIOA, 8, MODE_OUTPUT); set_gpio_output_type(GPIOA, 3, OUTPUT_TYPE_OPEN_DRAIN); set_gpio_pullup(GPIOA, 3, PULL_NONE); - set_gpio_mode(GPIOA, 3, MODE_OUTPUT); set_gpio_output(GPIOA, 3, 1); + set_gpio_mode(GPIOA, 3, MODE_OUTPUT); // G11,B10,D7,B11: transceiver enable set_gpio_pullup(GPIOG, 11, PULL_NONE); @@ -70,13 +70,10 @@ void red_chiplet_init(void) { set_gpio_pullup(GPIOD, 3, PULL_NONE); set_gpio_mode(GPIOD, 3, MODE_OUTPUT); - //B0: 5VOUT_S + // B0: 5VOUT_S set_gpio_pullup(GPIOB, 0, PULL_NONE); set_gpio_mode(GPIOB, 0, MODE_ANALOG); - // Turn on USB load switch. - red_chiplet_set_fan_or_usb_load_switch(true); - // Initialize harness harness_init(); @@ -95,7 +92,7 @@ void red_chiplet_init(void) { red_set_can_mode(CAN_MODE_NORMAL); // flip CAN0 and CAN2 if we are flipped - if (car_harness_status == HARNESS_STATUS_FLIPPED) { + if (harness.status == HARNESS_STATUS_FLIPPED) { can_flip_buses(0, 2); } } diff --git a/panda/board/boards/red_v2.h b/panda/board/boards/red_v2.h index 5b613358e..a9dc4d456 100644 --- a/panda/board/boards/red_v2.h +++ b/panda/board/boards/red_v2.h @@ -2,6 +2,14 @@ // Red Panda V2 with chiplet + Harness // // ///////////////////// // +void red_panda_v2_init(void) { + // common chiplet init + red_chiplet_init(); + + // Turn on USB load switch + red_chiplet_set_fan_or_usb_load_switch(true); +} + const board board_red_v2 = { .board_type = "Red_v2", .board_tick = unused_board_tick, @@ -14,7 +22,10 @@ const board board_red_v2 = { .has_canfd = true, .has_rtc_battery = true, .fan_max_rpm = 0U, - .init = red_chiplet_init, + .avdd_mV = 3300U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, + .init = red_panda_v2_init, .enable_can_transceiver = red_chiplet_enable_can_transceiver, .enable_can_transceivers = red_chiplet_enable_can_transceivers, .set_led = red_set_led, diff --git a/panda/board/boards/tres.h b/panda/board/boards/tres.h index c1b792c41..095d9f8cc 100644 --- a/panda/board/boards/tres.h +++ b/panda/board/boards/tres.h @@ -19,9 +19,9 @@ void tres_set_bootkick(bool enabled){ } bool tres_ignition_prev = false; -void tres_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen) { +void tres_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) { UNUSED(usb_enum); - if (ignition && !tres_ignition_prev) { + if ((ignition && !tres_ignition_prev) || harness_inserted) { // enable bootkick on rising edge of ignition tres_set_bootkick(true); } else if (heartbeat_seen) { @@ -62,11 +62,7 @@ void tres_init(void) { uart_init(&uart_ring_som_debug, 115200); // SPI init - set_gpio_alternate(GPIOE, 11, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 12, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 13, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 14, GPIO_AF5_SPI4); - register_set_bits(&(GPIOE->OSPEEDR), GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12 | GPIO_OSPEEDR_OSPEED13 | GPIO_OSPEEDR_OSPEED14); + gpio_spi_init(); // fan setup set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); @@ -97,7 +93,10 @@ const board board_tres = { .has_spi = true, .has_canfd = true, .has_rtc_battery = true, - .fan_max_rpm = 6500U, // TODO: verify this, copied from dos + .fan_max_rpm = 6600U, + .avdd_mV = 1800U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 3U, .init = tres_init, .enable_can_transceiver = red_chiplet_enable_can_transceiver, .enable_can_transceivers = red_chiplet_enable_can_transceivers, diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h index feb784dd3..9d019b37a 100644 --- a/panda/board/boards/uno.h +++ b/panda/board/boards/uno.h @@ -27,7 +27,7 @@ void uno_enable_can_transceiver(uint8_t transceiver, bool enabled) { void uno_enable_can_transceivers(bool enabled) { for(uint8_t i=1U; i<=4U; i++){ // Leave main CAN always on for CAN-based ignition detection - if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ + if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ uno_enable_can_transceiver(i, true); } else { uno_enable_can_transceiver(i, enabled); @@ -102,7 +102,7 @@ void uno_set_can_mode(uint8_t mode){ switch (mode) { case CAN_MODE_NORMAL: case CAN_MODE_OBD_CAN2: - if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(car_harness_status == HARNESS_STATUS_FLIPPED)) { + if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { // B12,B13: disable OBD mode set_gpio_mode(GPIOB, 12, MODE_INPUT); set_gpio_mode(GPIOB, 13, MODE_INPUT); @@ -126,10 +126,11 @@ void uno_set_can_mode(uint8_t mode){ } } -void uno_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen) { +void uno_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) { UNUSED(ignition); UNUSED(usb_enum); UNUSED(heartbeat_seen); + UNUSED(harness_inserted); if (bootkick_timer != 0U) { bootkick_timer--; } else { @@ -210,12 +211,12 @@ void uno_init(void) { uno_set_can_mode(CAN_MODE_NORMAL); // flip CAN0 and CAN2 if we are flipped - if (car_harness_status == HARNESS_STATUS_FLIPPED) { + if (harness.status == HARNESS_STATUS_FLIPPED) { can_flip_buses(0, 2); } // Switch to phone usb mode if harness connection is powered by less than 7V - if(adc_get_voltage() < 7000U){ + if((adc_get_mV(ADCCHAN_VIN) * VIN_READOUT_DIVIDER) < 7000U){ uno_set_usb_switch(true); } else { uno_set_usb_switch(false); @@ -251,6 +252,9 @@ const board board_uno = { .has_canfd = false, .has_rtc_battery = true, .fan_max_rpm = 5100U, + .avdd_mV = 3300U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = uno_init, .enable_can_transceiver = uno_enable_can_transceiver, .enable_can_transceivers = uno_enable_can_transceivers, diff --git a/panda/board/boards/unused_funcs.h b/panda/board/boards/unused_funcs.h index d3b4a4c6c..f5478f90d 100644 --- a/panda/board/boards/unused_funcs.h +++ b/panda/board/boards/unused_funcs.h @@ -22,10 +22,11 @@ uint32_t unused_read_current(void) { return 0U; } -void unused_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen) { +void unused_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) { UNUSED(ignition); UNUSED(usb_enum); UNUSED(heartbeat_seen); + UNUSED(harness_inserted); } bool unused_read_som_gpio(void) { diff --git a/panda/board/boards/white.h b/panda/board/boards/white.h index 0fe817db1..45d860181 100644 --- a/panda/board/boards/white.h +++ b/panda/board/boards/white.h @@ -142,7 +142,7 @@ void white_set_can_mode(uint8_t mode){ } uint32_t white_read_current(void){ - return adc_get(ADCCHAN_CURRENT); + return adc_get_raw(ADCCHAN_CURRENT); } bool white_check_ignition(void){ @@ -214,7 +214,7 @@ void white_grey_common_init(void) { white_set_can_mode(CAN_MODE_NORMAL); // Init usb power mode - uint32_t voltage = adc_get_voltage(); + uint32_t voltage = adc_get_mV(ADCCHAN_VIN) * VIN_READOUT_DIVIDER; // Init in CDP mode only if panda is powered by 12V. // Otherwise a PC would not be able to flash a standalone panda if (voltage > 8000U) { // 8V threshold @@ -247,6 +247,9 @@ const board board_white = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .avdd_mV = 3300U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = white_init, .enable_can_transceiver = white_enable_can_transceiver, .enable_can_transceivers = white_enable_can_transceivers, diff --git a/panda/board/config.h b/panda/board/config.h index 1eb7de69b..12b514007 100644 --- a/panda/board/config.h +++ b/panda/board/config.h @@ -8,12 +8,15 @@ //#define DEBUG_SPI //#define DEBUG_FAULTS //#define DEBUG_COMMS +//#define DEBUG_FAN #define CAN_INIT_TIMEOUT_MS 500U #define DEEPSLEEP_WAKEUP_DELAY 3U #define USBPACKET_MAX_SIZE 0x40U #define MAX_CAN_MSGS_PER_BULK_TRANSFER 51U +#define VIN_READOUT_DIVIDER 11U + // USB definitions #define USB_VID 0xBBAAU diff --git a/panda/board/drivers/bxcan.h b/panda/board/drivers/bxcan.h index 8c4a3fc46..815ef0fe1 100644 --- a/panda/board/drivers/bxcan.h +++ b/panda/board/drivers/bxcan.h @@ -111,7 +111,7 @@ void process_can(uint8_t can_number) { // check for empty mailbox CANPacket_t to_send; if ((CAN->TSR & (CAN_TSR_TERR0 | CAN_TSR_ALST0)) != 0) { // last TX failed due to error arbitration lost - can_health[can_number].total_rx_lost_cnt += 1U; + can_health[can_number].total_tx_lost_cnt += 1U; CAN->TSR |= (CAN_TSR_TERR0 | CAN_TSR_ALST0); } if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { @@ -167,7 +167,7 @@ void can_rx(uint8_t can_number) { uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); if ((CAN->RF0R & (CAN_RF0R_FOVR0)) != 0) { // RX message lost due to FIFO overrun - can_health[can_number].total_tx_lost_cnt += 1U; + can_health[can_number].total_rx_lost_cnt += 1U; CAN->RF0R &= ~(CAN_RF0R_FOVR0); } @@ -191,7 +191,7 @@ void can_rx(uint8_t can_number) { can_set_checksum(&to_push); // forwarding (panda only) - int bus_fwd_num = safety_fwd_hook(bus_number, &to_push); + int bus_fwd_num = safety_fwd_hook(bus_number, to_push.addr); if (bus_fwd_num != -1) { CANPacket_t to_send; diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index 9fd2743d6..3f2f0830a 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -199,7 +199,7 @@ void ignition_can_hook(CANPacket_t *to_push) { // GM exception if ((addr == 0x160) && (len == 5)) { // this message isn't all zeros when ignition is on - ignition_can = GET_BYTES_04(to_push) != 0U; + ignition_can = GET_BYTES(to_push, 0, 4) != 0U; ignition_can_cnt = 0U; } @@ -257,6 +257,7 @@ void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook) { } } else { safety_tx_blocked += 1U; + to_push->returned = 0U; to_push->rejected = 1U; // data changed diff --git a/panda/board/drivers/fake_siren.h b/panda/board/drivers/fake_siren.h index e6fd78f99..38c87deb0 100644 --- a/panda/board/drivers/fake_siren.h +++ b/panda/board/drivers/fake_siren.h @@ -1,27 +1,29 @@ #include "stm32h7/lli2c.h" #define CODEC_I2C_ADDR 0x10 + // 1Vpp sine wave with 1V offset const uint8_t fake_siren_lut[360] = { 134U, 135U, 137U, 138U, 139U, 140U, 141U, 143U, 144U, 145U, 146U, 148U, 149U, 150U, 151U, 152U, 154U, 155U, 156U, 157U, 158U, 159U, 160U, 162U, 163U, 164U, 165U, 166U, 167U, 168U, 169U, 170U, 171U, 172U, 174U, 175U, 176U, 177U, 177U, 178U, 179U, 180U, 181U, 182U, 183U, 184U, 185U, 186U, 186U, 187U, 188U, 189U, 190U, 190U, 191U, 192U, 193U, 193U, 194U, 195U, 195U, 196U, 196U, 197U, 197U, 198U, 199U, 199U, 199U, 200U, 200U, 201U, 201U, 202U, 202U, 202U, 203U, 203U, 203U, 203U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 205U, 205U, 205U, 205U, 205U, 205U, 205U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 203U, 203U, 203U, 203U, 202U, 202U, 202U, 201U, 201U, 200U, 200U, 199U, 199U, 199U, 198U, 197U, 197U, 196U, 196U, 195U, 195U, 194U, 193U, 193U, 192U, 191U, 190U, 190U, 189U, 188U, 187U, 186U, 186U, 185U, 184U, 183U, 182U, 181U, 180U, 179U, 178U, 177U, 177U, 176U, 175U, 174U, 172U, 171U, 170U, 169U, 168U, 167U, 166U, 165U, 164U, 163U, 162U, 160U, 159U, 158U, 157U, 156U, 155U, 154U, 152U, 151U, 150U, 149U, 148U, 146U, 145U, 144U, 143U, 141U, 140U, 139U, 138U, 137U, 135U, 134U, 133U, 132U, 130U, 129U, 128U, 127U, 125U, 124U, 123U, 122U, 121U, 119U, 118U, 117U, 116U, 115U, 113U, 112U, 111U, 110U, 109U, 108U, 106U, 105U, 104U, 103U, 102U, 101U, 100U, 99U, 98U, 97U, 96U, 95U, 94U, 93U, 92U, 91U, 90U, 89U, 88U, 87U, 86U, 85U, 84U, 83U, 82U, 82U, 81U, 80U, 79U, 78U, 78U, 77U, 76U, 76U, 75U, 74U, 74U, 73U, 72U, 72U, 71U, 71U, 70U, 70U, 69U, 69U, 68U, 68U, 67U, 67U, 67U, 66U, 66U, 66U, 65U, 65U, 65U, 65U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 63U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 65U, 65U, 65U, 65U, 66U, 66U, 66U, 67U, 67U, 67U, 68U, 68U, 69U, 69U, 70U, 70U, 71U, 71U, 72U, 72U, 73U, 74U, 74U, 75U, 76U, 76U, 77U, 78U, 78U, 79U, 80U, 81U, 82U, 82U, 83U, 84U, 85U, 86U, 87U, 88U, 89U, 90U, 91U, 92U, 93U, 94U, 95U, 96U, 97U, 98U, 99U, 100U, 101U, 102U, 103U, 104U, 105U, 106U, 108U, 109U, 110U, 111U, 112U, 113U, 115U, 116U, 117U, 118U, 119U, 121U, 122U, 123U, 124U, 125U, 127U, 128U, 129U, 130U, 132U, 133U }; bool fake_siren_enabled = false; void fake_siren_codec_enable(bool enabled) { - if(enabled) { - bool success = false; + if (enabled) { + bool success = true; success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2B, (1U << 1)); // Left speaker mix from INA1 success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2C, (1U << 1)); // Right speaker mix from INA1 - success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3D, 0x1F, 0b11111); // Left speaker volume - success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3E, 0x1F, 0b11111); // Right speaker volume + success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3D, 0x17, 0b11111); // Left speaker volume + success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3E, 0x17, 0b11111); // Right speaker volume success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x37, 0b101, 0b111); // INA gain success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7)); // Enable INA success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x51, (1U << 7)); // Disable global shutdown if (!success) { - print("Failed to setup the codec for fake siren\n"); + print("Siren codec enable failed\n"); + fault_occurred(FAULT_SIREN_MALFUNCTION); } } else { // Disable INA input. Make sure to retry a few times if the I2C bus is busy. - for(uint8_t i=0U; i<10U; i++) { + for (uint8_t i=0U; i<10U; i++) { if (i2c_clear_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7))) { break; } @@ -56,7 +58,7 @@ void fake_siren_init(void) { register_set(&DMA1_Stream1->M0AR, (uint32_t) fake_siren_lut, 0xFFFFFFFFU); register_set(&DMA1_Stream1->PAR, (uint32_t) &(DAC1->DHR8R1), 0xFFFFFFFFU); DMA1_Stream1->NDTR = sizeof(fake_siren_lut); - register_set(&DMA1_Stream1->FCR, 0U, 0xFFFFFFFFU); + register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U); DMA1_Stream1->CR = (0b11 << DMA_SxCR_PL_Pos); DMA1_Stream1->CR |= DMA_SxCR_MINC | DMA_SxCR_CIRC | (1 << DMA_SxCR_DIR_Pos); @@ -71,4 +73,4 @@ void fake_siren_init(void) { // Enable the I2C to the codec i2c_init(I2C5); fake_siren_codec_enable(false); -} \ No newline at end of file +} diff --git a/panda/board/drivers/fan.h b/panda/board/drivers/fan.h index d5e32ae5f..15296079a 100644 --- a/panda/board/drivers/fan.h +++ b/panda/board/drivers/fan.h @@ -5,55 +5,91 @@ struct fan_state_t { uint8_t power; float error_integral; uint8_t stall_counter; + uint8_t stall_threshold; + uint8_t total_stall_count; + uint8_t cooldown_counter; } fan_state_t; struct fan_state_t fan_state; -#define FAN_I 0.001f -#define FAN_STALL_THRESHOLD 25U +const float FAN_I = 0.001f; -void fan_set_power(uint8_t percentage){ - fan_state.target_rpm = ((current_board->fan_max_rpm * MIN(100U, MAX(0U, percentage))) / 100U); +const uint8_t FAN_TICK_FREQ = 8U; +const uint8_t FAN_STALL_THRESHOLD_MIN = 3U; +const uint8_t FAN_STALL_THRESHOLD_MAX = 8U; + + +void fan_set_power(uint8_t percentage) { + fan_state.target_rpm = ((current_board->fan_max_rpm * CLAMP(percentage, 0U, 100U)) / 100U); } -// Call this at 8Hz -void fan_tick(void){ +void llfan_init(void); +void fan_init(void) { + fan_state.stall_threshold = FAN_STALL_THRESHOLD_MIN; + fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * FAN_TICK_FREQ; + llfan_init(); +} + +// Call this at FAN_TICK_FREQ +void fan_tick(void) { if (current_board->fan_max_rpm > 0U) { - // 4 interrupts per rotation - uint16_t fan_rpm_fast = fan_state.tach_counter * (60U * 8U / 4U); + // Measure fan RPM + uint16_t fan_rpm_fast = fan_state.tach_counter * (60U * FAN_TICK_FREQ / 4U); // 4 interrupts per rotation fan_state.tach_counter = 0U; fan_state.rpm = (fan_rpm_fast + (3U * fan_state.rpm)) / 4U; - // Enable fan if we want it to spin - current_board->set_fan_enabled(fan_state.target_rpm > 0U); - // Stall detection - if (fan_state.power > 0U) { - if (fan_rpm_fast == 0U) { - fan_state.stall_counter = MIN(fan_state.stall_counter + 1U, 255U); + bool fan_stalled = false; + if (current_board->fan_stall_recovery) { + if (fan_state.target_rpm > 0U) { + if (fan_rpm_fast == 0U) { + fan_state.stall_counter = MIN(fan_state.stall_counter + 1U, 255U); + } else { + fan_state.stall_counter = 0U; + } + + if (fan_state.stall_counter > (fan_state.stall_threshold*FAN_TICK_FREQ)) { + fan_stalled = true; + fan_state.stall_counter = 0U; + fan_state.stall_threshold = CLAMP(fan_state.stall_threshold + 2U, FAN_STALL_THRESHOLD_MIN, FAN_STALL_THRESHOLD_MAX); + fan_state.total_stall_count += 1U; + + // datasheet gives this range as the minimum startup duty + fan_state.error_integral = CLAMP(fan_state.error_integral, 20.0f, 45.0f); + } } else { fan_state.stall_counter = 0U; + fan_state.stall_threshold = FAN_STALL_THRESHOLD_MIN; } + } - if (fan_state.stall_counter > FAN_STALL_THRESHOLD) { - // Stall detected, power cycling fan controller - current_board->set_fan_enabled(false); + #ifdef DEBUG_FAN + puth(fan_state.target_rpm); + print(" "); puth(fan_rpm_fast); + print(" "); puth(fan_state.power); + print(" "); puth(fan_state.stall_counter); + print("\n"); + #endif - // clip integral, can't fully reset otherwise we may always be stuck in stall detection - fan_state.error_integral = MIN(50.0f, MAX(0.0f, fan_state.error_integral)); - } + // Cooldown counter + if (fan_state.target_rpm > 0U) { + fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * FAN_TICK_FREQ; } else { - fan_state.stall_counter = 0U; - fan_state.error_integral = 0.0f; + if (fan_state.cooldown_counter > 0U) { + fan_state.cooldown_counter--; + } } // Update controller - float feedforward = (fan_state.target_rpm * 100.0f) / current_board->fan_max_rpm; - float error = fan_state.target_rpm - fan_rpm_fast; + if (fan_state.target_rpm == 0U) { + fan_state.error_integral = 0.0f; + } else { + float error = fan_state.target_rpm - fan_rpm_fast; + fan_state.error_integral += FAN_I * error; + } + fan_state.power = CLAMP(fan_state.error_integral, 0U, 100U); - fan_state.error_integral += FAN_I * error; - fan_state.error_integral = MIN(70.0f, MAX(-70.0f, fan_state.error_integral)); - - fan_state.power = MIN(100U, MAX(0U, feedforward + fan_state.error_integral)); + // Set PWM and enable line pwm_set(TIM3, 3, fan_state.power); + current_board->set_fan_enabled(!fan_stalled && ((fan_state.target_rpm > 0U) || (fan_state.cooldown_counter > 0U))); } } diff --git a/panda/board/drivers/fdcan.h b/panda/board/drivers/fdcan.h index d8793fd66..4dcc3fdf4 100644 --- a/panda/board/drivers/fdcan.h +++ b/panda/board/drivers/fdcan.h @@ -61,9 +61,6 @@ void update_can_health_pkt(uint8_t can_number, bool error_irq) { if (error_irq) { can_health[can_number].total_error_cnt += 1U; - if ((CANx->IR & (FDCAN_IR_RF0L)) != 0) { - can_health[can_number].total_rx_lost_cnt += 1U; - } if ((CANx->IR & (FDCAN_IR_TEFL)) != 0) { can_health[can_number].total_tx_lost_cnt += 1U; } @@ -88,6 +85,7 @@ void process_can(uint8_t can_number) { can_health[can_number].total_tx_cnt += 1U; uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE); + // get the index of the next TX FIFO element (0 to FDCAN_TX_FIFO_EL_CNT - 1) uint8_t tx_index = (CANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F; // only send if we have received a packet canfd_fifo *fifo; @@ -145,9 +143,15 @@ void can_rx(uint8_t can_number) { // can is live pending_can_live = 1; - // getting new message index (0 to 63) + // get the index of the next RX FIFO element (0 to FDCAN_RX_FIFO_0_EL_CNT - 1) uint8_t rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F); + // Recommended to offset get index by at least +1 if RX FIFO is in overwrite mode and full (datasheet) + if((CANx->RXF0S & FDCAN_RXF0S_F0F) == FDCAN_RXF0S_F0F) { + rx_fifo_idx = ((rx_fifo_idx + 1U) >= FDCAN_RX_FIFO_0_EL_CNT) ? 0U : (rx_fifo_idx + 1U); + can_health[can_number].total_rx_lost_cnt += 1U; // At least one message was lost + } + uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET); CANPacket_t to_push; canfd_fifo *fifo; @@ -173,7 +177,7 @@ void can_rx(uint8_t can_number) { can_set_checksum(&to_push); // forwarding (panda only) - int bus_fwd_num = safety_fwd_hook(bus_number, &to_push); + int bus_fwd_num = safety_fwd_hook(bus_number, to_push.addr); if (bus_fwd_num != -1) { CANPacket_t to_send; diff --git a/panda/board/drivers/harness.h b/panda/board/drivers/harness.h index c2b5ac95f..6bf3fe1b9 100644 --- a/panda/board/drivers/harness.h +++ b/panda/board/drivers/harness.h @@ -1,8 +1,16 @@ -uint8_t car_harness_status = 0U; #define HARNESS_STATUS_NC 0U #define HARNESS_STATUS_NORMAL 1U #define HARNESS_STATUS_FLIPPED 2U +struct harness_t { + uint8_t status; + uint16_t sbu1_voltage_mV; + uint16_t sbu2_voltage_mV; + bool relay_driven; + bool sbu_adc_lock; +}; +struct harness_t harness; + struct harness_configuration { const bool has_harness; GPIO_TypeDef *GPIO_SBU1; @@ -17,26 +25,42 @@ struct harness_configuration { uint8_t adc_channel_SBU2; }; -// this function will be the API for tici void set_intercept_relay(bool intercept) { - if (car_harness_status != HARNESS_STATUS_NC) { - if (intercept) { - print("switching harness to intercept (relay on)\n"); - } else { - print("switching harness to passthrough (relay off)\n"); + if (current_board->harness_config->has_harness) { + bool drive_relay = intercept; + if (harness.status == HARNESS_STATUS_NC) { + // no harness, no relay to drive + drive_relay = false; } - if(car_harness_status == HARNESS_STATUS_NORMAL){ - set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !intercept); + if (drive_relay) { + harness.relay_driven = true; + } + + // wait until we're not reading the analog voltages anymore + while (harness.sbu_adc_lock == true) {} + + if (harness.status == HARNESS_STATUS_NORMAL) { + set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, true); + set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !drive_relay); } else { - set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !intercept); + set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !drive_relay); + set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, true); + } + + if (!drive_relay) { + harness.relay_driven = false; } } } bool harness_check_ignition(void) { bool ret = false; - switch(car_harness_status){ + + // wait until we're not reading the analog voltages anymore + while (harness.sbu_adc_lock == true) {} + + switch(harness.status){ case HARNESS_STATUS_NORMAL: ret = !get_gpio_input(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1); break; @@ -50,27 +74,46 @@ bool harness_check_ignition(void) { } uint8_t harness_detect_orientation(void) { - uint8_t ret = HARNESS_STATUS_NC; + uint8_t ret = harness.status; #ifndef BOOTSTUB - uint32_t sbu1_voltage = adc_get(current_board->harness_config->adc_channel_SBU1); - uint32_t sbu2_voltage = adc_get(current_board->harness_config->adc_channel_SBU2); + // We can't detect orientation if the relay is being driven + if (!harness.relay_driven && current_board->harness_config->has_harness) { + harness.sbu_adc_lock = true; + set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_ANALOG); + set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_ANALOG); - // Detect connection and orientation - if((sbu1_voltage < HARNESS_CONNECTED_THRESHOLD) || (sbu2_voltage < HARNESS_CONNECTED_THRESHOLD)){ - if (sbu1_voltage < sbu2_voltage) { - // orientation flipped (PANDA_SBU1->HARNESS_SBU1(relay), PANDA_SBU2->HARNESS_SBU2(ign)) - ret = HARNESS_STATUS_FLIPPED; + harness.sbu1_voltage_mV = adc_get_mV(current_board->harness_config->adc_channel_SBU1); + harness.sbu2_voltage_mV = adc_get_mV(current_board->harness_config->adc_channel_SBU2); + uint16_t detection_threshold = current_board->avdd_mV / 2U; + + // Detect connection and orientation + if((harness.sbu1_voltage_mV < detection_threshold) || (harness.sbu2_voltage_mV < detection_threshold)){ + if (harness.sbu1_voltage_mV < harness.sbu2_voltage_mV) { + // orientation flipped (PANDA_SBU1->HARNESS_SBU1(relay), PANDA_SBU2->HARNESS_SBU2(ign)) + ret = HARNESS_STATUS_FLIPPED; + } else { + // orientation normal (PANDA_SBU2->HARNESS_SBU1(relay), PANDA_SBU1->HARNESS_SBU2(ign)) + ret = HARNESS_STATUS_NORMAL; + } } else { - // orientation normal (PANDA_SBU2->HARNESS_SBU1(relay), PANDA_SBU1->HARNESS_SBU2(ign)) - ret = HARNESS_STATUS_NORMAL; + ret = HARNESS_STATUS_NC; } + + // Pins are not 5V tolerant in ADC mode + set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT); + set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT); + harness.sbu_adc_lock = false; } #endif return ret; } +void harness_tick(void) { + harness.status = harness_detect_orientation(); +} + void harness_init(void) { // delay such that the connection is fully made before trying orientation detection current_board->set_led(LED_BLUE, true); @@ -78,18 +121,13 @@ void harness_init(void) { current_board->set_led(LED_BLUE, false); // try to detect orientation - uint8_t ret = harness_detect_orientation(); - if (ret != HARNESS_STATUS_NC) { - print("detected car harness with orientation "); puth2(ret); print("\n"); - car_harness_status = ret; - - // set the SBU lines to be inputs before using the relay. The lines are not 5V tolerant in ADC mode! - set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT); - set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT); - - // keep busses connected by default - set_intercept_relay(false); + harness.status = harness_detect_orientation(); + if (harness.status != HARNESS_STATUS_NC) { + print("detected car harness with orientation "); puth2(harness.status); print("\n"); } else { print("failed to detect car harness!\n"); } + + // keep buses connected by default + set_intercept_relay(false); } diff --git a/panda/board/drivers/rtc.h b/panda/board/drivers/rtc.h index 552b86758..df121e3e8 100644 --- a/panda/board/drivers/rtc.h +++ b/panda/board/drivers/rtc.h @@ -1,6 +1,3 @@ -#define RCC_BDCR_OPTIONS_LSE (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL_0 | RCC_BDCR_LSEON) -#define RCC_BDCR_OPTIONS_LSI (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL_1) - #define YEAR_OFFSET 2000U typedef struct __attribute__((packed)) timestamp_t { @@ -21,39 +18,6 @@ uint16_t from_bcd(uint8_t value){ return (((value & 0xF0U) >> 4U) * 10U) + (value & 0x0FU); } -void rtc_init(void){ - uint32_t bdcr_opts = 0U; - uint32_t bdcr_mask = 0U; - if (current_board->has_rtc_battery) { - bdcr_opts = RCC_BDCR_OPTIONS_LSE; - bdcr_mask = RCC_BDCR_MASK_LSE; - } else { - bdcr_opts = RCC_BDCR_OPTIONS_LSI; - bdcr_mask = RCC_BDCR_MASK_LSI; - RCC->CSR |= RCC_CSR_LSION; - while((RCC->CSR & RCC_CSR_LSIRDY) == 0){} - } - - // Initialize RTC module and clock if not done already. - if((RCC->BDCR & bdcr_mask) != bdcr_opts){ - print("Initializing RTC\n"); - // Reset backup domain - register_set_bits(&(RCC->BDCR), RCC_BDCR_BDRST); - - // Disable write protection - disable_bdomain_protection(); - - // Clear backup domain reset - register_clear_bits(&(RCC->BDCR), RCC_BDCR_BDRST); - - // Set RTC options - register_set(&(RCC->BDCR), bdcr_opts, bdcr_mask); - - // Enable write protection - enable_bdomain_protection(); - } -} - void rtc_set_time(timestamp_t time){ print("Setting RTC time\n"); diff --git a/panda/board/drivers/simple_watchdog.h b/panda/board/drivers/simple_watchdog.h new file mode 100644 index 000000000..fe0c856af --- /dev/null +++ b/panda/board/drivers/simple_watchdog.h @@ -0,0 +1,26 @@ +typedef struct simple_watchdog_state_t { + uint32_t fault; + uint32_t last_ts; + uint32_t threshold; +} simple_watchdog_state_t; + +simple_watchdog_state_t wd_state; + + +void simple_watchdog_kick(void) { + uint32_t ts = microsecond_timer_get(); + + uint32_t et = get_ts_elapsed(ts, wd_state.last_ts); + if (et > wd_state.threshold) { + print("WD timeout 0x"); puth(et); print("\n"); + fault_occurred(wd_state.fault); + } + + wd_state.last_ts = ts; +} + +void simple_watchdog_init(uint32_t fault, uint32_t threshold) { + wd_state.fault = fault; + wd_state.threshold = threshold; + wd_state.last_ts = microsecond_timer_get(); +} diff --git a/panda/board/drivers/spi.h b/panda/board/drivers/spi.h index f7382e469..a18597498 100644 --- a/panda/board/drivers/spi.h +++ b/panda/board/drivers/spi.h @@ -3,6 +3,10 @@ #define SPI_BUF_SIZE 1024U #define SPI_TIMEOUT_US 10000U +// we expect less than 50 transactions (including control messages and +// CAN buffers) at the 100Hz boardd interval, plus some buffer +#define SPI_IRQ_RATE 6500U + #ifdef STM32H7 __attribute__((section(".ram_d1"))) uint8_t spi_buf_rx[SPI_BUF_SIZE]; __attribute__((section(".ram_d1"))) uint8_t spi_buf_tx[SPI_BUF_SIZE]; @@ -27,10 +31,13 @@ enum { SPI_STATE_DATA_TX }; +bool spi_tx_dma_done = false; uint8_t spi_state = SPI_STATE_HEADER; uint8_t spi_endpoint; uint16_t spi_data_len_mosi; uint16_t spi_data_len_miso; +uint16_t spi_checksum_error_count = 0; + #define SPI_HEADER_SIZE 7U @@ -40,10 +47,6 @@ void llspi_mosi_dma(uint8_t *addr, int len); void llspi_miso_dma(uint8_t *addr, int len); void spi_init(void) { - // clear buffers (for debugging) - (void)memset(spi_buf_rx, 0, SPI_BUF_SIZE); - (void)memset(spi_buf_tx, 0, SPI_BUF_SIZE); - // platform init llspi_init(); @@ -61,8 +64,10 @@ bool check_checksum(uint8_t *data, uint16_t len) { return checksum == 0U; } -void spi_handle_rx(void) { - uint8_t next_rx_state = SPI_STATE_HEADER; +void spi_rx_done(void) { + uint16_t response_len = 0U; + uint8_t next_rx_state = SPI_STATE_HEADER_NACK; + bool checksum_valid = false; // parse header spi_endpoint = spi_buf_rx[1]; @@ -70,45 +75,47 @@ void spi_handle_rx(void) { spi_data_len_miso = (spi_buf_rx[5] << 8) | spi_buf_rx[4]; if (spi_state == SPI_STATE_HEADER) { - if ((spi_buf_rx[0] == SPI_SYNC_BYTE) && check_checksum(spi_buf_rx, SPI_HEADER_SIZE)) { + checksum_valid = check_checksum(spi_buf_rx, SPI_HEADER_SIZE); + if ((spi_buf_rx[0] == SPI_SYNC_BYTE) && checksum_valid) { // response: ACK and start receiving data portion spi_buf_tx[0] = SPI_HACK; next_rx_state = SPI_STATE_HEADER_ACK; + response_len = 1U; } else { // response: NACK and reset state machine print("- incorrect header sync or checksum "); hexdump(spi_buf_rx, SPI_HEADER_SIZE); spi_buf_tx[0] = SPI_NACK; next_rx_state = SPI_STATE_HEADER_NACK; + response_len = 1U; } - llspi_miso_dma(spi_buf_tx, 1); } else if (spi_state == SPI_STATE_DATA_RX) { // We got everything! Based on the endpoint specified, call the appropriate handler - uint16_t response_len = 0U; - bool reponse_ack = false; - if (check_checksum(&(spi_buf_rx[SPI_HEADER_SIZE]), spi_data_len_mosi + 1U)) { + bool response_ack = false; + checksum_valid = check_checksum(&(spi_buf_rx[SPI_HEADER_SIZE]), spi_data_len_mosi + 1U); + if (checksum_valid) { if (spi_endpoint == 0U) { if (spi_data_len_mosi >= sizeof(ControlPacket_t)) { ControlPacket_t ctrl; (void)memcpy(&ctrl, &spi_buf_rx[SPI_HEADER_SIZE], sizeof(ControlPacket_t)); response_len = comms_control_handler(&ctrl, &spi_buf_tx[3]); - reponse_ack = true; + response_ack = true; } else { print("SPI: insufficient data for control handler\n"); } } else if ((spi_endpoint == 1U) || (spi_endpoint == 0x81U)) { if (spi_data_len_mosi == 0U) { response_len = comms_can_read(&(spi_buf_tx[3]), spi_data_len_miso); - reponse_ack = true; + response_ack = true; } else { print("SPI: did not expect data for can_read\n"); } } else if (spi_endpoint == 2U) { comms_endpoint2_write(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi); - reponse_ack = true; + response_ack = true; } else if (spi_endpoint == 3U) { if (spi_data_len_mosi > 0U) { comms_can_write(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi); - reponse_ack = true; + response_ack = true; } else { print("SPI: did expect data for can_write\n"); } @@ -117,35 +124,56 @@ void spi_handle_rx(void) { } } else { // Checksum was incorrect - reponse_ack = false; - print("- incorrect data checksum\n"); + response_ack = false; + print("- incorrect data checksum "); + puth2(spi_data_len_mosi); + print("\n"); + hexdump(spi_buf_rx, SPI_HEADER_SIZE); + hexdump(&(spi_buf_rx[SPI_HEADER_SIZE]), MIN(spi_data_len_mosi, 64)); + print("\n"); } - // Setup response header - spi_buf_tx[0] = reponse_ack ? SPI_DACK : SPI_NACK; - spi_buf_tx[1] = response_len & 0xFFU; - spi_buf_tx[2] = (response_len >> 8) & 0xFFU; + if (!response_ack) { + spi_buf_tx[0] = SPI_NACK; + next_rx_state = SPI_STATE_HEADER_NACK; + response_len = 1U; + } else { + // Setup response header + spi_buf_tx[0] = SPI_DACK; + spi_buf_tx[1] = response_len & 0xFFU; + spi_buf_tx[2] = (response_len >> 8) & 0xFFU; - // Add checksum - uint8_t checksum = SPI_CHECKSUM_START; - for(uint16_t i = 0U; i < (response_len + 3U); i++) { - checksum ^= spi_buf_tx[i]; + // Add checksum + uint8_t checksum = SPI_CHECKSUM_START; + for(uint16_t i = 0U; i < (response_len + 3U); i++) { + checksum ^= spi_buf_tx[i]; + } + spi_buf_tx[response_len + 3U] = checksum; + response_len += 4U; + + next_rx_state = SPI_STATE_DATA_TX; } - spi_buf_tx[response_len + 3U] = checksum; - - // Write response - llspi_miso_dma(spi_buf_tx, response_len + 4U); - - next_rx_state = SPI_STATE_DATA_TX; } else { print("SPI: RX unexpected state: "); puth(spi_state); print("\n"); } + // send out response + if (response_len == 0U) { + print("SPI: no response\n"); + spi_buf_tx[0] = SPI_NACK; + spi_state = SPI_STATE_HEADER_NACK; + response_len = 1U; + } + llspi_miso_dma(spi_buf_tx, response_len); + spi_state = next_rx_state; + if (!checksum_valid && (spi_checksum_error_count < __UINT16_MAX__)) { + spi_checksum_error_count += 1U; + } } -void spi_handle_tx(bool timed_out) { - if ((spi_state == SPI_STATE_HEADER_NACK) || timed_out) { +void spi_tx_done(bool reset) { + if ((spi_state == SPI_STATE_HEADER_NACK) || reset) { // Reset state spi_state = SPI_STATE_HEADER; llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); diff --git a/panda/board/drivers/usb.h b/panda/board/drivers/usb.h index 1ac5b21b3..469299d74 100644 --- a/panda/board/drivers/usb.h +++ b/panda/board/drivers/usb.h @@ -24,6 +24,7 @@ typedef union _USB_Setup { USB_Setup_TypeDef; bool usb_enumerated = false; +uint16_t usb_last_frame_num = 0U; void usb_init(void); void usb_cb_ep3_out_complete(void); @@ -471,6 +472,12 @@ char to_hex_char(int a) { return ret; } +void usb_tick(void) { + uint16_t current_frame_num = (USBx_DEVICE->DSTS & USB_OTG_DSTS_FNSOF_Msk) >> USB_OTG_DSTS_FNSOF_Pos; + usb_enumerated = (current_frame_num != usb_last_frame_num); + usb_last_frame_num = current_frame_num; +} + void usb_setup(void) { int resp_len; ControlPacket_t control_req; @@ -673,24 +680,11 @@ void usb_irqhandler(void) { print("connector ID status change\n"); } - if ((gintsts & USB_OTG_GINTSTS_ESUSP) != 0) { - print("ESUSP detected\n"); - } - - if ((gintsts & USB_OTG_GINTSTS_EOPF) != 0) { - usb_enumerated = true; - } - if ((gintsts & USB_OTG_GINTSTS_USBRST) != 0) { print("USB reset\n"); - usb_enumerated = false; usb_reset(); } - if ((gintsts & USB_OTG_GINTSTS_USBSUSP) != 0) { - usb_enumerated = false; - } - if ((gintsts & USB_OTG_GINTSTS_ENUMDNE) != 0) { print("enumeration done"); // Full speed, ENUMSPD diff --git a/panda/board/drivers/watchdog.h b/panda/board/drivers/watchdog.h new file mode 100644 index 000000000..d0ee32cb2 --- /dev/null +++ b/panda/board/drivers/watchdog.h @@ -0,0 +1,30 @@ +// TODO: why doesn't it define these? +#ifdef STM32F2 +#define IWDG_PR_PR_Msk 0x7U +#define IWDG_RLR_RL_Msk 0xFFFU +#endif + +typedef enum { + WATCHDOG_50_MS = (400U - 1U), + WATCHDOG_500_MS = 4000U, +} WatchdogTimeout; + +void watchdog_feed(void) { + IND_WDG->KR = 0xAAAAU; +} + +void watchdog_init(WatchdogTimeout timeout) { + // enable watchdog + IND_WDG->KR = 0xCCCCU; + IND_WDG->KR = 0x5555U; + + // 32KHz / 4 prescaler = 8000Hz + register_set(&(IND_WDG->PR), 0x0U, IWDG_PR_PR_Msk); + register_set(&(IND_WDG->RLR), timeout, IWDG_RLR_RL_Msk); + + // wait for watchdog to be updated + while (IND_WDG->SR != 0U); + + // start the countdown + watchdog_feed(); +} diff --git a/panda/board/faults.h b/panda/board/faults.h index c5df1131b..294ac1176 100644 --- a/panda/board/faults.h +++ b/panda/board/faults.h @@ -2,7 +2,7 @@ #define FAULT_STATUS_TEMPORARY 1U #define FAULT_STATUS_PERMANENT 2U -// Fault types +// Fault types, matches cereal.log.PandaState.FaultType #define FAULT_RELAY_MALFUNCTION (1U << 0) #define FAULT_UNUSED_INTERRUPT_HANDLED (1U << 1) #define FAULT_INTERRUPT_RATE_CAN_1 (1U << 2) @@ -28,6 +28,8 @@ #define FAULT_INTERRUPT_RATE_EXTI (1U << 22) #define FAULT_INTERRUPT_RATE_SPI (1U << 23) #define FAULT_INTERRUPT_RATE_UART_7 (1U << 24) +#define FAULT_SIREN_MALFUNCTION (1U << 25) +#define FAULT_HEARTBEAT_LOOP_WATCHDOG (1U << 26) // Permanent faults #define PERMANENT_FAULTS 0U diff --git a/panda/board/flash.py b/panda/board/flash.py new file mode 100755 index 000000000..d1a78e2c5 --- /dev/null +++ b/panda/board/flash.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python3 +import os +import subprocess + +from panda import Panda + +board_path = os.path.dirname(os.path.realpath(__file__)) + +if __name__ == "__main__": + subprocess.check_call(f"scons -C {board_path}/.. -j$(nproc) {board_path}", shell=True) + + serials = Panda.list() + print(f"found {len(serials)} panda(s) - {serials}") + for s in serials: + print("flashing", s) + with Panda(serial=s) as p: + p.flash() diff --git a/panda/board/flash.sh b/panda/board/flash.sh deleted file mode 100755 index cdb8f1be6..000000000 --- a/panda/board/flash.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env sh -set -e - -scons -u -j$(nproc) -printf %b 'from python import Panda\nfor serial in Panda.list(): Panda(serial).flash()' | PYTHONPATH=.. python3 diff --git a/panda/board/flasher.h b/panda/board/flasher.h index f1a4f645e..97d81096e 100644 --- a/panda/board/flasher.h +++ b/panda/board/flasher.h @@ -2,6 +2,8 @@ uint32_t *prog_ptr = NULL; bool unlocked = false; +void spi_init(void); + #ifdef uart_ring void debug_ring_callback(uart_ring *ring) {} #endif @@ -41,6 +43,11 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { resp[1] = 0xff; } break; + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; // **** 0xc3: fetch MCU UID case 0xc3: #ifdef UID_BASE @@ -289,6 +296,12 @@ void soft_flasher_start(void) { // enable USB usb_init(); + // enable SPI + if (current_board->has_spi) { + gpio_spi_init(); + spi_init(); + } + // green LED on for flashing current_board->set_led(LED_GREEN, 1); diff --git a/panda/board/health.h b/panda/board/health.h index afe4f7254..4dce741fb 100644 --- a/panda/board/health.h +++ b/panda/board/health.h @@ -1,7 +1,6 @@ // When changing these structs, python/__init__.py needs to be kept up to date! -#define HEALTH_PACKET_VERSION 11 - +#define HEALTH_PACKET_VERSION 14 struct __attribute__((packed)) health_t { uint32_t uptime_pkt; uint32_t voltage_pkt; @@ -26,6 +25,10 @@ struct __attribute__((packed)) health_t { float interrupt_load; uint8_t fan_power; uint8_t safety_rx_checks_invalid; + uint16_t spi_checksum_error_count; + uint8_t fan_stall_count; + uint16_t sbu1_voltage_mV; + uint16_t sbu2_voltage_mV; }; #define CAN_HEALTH_PACKET_VERSION 4 @@ -38,11 +41,11 @@ typedef struct __attribute__((packed)) { uint8_t last_stored_error; // last LEC positive error code stored uint8_t last_data_error; // DLEC (for CANFD only) uint8_t last_data_stored_error; // last DLEC positive error code stored (for CANFD only) - uint8_t receive_error_cnt; // REC - uint8_t transmit_error_cnt; // TEC - uint32_t total_error_cnt; // How many times any error interrupt were invoked - uint32_t total_tx_lost_cnt; // Tx event FIFO element Lost - uint32_t total_rx_lost_cnt; // Rx FIFO 0 message Lost + uint8_t receive_error_cnt; // Actual state of the receive error counter, values between 0 and 127. FDCAN_ECR.REC + uint8_t transmit_error_cnt; // Actual state of the transmit error counter, values between 0 and 255. FDCAN_ECR.TEC + uint32_t total_error_cnt; // How many times any error interrupt was invoked + uint32_t total_tx_lost_cnt; // Tx event FIFO element lost + uint32_t total_rx_lost_cnt; // Rx FIFO 0 message lost due to FIFO full condition uint32_t total_tx_cnt; uint32_t total_rx_cnt; uint32_t total_fwd_cnt; // Messages forwarded from one bus to another diff --git a/panda/board/main.c b/panda/board/main.c index 4620a6157..b8831c9d1 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -5,6 +5,7 @@ #include "drivers/usb.h" #include "drivers/gmlan_alt.h" #include "drivers/kline_init.h" +#include "drivers/simple_watchdog.h" #include "early_init.h" #include "provision.h" @@ -143,6 +144,7 @@ void __attribute__ ((noinline)) enable_fpu(void) { // called at 8Hz uint8_t loop_counter = 0U; +uint8_t previous_harness_status = HARNESS_STATUS_NC; void tick_handler(void) { if (TICK_TIMER->SR != 0) { // siren @@ -150,6 +152,8 @@ void tick_handler(void) { // tick drivers at 8Hz fan_tick(); + usb_tick(); + simple_watchdog_kick(); // decimated to 1Hz if (loop_counter == 0U) { @@ -177,8 +181,11 @@ void tick_handler(void) { current_board->set_led(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED)); // tick drivers at 1Hz + harness_tick(); + const bool recent_heartbeat = heartbeat_counter == 0U; - current_board->board_tick(check_started(), usb_enumerated, recent_heartbeat); + current_board->board_tick(check_started(), usb_enumerated, recent_heartbeat, ((harness.status != previous_harness_status) && (harness.status != HARNESS_STATUS_NC))); + previous_harness_status = harness.status; // increase heartbeat counter and cap it at the uint32 limit if (heartbeat_counter < __UINT32_MAX__) { @@ -194,7 +201,7 @@ void tick_handler(void) { siren_countdown -= 1U; } - if (controls_allowed) { + if (controls_allowed || heartbeat_engaged) { controls_allowed_countdown = 30U; } else if (controls_allowed_countdown > 0U) { controls_allowed_countdown -= 1U; @@ -229,6 +236,9 @@ void tick_handler(void) { heartbeat_lost = true; } + // clear heartbeat engaged state + heartbeat_engaged = false; + if (current_safety_mode != SAFETY_SILENT) { set_safety_mode(SAFETY_SILENT, 0U); } @@ -353,7 +363,7 @@ int main(void) { } if (current_board->fan_max_rpm > 0U) { - llfan_init(); + fan_init(); } microsecond_timer_init(); @@ -364,6 +374,9 @@ int main(void) { // enable CAN TXs current_board->enable_can_transceivers(true); + // init watchdog for heartbeat loop, fed at 8Hz + simple_watchdog_init(FAULT_HEARTBEAT_LOOP_WATCHDOG, (3U * 1000000U / 8U)); + // 8Hz timer REGISTER_INTERRUPT(TICK_TIMER_IRQ, tick_handler, 10U, FAULT_INTERRUPT_RATE_TICK) tick_timer_init(); diff --git a/panda/board/main_comms.h b/panda/board/main_comms.h index 0d3a97aa3..b58ebd1df 100644 --- a/panda/board/main_comms.h +++ b/panda/board/main_comms.h @@ -9,10 +9,10 @@ int get_health_pkt(void *dat) { struct health_t * health = (struct health_t*)dat; health->uptime_pkt = uptime_cnt; - health->voltage_pkt = adc_get_voltage(); + health->voltage_pkt = adc_get_mV(ADCCHAN_VIN) * VIN_READOUT_DIVIDER; health->current_pkt = current_board->read_current(); - //Use the GPIO pin to determine ignition or use a CAN based logic + // Use the GPIO pin to determine ignition or use a CAN based logic health->ignition_line_pkt = (uint8_t)(current_board->check_ignition()); health->ignition_can_pkt = (uint8_t)(ignition_can); @@ -23,7 +23,7 @@ int get_health_pkt(void *dat) { health->tx_buffer_overflow_pkt = tx_buffer_overflow; health->rx_buffer_overflow_pkt = rx_buffer_overflow; health->gmlan_send_errs_pkt = gmlan_send_errs; - health->car_harness_status_pkt = car_harness_status; + health->car_harness_status_pkt = harness.status; health->safety_mode_pkt = (uint8_t)(current_safety_mode); health->safety_param_pkt = current_safety_param; health->alternative_experience_pkt = alternative_experience; @@ -31,12 +31,18 @@ int get_health_pkt(void *dat) { health->heartbeat_lost_pkt = (uint8_t)(heartbeat_lost); health->safety_rx_checks_invalid = safety_rx_checks_invalid; + health->spi_checksum_error_count = spi_checksum_error_count; + health->fault_status_pkt = fault_status; health->faults_pkt = faults; health->interrupt_load = interrupt_load; health->fan_power = fan_state.power; + health->fan_stall_count = fan_state.total_stall_count; + + health->sbu1_voltage_mV = harness.sbu1_voltage_mV; + health->sbu2_voltage_mV = harness.sbu2_voltage_mV; return sizeof(*health); } diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c index 71df7fe4e..e6b197cf7 100644 --- a/panda/board/pedal/main.c +++ b/panda/board/pedal/main.c @@ -244,8 +244,8 @@ void TIM3_IRQ_Handler(void) { void pedal(void) { // read/write - pdl0 = adc_get(ADCCHAN_ACCEL0); - pdl1 = adc_get(ADCCHAN_ACCEL1); + pdl0 = adc_get_raw(ADCCHAN_ACCEL0); + pdl1 = adc_get_raw(ADCCHAN_ACCEL1); // write the pedal to the DAC if (state == NO_FAULT) { @@ -302,7 +302,7 @@ int main(void) { timer_init(TIM3, 15); NVIC_EnableIRQ(TIM3_IRQn); - watchdog_init(); + watchdog_init(WATCHDOG_50_MS); print("**** INTERRUPTS ON ****\n"); enable_interrupts(); diff --git a/panda/board/recover.py b/panda/board/recover.py new file mode 100755 index 000000000..5284a4e29 --- /dev/null +++ b/panda/board/recover.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +import os +import time +import subprocess + +from panda import Panda, PandaDFU + +board_path = os.path.dirname(os.path.realpath(__file__)) + +if __name__ == "__main__": + subprocess.check_call(f"scons -C {board_path}/.. -j$(nproc) {board_path}", shell=True) + + for s in Panda.list(): + print("putting", s, "in DFU mode") + with Panda(serial=s) as p: + p.reset(enter_bootstub=True) + p.reset(enter_bootloader=True) + + # wait for reset pandas to come back up + time.sleep(1) + + dfu_serials = PandaDFU.list() + print(f"found {len(dfu_serials)} panda(s) in DFU - {dfu_serials}") + for s in dfu_serials: + print("flashing", s) + PandaDFU(s).recover() diff --git a/panda/board/recover.sh b/panda/board/recover.sh deleted file mode 100755 index f003cba67..000000000 --- a/panda/board/recover.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env sh -set -e - -scons -u -j$(nproc) -# Recovers panda from DFU mode only, use flash.sh after power cycling panda -printf %b 'from python import Panda\nfor serial in Panda.list(): Panda(serial).reset(enter_bootstub=True); Panda(serial).reset(enter_bootloader=True)' | PYTHONPATH=.. python3 -sleep 1 -printf %b 'from python import PandaDFU\nfor serial in PandaDFU.list(): PandaDFU(serial).recover()' | PYTHONPATH=.. python3 diff --git a/panda/board/safety.h b/panda/board/safety.h index 474c3c663..4d7efd2a0 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -11,7 +11,7 @@ #include "safety/safety_hyundai.h" #include "safety/safety_chrysler.h" #include "safety/safety_subaru.h" -#include "safety/safety_subaru_legacy.h" +#include "safety/safety_subaru_preglobal.h" #include "safety/safety_mazda.h" #include "safety/safety_nissan.h" #include "safety/safety_volkswagen_mqb.h" @@ -44,7 +44,7 @@ #define SAFETY_NOOUTPUT 19U #define SAFETY_HONDA_BOSCH 20U #define SAFETY_VOLKSWAGEN_PQ 21U -#define SAFETY_SUBARU_LEGACY 22U +#define SAFETY_SUBARU_PREGLOBAL 22U #define SAFETY_HYUNDAI_LEGACY 23U #define SAFETY_HYUNDAI_COMMUNITY 24U #define SAFETY_STELLANTIS 25U @@ -77,8 +77,8 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len) { return current_hooks->tx_lin(lin_num, data, len); } -int safety_fwd_hook(int bus_num, CANPacket_t *to_fwd) { - return (relay_malfunction ? -1 : current_hooks->fwd(bus_num, to_fwd)); +int safety_fwd_hook(int bus_num, int addr) { + return (relay_malfunction ? -1 : current_hooks->fwd(bus_num, addr)); } bool get_longitudinal_allowed(void) { @@ -139,7 +139,7 @@ int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], cons for (int i = 0; i < len; i++) { // if multiple msgs are allowed, determine which one is present on the bus if (!addr_list[i].msg_seen) { - for (uint8_t j = 0U; addr_list[i].msg[j].addr != 0; j++) { + for (uint8_t j = 0U; (j < MAX_ADDR_CHECK_MSGS) && (addr_list[i].msg[j].addr != 0); j++) { if ((addr == addr_list[i].msg[j].addr) && (bus == addr_list[i].msg[j].bus) && (length == addr_list[i].msg[j].len)) { addr_list[i].index = j; @@ -149,11 +149,13 @@ int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], cons } } - int idx = addr_list[i].index; - if ((addr == addr_list[i].msg[idx].addr) && (bus == addr_list[i].msg[idx].bus) && - (length == addr_list[i].msg[idx].len)) { - index = i; - break; + if (addr_list[i].msg_seen) { + int idx = addr_list[i].index; + if ((addr == addr_list[i].msg[idx].addr) && (bus == addr_list[i].msg[idx].bus) && + (length == addr_list[i].msg[idx].len)) { + index = i; + break; + } } } return index; @@ -196,7 +198,7 @@ void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter) { bool is_msg_valid(AddrCheckStruct addr_list[], int index) { bool valid = true; if (index != -1) { - if ((!addr_list[index].valid_checksum) || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) { + if (!addr_list[index].valid_checksum || !addr_list[index].valid_quality_flag || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) { valid = false; controls_allowed = 0; } @@ -215,7 +217,8 @@ bool addr_safety_check(CANPacket_t *to_push, const addr_checks *rx_checks, uint32_t (*get_checksum)(CANPacket_t *to_push), uint32_t (*compute_checksum)(CANPacket_t *to_push), - uint8_t (*get_counter)(CANPacket_t *to_push)) { + uint8_t (*get_counter)(CANPacket_t *to_push), + bool (*get_quality_flag_valid)(CANPacket_t *to_push)) { int index = get_addr_check_index(to_push, rx_checks->check, rx_checks->len); update_addr_timestamp(rx_checks->check, index); @@ -237,6 +240,13 @@ bool addr_safety_check(CANPacket_t *to_push, } else { rx_checks->check[index].wrong_counters = 0U; } + + // quality flag check + if ((get_quality_flag_valid != NULL) && rx_checks->check[index].msg[rx_checks->check[index].index].quality_flag) { + rx_checks->check[index].valid_quality_flag = get_quality_flag_valid(to_push); + } else { + rx_checks->check[index].valid_quality_flag = true; + } } return is_msg_valid(rx_checks->check, index); } @@ -297,15 +307,15 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_HYUNDAI_LEGACY, &hyundai_legacy_hooks}, {SAFETY_MAZDA, &mazda_hooks}, {SAFETY_BODY, &body_hooks}, + {SAFETY_FORD, &ford_hooks}, #ifdef CANFD {SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks}, #endif #ifdef ALLOW_DEBUG {SAFETY_TESLA, &tesla_hooks}, - {SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks}, + {SAFETY_SUBARU_PREGLOBAL, &subaru_preglobal_hooks}, {SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, - {SAFETY_FORD, &ford_hooks}, #endif }; @@ -322,7 +332,6 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { regen_braking = false; regen_braking_prev = false; cruise_engaged_prev = false; - vehicle_speed = 0; vehicle_moving = false; acc_main_on = false; cruise_button_prev = 0; @@ -335,7 +344,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { valid_steer_req_count = 0; invalid_steer_req_count = 0; - torque_meas.max = 0; + vehicle_speed.min = 0; + vehicle_speed.max = 0; + torque_meas.min = 0; torque_meas.max = 0; torque_driver.min = 0; torque_driver.max = 0; @@ -401,7 +412,7 @@ bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) { return (val > MAX_VAL) || (val < MIN_VAL); } -// check that commanded value isn't too far from measured +// check that commanded torque value isn't too far from measured bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) { @@ -414,7 +425,7 @@ bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, MIN(val_meas->min, 0) - MAX_ERROR)); // check for violation - return (val < lowest_allowed) || (val > highest_allowed); + return max_limit_check(val, highest_allowed, lowest_allowed); } // check that commanded value isn't fighting against driver @@ -437,7 +448,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, MIN(driver_min_limit, 0))); // check for violation - return (val < lowest_allowed) || (val > highest_allowed); + return max_limit_check(val, highest_allowed, lowest_allowed); } @@ -449,7 +460,7 @@ bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) { int lowest_val = MIN(val_last, 0) - MAX_RT_DELTA; // check for violation - return (val < lowest_val) || (val > highest_val); + return max_limit_check(val, highest_val, lowest_val); } @@ -483,15 +494,15 @@ float interpolate(struct lookup_t xy, float x) { return ret; } +int ROUND(float val) { + return val + ((val > 0.0) ? 0.5 : -0.5); +} + // Safety checks for longitudinal actuation bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) { - bool violation = false; - if (!get_longitudinal_allowed()) { - violation |= desired_accel != limits.inactive_accel; - } else { - violation |= max_limit_check(desired_accel, limits.max_accel, limits.min_accel); - } - return violation; + bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel); + bool accel_inactive = desired_accel == limits.inactive_accel; + return !(accel_valid || accel_inactive); } bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits) { @@ -499,13 +510,9 @@ bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limit } bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) { - bool violation = false; - if (!get_longitudinal_allowed()) { - violation |= desired_gas != limits.inactive_gas; - } else { - violation |= max_limit_check(desired_gas, limits.max_gas, limits.min_gas); - } - return violation; + bool gas_valid = get_longitudinal_allowed() && !max_limit_check(desired_gas, limits.max_gas, limits.min_gas); + bool gas_inactive = desired_gas == limits.inactive_gas; + return !(gas_valid || gas_inactive); } bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits) { @@ -611,22 +618,45 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const if (controls_allowed && steer_control_enabled) { // convert floating point angle rate limits to integers in the scale of the desired angle on CAN, - // add 1 to not false trigger the violation - int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; - int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, vehicle_speed) * limits.angle_deg_to_can) + 1.; + // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are + // always slightly above openpilot's in case we read an updated speed in between angle commands + // TODO: this speed fudge can be much lower, look at data to determine the lowest reasonable offset + int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.; + int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.; + // allow down limits at zero since small floats will be rounded to 0 int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); + // check that commanded angle value isn't too far from measured, used to limit torque for some safety modes + // ensure we start moving in direction of meas while respecting rate limits if error is exceeded + if (limits.enforce_angle_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) { + // the rate limits above are liberally above openpilot's to avoid false positives. + // likewise, allow a lower rate for moving towards meas when error is exceeded + int delta_angle_up_lower = interpolate(limits.angle_rate_up_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can; + int delta_angle_down_lower = interpolate(limits.angle_rate_down_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can; + + int highest_desired_angle_lower = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up_lower : delta_angle_down_lower); + int lowest_desired_angle_lower = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down_lower : delta_angle_up_lower); + + lowest_desired_angle = MIN(MAX(lowest_desired_angle, angle_meas.min - limits.max_angle_error - 1), highest_desired_angle_lower); + highest_desired_angle = MAX(MIN(highest_desired_angle, angle_meas.max + limits.max_angle_error + 1), lowest_desired_angle_lower); + + // don't enforce above the max steer + lowest_desired_angle = CLAMP(lowest_desired_angle, -limits.max_steer, limits.max_steer); + highest_desired_angle = CLAMP(highest_desired_angle, -limits.max_steer, limits.max_steer); + } + // check for violation; violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); } desired_angle_last = desired_angle; - // Angle should be the same as current angle while not steering - violation |= (!controls_allowed && - ((desired_angle < (angle_meas.min - 1)) || - (desired_angle > (angle_meas.max + 1)))); + // Angle should either be 0 or same as current angle while not steering + if (!steer_control_enabled) { + violation |= (limits.inactive_angle_is_zero ? (desired_angle != 0) : + max_limit_check(desired_angle, angle_meas.max + 1, angle_meas.min - 1)); + } // No angle control allowed when controls are not allowed violation |= !controls_allowed && steer_control_enabled; diff --git a/panda/board/safety/safety_body.h b/panda/board/safety/safety_body.h index d9a9fcd33..61ebc46dc 100644 --- a/panda/board/safety/safety_body.h +++ b/panda/board/safety/safety_body.h @@ -9,7 +9,7 @@ addr_checks body_rx_checks = {body_addr_checks, BODY_ADDR_CHECK_LEN}; static int body_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &body_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &body_rx_checks, NULL, NULL, NULL, NULL); controls_allowed = valid; @@ -31,7 +31,7 @@ static int body_tx_hook(CANPacket_t *to_send) { } // Allow going into CAN flashing mode even if controls are not allowed - if (!controls_allowed && ((uint32_t)GET_BYTES_04(to_send) == 0xdeadfaceU) && ((uint32_t)GET_BYTES_48(to_send) == 0x0ab00b1eU)) { + if (!controls_allowed && (GET_BYTES(to_send, 0, 4) == 0xdeadfaceU) && (GET_BYTES(to_send, 4, 4) == 0x0ab00b1eU)) { tx = 1; } diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index 7c444bc0e..0caaa2c64 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -9,7 +9,7 @@ const SteeringLimits CHRYSLER_STEERING_LIMITS = { }; const SteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = { - .max_steer = 261, + .max_steer = 350, .max_rt_delta = 112, .max_rt_interval = 250000, .max_rate_up = 6, @@ -180,7 +180,7 @@ static int chrysler_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &chrysler_rx_checks, chrysler_get_checksum, chrysler_compute_checksum, - chrysler_get_counter); + chrysler_get_counter, NULL); const int bus = GET_BUS(to_push); const int addr = GET_ADDR(to_push); @@ -265,9 +265,8 @@ static int chrysler_tx_hook(CANPacket_t *to_send) { return tx; } -static int chrysler_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int chrysler_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); // forward to camera if (bus_num == 0) { diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h index 41c886295..90c8a0655 100644 --- a/panda/board/safety/safety_defaults.h +++ b/panda/board/safety/safety_defaults.h @@ -27,9 +27,9 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) { return false; } -static int default_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int default_fwd_hook(int bus_num, int addr) { UNUSED(bus_num); - UNUSED(to_fwd); + UNUSED(addr); return -1; } @@ -65,9 +65,9 @@ static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) { return true; } -static int alloutput_fwd_hook(int bus_num, CANPacket_t *to_fwd) { - UNUSED(to_fwd); +static int alloutput_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; + UNUSED(addr); if (alloutput_passthrough) { if (bus_num == 0) { diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 746c685d1..93c1e3a18 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -2,7 +2,11 @@ #define MSG_EngBrakeData 0x165 // RX from PCM, for driver brake pedal and cruise state #define MSG_EngVehicleSpThrottle 0x204 // RX from PCM, for driver throttle input #define MSG_DesiredTorqBrk 0x213 // RX from ABS, for standstill state +#define MSG_BrakeSysFeatures 0x415 // RX from ABS, for vehicle speed +#define MSG_EngVehicleSpThrottle2 0x202 // RX from PCM, for second vehicle speed +#define MSG_Yaw_Data_FD1 0x91 // RX from RCM, for yaw rate #define MSG_Steering_Data_FD1 0x083 // TX by OP, various driver switches and LKAS/CC buttons +#define MSG_ACCDATA 0x186 // TX by OP, ACC controls #define MSG_ACCDATA_3 0x18A // TX by OP, ACC/TJA user interface #define MSG_Lane_Assist_Data1 0x3CA // TX by OP, Lane Keep Assist #define MSG_LateralMotionControl 0x3D3 // TX by OP, Traffic Jam Assist @@ -12,7 +16,7 @@ #define FORD_MAIN_BUS 0U #define FORD_CAM_BUS 2U -const CanMsg FORD_TX_MSGS[] = { +const CanMsg FORD_STOCK_TX_MSGS[] = { {MSG_Steering_Data_FD1, 0, 8}, {MSG_Steering_Data_FD1, 2, 8}, {MSG_ACCDATA_3, 0, 8}, @@ -20,9 +24,27 @@ const CanMsg FORD_TX_MSGS[] = { {MSG_LateralMotionControl, 0, 8}, {MSG_IPMA_Data, 0, 8}, }; -#define FORD_TX_LEN (sizeof(FORD_TX_MSGS) / sizeof(FORD_TX_MSGS[0])) +#define FORD_STOCK_TX_LEN (sizeof(FORD_STOCK_TX_MSGS) / sizeof(FORD_STOCK_TX_MSGS[0])) +const CanMsg FORD_LONG_TX_MSGS[] = { + {MSG_Steering_Data_FD1, 0, 8}, + {MSG_Steering_Data_FD1, 2, 8}, + {MSG_ACCDATA, 0, 8}, + {MSG_ACCDATA_3, 0, 8}, + {MSG_Lane_Assist_Data1, 0, 8}, + {MSG_LateralMotionControl, 0, 8}, + {MSG_IPMA_Data, 0, 8}, +}; +#define FORD_LONG_TX_LEN (sizeof(FORD_LONG_TX_MSGS) / sizeof(FORD_LONG_TX_MSGS[0])) + +// warning: quality flags are not yet checked in openpilot's CAN parser, +// this may be the cause of blocked messages AddrCheckStruct ford_addr_checks[] = { + {.msg = {{MSG_BrakeSysFeatures, 0, 8, .check_checksum = true, .max_counter = 15U, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + // TODO: MSG_EngVehicleSpThrottle2 has a counter that skips by 2, understand and enable counter check + {.msg = {{MSG_EngVehicleSpThrottle2, 0, 8, .check_checksum = true, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{MSG_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + // These messages have no counter or checksum {.msg = {{MSG_EngBrakeData, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, {.msg = {{MSG_EngVehicleSpThrottle, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, {.msg = {{MSG_DesiredTorqBrk, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, @@ -30,10 +52,109 @@ AddrCheckStruct ford_addr_checks[] = { #define FORD_ADDR_CHECK_LEN (sizeof(ford_addr_checks) / sizeof(ford_addr_checks[0])) addr_checks ford_rx_checks = {ford_addr_checks, FORD_ADDR_CHECK_LEN}; +static uint8_t ford_get_counter(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t cnt; + if (addr == MSG_BrakeSysFeatures) { + // Signal: VehVActlBrk_No_Cnt + cnt = (GET_BYTE(to_push, 2) >> 2) & 0xFU; + } else if (addr == MSG_EngVehicleSpThrottle2) { + // Signal: VehVActlEng_No_Cnt + cnt = (GET_BYTE(to_push, 2) >> 3) & 0xFU; + } else if (addr == MSG_Yaw_Data_FD1) { + // Signal: VehRollYaw_No_Cnt + cnt = GET_BYTE(to_push, 5); + } else { + cnt = 0; + } + return cnt; +} + +static uint32_t ford_get_checksum(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum; + if (addr == MSG_BrakeSysFeatures) { + // Signal: VehVActlBrk_No_Cs + chksum = GET_BYTE(to_push, 3); + } else if (addr == MSG_EngVehicleSpThrottle2) { + // Signal: VehVActlEng_No_Cs + chksum = GET_BYTE(to_push, 1); + } else if (addr == MSG_Yaw_Data_FD1) { + // Signal: VehRollYawW_No_Cs + chksum = GET_BYTE(to_push, 4); + } else { + chksum = 0; + } + return chksum; +} + +static uint32_t ford_compute_checksum(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum = 0; + if (addr == MSG_BrakeSysFeatures) { + chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // Veh_V_ActlBrk + chksum += GET_BYTE(to_push, 2) >> 6; // VehVActlBrk_D_Qf + chksum += (GET_BYTE(to_push, 2) >> 2) & 0xFU; // VehVActlBrk_No_Cnt + chksum = 0xFFU - chksum; + } else if (addr == MSG_EngVehicleSpThrottle2) { + chksum += (GET_BYTE(to_push, 2) >> 3) & 0xFU; // VehVActlEng_No_Cnt + chksum += (GET_BYTE(to_push, 4) >> 5) & 0x3U; // VehVActlEng_D_Qf + chksum += GET_BYTE(to_push, 6) + GET_BYTE(to_push, 7); // Veh_V_ActlEng + chksum = 0xFFU - chksum; + } else if (addr == MSG_Yaw_Data_FD1) { + chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // VehRol_W_Actl + chksum += GET_BYTE(to_push, 2) + GET_BYTE(to_push, 3); // VehYaw_W_Actl + chksum += GET_BYTE(to_push, 5); // VehRollYaw_No_Cnt + chksum += GET_BYTE(to_push, 6) >> 6; // VehRolWActl_D_Qf + chksum += (GET_BYTE(to_push, 6) >> 4) & 0x3U; // VehYawWActl_D_Qf + chksum = 0xFFU - chksum; + } else { + } + + return chksum; +} + +static bool ford_get_quality_flag_valid(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + bool valid = false; + if (addr == MSG_BrakeSysFeatures) { + valid = (GET_BYTE(to_push, 2) >> 6) == 0x3U; // VehVActlBrk_D_Qf + } else if (addr == MSG_EngVehicleSpThrottle2) { + valid = ((GET_BYTE(to_push, 4) >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf + } else if (addr == MSG_Yaw_Data_FD1) { + valid = (GET_BYTE(to_push, 6) >> 4) == 0xFU; // VehRolWActl_D_Qf & VehYawWActl_D_Qf + } else { + } + return valid; +} + +const uint16_t FORD_PARAM_LONGITUDINAL = 1; + +bool ford_longitudinal = false; + +const LongitudinalLimits FORD_LONG_LIMITS = { + // acceleration cmd limits (used for brakes) + // Signal: AccBrkTot_A_Rq + .max_accel = 5641, // 1.9999 m/s^s + .min_accel = 4231, // -3.4991 m/s^2 + .inactive_accel = 5128, // -0.0008 m/s^2 + + // gas cmd limits + // Signal: AccPrpl_A_Rq + .max_gas = 700, // 2.0 m/s^2 + .min_gas = 450, // -0.5 m/s^2 + .inactive_gas = 0, // -5.0 m/s^2 +}; + #define INACTIVE_CURVATURE 1000U #define INACTIVE_CURVATURE_RATE 4096U #define INACTIVE_PATH_OFFSET 512U #define INACTIVE_PATH_ANGLE 1000U +#define FORD_MAX_SPEED_DELTA 2.0 // m/s static bool ford_lkas_msg_check(int addr) { return (addr == MSG_ACCDATA_3) @@ -42,8 +163,30 @@ static bool ford_lkas_msg_check(int addr) { || (addr == MSG_IPMA_Data); } +// Curvature rate limits +const SteeringLimits FORD_STEERING_LIMITS = { + .max_steer = 1000, + .angle_deg_to_can = 50000, // 1 / (2e-5) rad to can + .max_angle_error = 100, // 0.002 * FORD_STEERING_LIMITS.angle_deg_to_can + .angle_rate_up_lookup = { + {5., 25., 25.}, + {0.0002, 0.0001, 0.0001} + }, + .angle_rate_down_lookup = { + {5., 25., 25.}, + {0.000225, 0.00015, 0.00015} + }, + + // no blending at low speed due to lack of torque wind-up and inaccurate current curvature + .angle_error_min_speed = 10.0, // m/s + + .enforce_angle_error = true, + .inactive_angle_is_zero = true, +}; + static int ford_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &ford_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &ford_rx_checks, + ford_get_checksum, ford_compute_checksum, ford_get_counter, ford_get_quality_flag_valid); if (valid && (GET_BUS(to_push) == FORD_MAIN_BUS)) { int addr = GET_ADDR(to_push); @@ -54,6 +197,31 @@ static int ford_rx_hook(CANPacket_t *to_push) { vehicle_moving = ((GET_BYTE(to_push, 3) >> 3) & 0x3U) == 0U; } + // Update vehicle speed + if (addr == MSG_BrakeSysFeatures) { + // Signal: Veh_V_ActlBrk + update_sample(&vehicle_speed, ROUND(((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6 * VEHICLE_SPEED_FACTOR)); + } + + // Check vehicle speed against a second source + if (addr == MSG_EngVehicleSpThrottle2) { + // Disable controls if speeds from ABS and PCM ECUs are too far apart. + // Signal: Veh_V_ActlEng + float filtered_pcm_speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01 / 3.6; + if (ABS(filtered_pcm_speed - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > FORD_MAX_SPEED_DELTA) { + controls_allowed = 0; + } + } + + // Update vehicle yaw rate + if (addr == MSG_Yaw_Data_FD1) { + // Signal: VehYaw_W_Actl + float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5; + float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1); + // convert current curvature into units on CAN for comparison with desired curvature + update_sample(&angle_meas, ROUND(current_curvature * (float)FORD_STEERING_LIMITS.angle_deg_to_can)); + } + // Update gas pedal if (addr == MSG_EngVehicleSpThrottle) { // Pedal position: (0.1 * val) in percent @@ -81,12 +249,34 @@ static int ford_rx_hook(CANPacket_t *to_push) { } static int ford_tx_hook(CANPacket_t *to_send) { - int tx = 1; int addr = GET_ADDR(to_send); - if (!msg_allowed(to_send, FORD_TX_MSGS, FORD_TX_LEN)) { - tx = 0; + if (ford_longitudinal) { + tx = msg_allowed(to_send, FORD_LONG_TX_MSGS, FORD_LONG_TX_LEN); + } else { + tx = msg_allowed(to_send, FORD_STOCK_TX_MSGS, FORD_STOCK_TX_LEN); + } + + // Safety check for ACCDATA accel and brake requests + if (addr == MSG_ACCDATA) { + // Signal: AccPrpl_A_Rq + int gas = ((GET_BYTE(to_send, 6) & 0x3U) << 8) | GET_BYTE(to_send, 7); + // Signal: AccBrkTot_A_Rq + int accel = ((GET_BYTE(to_send, 0) & 0x1FU) << 8) | GET_BYTE(to_send, 1); + // Signal: CmbbDeny_B_Actl + int cmbb_deny = GET_BIT(to_send, 37U); + + bool violation = false; + violation |= longitudinal_accel_checks(accel, FORD_LONG_LIMITS); + violation |= longitudinal_gas_checks(gas, FORD_LONG_LIMITS); + + // Safety check for stock AEB + violation |= cmbb_deny != 0; // do not prevent stock AEB actuation + + if (violation) { + tx = 0; + } } // Safety check for Steering_Data_FD1 button signals @@ -119,20 +309,20 @@ static int ford_tx_hook(CANPacket_t *to_send) { // Safety check for LateralMotionControl action if (addr == MSG_LateralMotionControl) { // Signal: LatCtl_D_Rq - unsigned int steer_control_type = (GET_BYTE(to_send, 4) >> 2) & 0x7U; - unsigned int curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5); - unsigned int curvature_rate = ((GET_BYTE(to_send, 1) & 0x1FU) << 8) | GET_BYTE(to_send, 2); - unsigned int path_angle = (GET_BYTE(to_send, 3) << 3) | (GET_BYTE(to_send, 4) >> 5); - unsigned int path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6); + bool steer_control_enabled = ((GET_BYTE(to_send, 4) >> 2) & 0x7U) != 0U; + unsigned int raw_curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5); + unsigned int raw_curvature_rate = ((GET_BYTE(to_send, 1) & 0x1FU) << 8) | GET_BYTE(to_send, 2); + unsigned int raw_path_angle = (GET_BYTE(to_send, 3) << 3) | (GET_BYTE(to_send, 4) >> 5); + unsigned int raw_path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6); // These signals are not yet tested with the current safety limits - if ((curvature_rate != INACTIVE_CURVATURE_RATE) || (path_angle != INACTIVE_PATH_ANGLE) || (path_offset != INACTIVE_PATH_OFFSET)) { - tx = 0; - } + bool violation = (raw_curvature_rate != INACTIVE_CURVATURE_RATE) || (raw_path_angle != INACTIVE_PATH_ANGLE) || (raw_path_offset != INACTIVE_PATH_OFFSET); - // No steer control allowed when controls are not allowed - bool steer_control_enabled = (steer_control_type != 0U) || (curvature != INACTIVE_CURVATURE); - if (!controls_allowed && steer_control_enabled) { + // Check angle error and steer_control_enabled + int desired_curvature = raw_curvature - INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); + + if (violation) { tx = 0; } } @@ -141,8 +331,7 @@ static int ford_tx_hook(CANPacket_t *to_send) { return tx; } -static int ford_fwd_hook(int bus_num, CANPacket_t *to_fwd) { - int addr = GET_ADDR(to_fwd); +static int ford_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; switch (bus_num) { @@ -152,8 +341,14 @@ static int ford_fwd_hook(int bus_num, CANPacket_t *to_fwd) { break; } case FORD_CAM_BUS: { - // Block stock LKAS messages - if (!ford_lkas_msg_check(addr)) { + if (ford_lkas_msg_check(addr)) { + // Block stock LKAS and UI messages + bus_fwd = -1; + } else if (ford_longitudinal && (addr == MSG_ACCDATA)) { + // Block stock ACC message + bus_fwd = -1; + } else { + // Forward remaining traffic bus_fwd = FORD_MAIN_BUS; } break; @@ -170,7 +365,9 @@ static int ford_fwd_hook(int bus_num, CANPacket_t *to_fwd) { static const addr_checks* ford_init(uint16_t param) { UNUSED(param); - +#ifdef ALLOW_DEBUG + ford_longitudinal = GET_FLAG(param, FORD_PARAM_LONGITUDINAL); +#endif return &ford_rx_checks; } diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 3e7c2d3b9..76e55f0bc 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -1,8 +1,8 @@ const SteeringLimits GM_STEERING_LIMITS = { .max_steer = 300, .max_rate_up = 10, - .max_rate_down = 25, - .driver_torque_allowance = 50, + .max_rate_down = 15, + .driver_torque_allowance = 65, .driver_torque_factor = 4, .max_rt_delta = 128, .max_rt_interval = 250000, @@ -68,7 +68,7 @@ bool gm_pcm_cruise = false; static int gm_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &gm_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &gm_rx_checks, NULL, NULL, NULL, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); @@ -210,12 +210,11 @@ static int gm_tx_hook(CANPacket_t *to_send) { return tx; } -static int gm_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int gm_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; if (gm_hw == GM_CAM) { - int addr = GET_ADDR(to_fwd); if (bus_num == 0) { // block PSCMStatus; forwarded through openpilot to hide an alert from the camera bool is_pscm_msg = (addr == 388); diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index b8af7cac2..e8100cad7 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -8,8 +8,9 @@ // brake > 0mph const CanMsg HONDA_N_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x200, 0, 6}, {0x30C, 0, 8}, {0x33D, 0, 5}}; const CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}, {0x33DA, 0, 5}, {0x33DB, 0, 8}}; // Bosch -const CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5}, {0x296, 2, 4}, {0x33D, 0, 8}}; // Bosch radarless const CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1, 8}, {0x1FA, 1, 8}, {0x30C, 1, 8}, {0x33D, 1, 5}, {0x33DA, 1, 5}, {0x33DB, 1, 8}, {0x39F, 1, 8}, {0x18DAB0F1, 1, 8}}; // Bosch w/ gas and brakes +const CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5}, {0x296, 2, 4}, {0x33D, 0, 8}}; // Bosch radarless +const CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5}, {0x33D, 0, 8}, {0x1C8, 0, 8}, {0x30C, 0, 8}}; // Bosch radarless w/ gas and brakes // panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches // If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state @@ -22,7 +23,6 @@ const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = { .min_accel = -350, .max_gas = 2000, - .min_gas = -30000, .inactive_gas = -30000, }; @@ -38,7 +38,8 @@ AddrCheckStruct honda_common_addr_checks[] = { {.msg = {{0x1A6, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U}, // SCM_BUTTONS {0x296, 0, 4, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U}, { 0 }}}, {.msg = {{0x158, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // ENGINE_DATA - {.msg = {{0x17C, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // POWERTRAIN_DATA + {.msg = {{0x17C, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, // POWERTRAIN_DATA + {0x1BE, 0, 3, .check_checksum = true, .max_counter = 3U, .expected_timestep = 20000U}, { 0 }}}, // BRAKE_MODULE (for bosch radarless) {.msg = {{0x326, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // SCM_FEEDBACK }; #define HONDA_COMMON_ADDR_CHECKS_LEN (sizeof(honda_common_addr_checks) / sizeof(honda_common_addr_checks[0])) @@ -119,7 +120,7 @@ static uint8_t honda_get_counter(CANPacket_t *to_push) { static int honda_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &honda_rx_checks, - honda_get_checksum, honda_compute_checksum, honda_get_counter); + honda_get_checksum, honda_compute_checksum, honda_get_counter, NULL); if (valid) { const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \ @@ -266,8 +267,10 @@ static int honda_tx_hook(CANPacket_t *to_send) { int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); - if ((honda_hw == HONDA_BOSCH) && honda_bosch_radarless) { + if ((honda_hw == HONDA_BOSCH) && honda_bosch_radarless && !honda_bosch_long) { tx = msg_allowed(to_send, HONDA_RADARLESS_TX_MSGS, sizeof(HONDA_RADARLESS_TX_MSGS)/sizeof(HONDA_RADARLESS_TX_MSGS[0])); + } else if ((honda_hw == HONDA_BOSCH) && honda_bosch_radarless && honda_bosch_long) { + tx = msg_allowed(to_send, HONDA_RADARLESS_LONG_TX_MSGS, sizeof(HONDA_RADARLESS_LONG_TX_MSGS)/sizeof(HONDA_RADARLESS_LONG_TX_MSGS[0])); } else if ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) { tx = msg_allowed(to_send, HONDA_BOSCH_TX_MSGS, sizeof(HONDA_BOSCH_TX_MSGS)/sizeof(HONDA_BOSCH_TX_MSGS[0])); } else if ((honda_hw == HONDA_BOSCH) && honda_bosch_long) { @@ -319,6 +322,18 @@ static int honda_tx_hook(CANPacket_t *to_send) { } } + // ACCEL: safety check (radarless) + if ((addr == 0x1C8) && (bus == bus_pt)) { + int accel = (GET_BYTE(to_send, 0) << 4) | (GET_BYTE(to_send, 1) >> 4); + accel = to_signed(accel, 12); + + bool violation = false; + violation |= longitudinal_accel_checks(accel, HONDA_BOSCH_LONG_LIMITS); + if (violation) { + tx = 0; + } + } + // STEER: safety check if ((addr == 0xE4) || (addr == 0x194)) { if (!controls_allowed) { @@ -331,7 +346,7 @@ static int honda_tx_hook(CANPacket_t *to_send) { // Bosch supplemental control check if (addr == 0xE5) { - if ((GET_BYTES_04(to_send) != 0x10800004U) || ((GET_BYTES_48(to_send) & 0x00FFFFFFU) != 0x0U)) { + if ((GET_BYTES(to_send, 0, 4) != 0x10800004U) || ((GET_BYTES(to_send, 4, 4) & 0x00FFFFFFU) != 0x0U)) { tx = 0; } } @@ -354,7 +369,7 @@ static int honda_tx_hook(CANPacket_t *to_send) { // Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address if (addr == 0x18DAB0F1) { - if ((GET_BYTES_04(to_send) != 0x00803E02U) || (GET_BYTES_48(to_send) != 0x0U)) { + if ((GET_BYTES(to_send, 0, 4) != 0x00803E02U) || (GET_BYTES(to_send, 4, 4) != 0x0U)) { tx = 0; } } @@ -382,11 +397,11 @@ static const addr_checks* honda_bosch_init(uint16_t param) { honda_hw = HONDA_BOSCH; honda_bosch_radarless = GET_FLAG(param, HONDA_PARAM_RADARLESS); // Checking for alternate brake override from safety parameter - honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE) && !honda_bosch_radarless; + honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE); // radar disabled so allow gas/brakes #ifdef ALLOW_DEBUG - honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG) && !honda_bosch_radarless; + honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG); #endif if (honda_bosch_radarless) { @@ -397,7 +412,7 @@ static const addr_checks* honda_bosch_init(uint16_t param) { return &honda_rx_checks; } -static int honda_nidec_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int honda_nidec_fwd_hook(int bus_num, int addr) { // fwd from car to camera. also fwd certain msgs from camera to car // 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX, // 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud @@ -409,7 +424,6 @@ static int honda_nidec_fwd_hook(int bus_num, CANPacket_t *to_fwd) { if (bus_num == 2) { // block stock lkas messages and stock acc messages (if OP is doing ACC) - int addr = GET_ADDR(to_fwd); bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D); bool is_acc_hud_msg = addr == 0x30C; bool is_brake_msg = addr == 0x1FA; @@ -422,16 +436,17 @@ static int honda_nidec_fwd_hook(int bus_num, CANPacket_t *to_fwd) { return bus_fwd; } -static int honda_bosch_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int honda_bosch_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; if (bus_num == 0) { bus_fwd = 2; } if (bus_num == 2) { - int addr = GET_ADDR(to_fwd); int is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D) || (addr == 0x33DA) || (addr == 0x33DB); - if (!is_lkas_msg) { + int is_acc_msg = ((addr == 0x1C8) || (addr == 0x30C)) && honda_bosch_radarless && honda_bosch_long; + bool block_msg = is_lkas_msg || is_acc_msg; + if (!block_msg) { bus_fwd = 0; } } diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 6202c1d73..8ba252a1b 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -170,7 +170,7 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &hyundai_rx_checks, hyundai_get_checksum, hyundai_compute_checksum, - hyundai_get_counter); + hyundai_get_counter, NULL); int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -178,13 +178,13 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { // SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others if (valid && (addr == 1057) && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) { // 2 bits: 13-14 - int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3U; + int cruise_engaged = (GET_BYTES(to_push, 0, 4) >> 13) & 0x3U; hyundai_common_cruise_state_check(cruise_engaged); } if (valid && (bus == 0)) { if (addr == 593) { - int torque_driver_new = ((GET_BYTES_04(to_push) & 0x7ffU) * 0.79) - 808; // scale down new driver torque signal to match previous one + int torque_driver_new = ((GET_BYTES(to_push, 0, 4) & 0x7ffU) * 0.79) - 808; // scale down new driver torque signal to match previous one // update array of samples update_sample(&torque_driver, torque_driver_new); } @@ -208,7 +208,7 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { // sample wheel speed, averaging opposite corners if (addr == 902) { - uint32_t hyundai_speed = (GET_BYTES_04(to_push) & 0x3FFFU) + ((GET_BYTES_48(to_push) >> 16) & 0x3FFFU); // FL + RR + uint32_t hyundai_speed = (GET_BYTES(to_push, 0, 4) & 0x3FFFU) + ((GET_BYTES(to_push, 4, 4) >> 16) & 0x3FFFU); // FL + RR hyundai_speed /= 2; vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD; } @@ -275,7 +275,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) { // LKA STEER: safety check if (addr == 832) { - int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ffU) - 1024U; + int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x7ffU) - 1024U; bool steer_req = GET_BIT(to_send, 27U) != 0U; const SteeringLimits limits = hyundai_alt_limits ? HYUNDAI_STEERING_LIMITS_ALT : HYUNDAI_STEERING_LIMITS; @@ -286,7 +286,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) { // UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address if (addr == 2000) { - if ((GET_BYTES_04(to_send) != 0x00803E02U) || (GET_BYTES_48(to_send) != 0x0U)) { + if ((GET_BYTES(to_send, 0, 4) != 0x00803E02U) || (GET_BYTES(to_send, 4, 4) != 0x0U)) { tx = 0; } } @@ -305,10 +305,9 @@ static int hyundai_tx_hook(CANPacket_t *to_send) { return tx; } -static int hyundai_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int hyundai_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); // forward cam to ccan and viceversa, except lkas cmd if (bus_num == 0) { diff --git a/panda/board/safety/safety_hyundai_canfd.h b/panda/board/safety/safety_hyundai_canfd.h index 3e002941b..2dc6e9bbf 100644 --- a/panda/board/safety/safety_hyundai_canfd.h +++ b/panda/board/safety/safety_hyundai_canfd.h @@ -167,7 +167,7 @@ static uint32_t hyundai_canfd_compute_checksum(CANPacket_t *to_push) { static int hyundai_canfd_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &hyundai_canfd_rx_checks, - hyundai_canfd_get_checksum, hyundai_canfd_compute_checksum, hyundai_canfd_get_counter); + hyundai_canfd_get_checksum, hyundai_canfd_compute_checksum, hyundai_canfd_get_counter, NULL); int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -282,7 +282,7 @@ static int hyundai_canfd_tx_hook(CANPacket_t *to_send) { // UDS: only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address if ((addr == 0x730) && hyundai_canfd_hda2) { - if ((GET_BYTES_04(to_send) != 0x00803E02U) || (GET_BYTES_48(to_send) != 0x0U)) { + if ((GET_BYTES(to_send, 0, 4) != 0x00803E02U) || (GET_BYTES(to_send, 4, 4) != 0x0U)) { tx = 0; } } @@ -312,9 +312,8 @@ static int hyundai_canfd_tx_hook(CANPacket_t *to_send) { return tx; } -static int hyundai_canfd_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int hyundai_canfd_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); if (bus_num == 0) { bus_fwd = 2; diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index dae484ce3..e17f1ff4b 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -37,7 +37,7 @@ addr_checks mazda_rx_checks = {mazda_addr_checks, MAZDA_ADDR_CHECKS_LEN}; // track msgs coming from OP so that we know what CAM msgs to drop and what to forward static int mazda_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &mazda_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &mazda_rx_checks, NULL, NULL, NULL, NULL); if (valid && ((int)GET_BUS(to_push) == MAZDA_MAIN)) { int addr = GET_ADDR(to_push); @@ -108,9 +108,8 @@ static int mazda_tx_hook(CANPacket_t *to_send) { return tx; } -static int mazda_fwd_hook(int bus, CANPacket_t *to_fwd) { +static int mazda_fwd_hook(int bus, int addr) { int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); if (bus == MAZDA_MAIN) { bus_fwd = MAZDA_CAM; diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h index c977cf66f..1f44238e4 100644 --- a/panda/board/safety/safety_nissan.h +++ b/panda/board/safety/safety_nissan.h @@ -1,12 +1,12 @@ const SteeringLimits NISSAN_STEERING_LIMITS = { .angle_deg_to_can = 100, .angle_rate_up_lookup = { - {2., 7., 17.}, + {0., 5., 15.}, {5., .8, .15} }, .angle_rate_down_lookup = { - {2., 7., 17.}, - {5., 3.5, .5} + {0., 5., 15.}, + {5., 3.5, .4} }, }; @@ -42,7 +42,7 @@ bool nissan_alt_eps = false; static int nissan_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &nissan_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &nissan_rx_checks, NULL, NULL, NULL, NULL); if (valid) { int bus = GET_BUS(to_push); @@ -52,7 +52,7 @@ static int nissan_rx_hook(CANPacket_t *to_push) { if (addr == 0x2) { // Current steering angle // Factor -0.1, little endian - int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFFU); + int angle_meas_new = (GET_BYTES(to_push, 0, 4) & 0xFFFFU); // Need to multiply by 10 here as LKAS and Steering wheel are different base unit angle_meas_new = to_signed(angle_meas_new, 16) * 10; @@ -65,7 +65,7 @@ static int nissan_rx_hook(CANPacket_t *to_push) { uint16_t right_rear = (GET_BYTE(to_push, 0) << 8) | (GET_BYTE(to_push, 1)); uint16_t left_rear = (GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3)); vehicle_moving = (right_rear | left_rear) != 0U; - vehicle_speed = left_rear * 0.005 / 3.6; + update_sample(&vehicle_speed, ROUND((right_rear + left_rear) / 2.0 * 0.005 / 3.6 * VEHICLE_SPEED_FACTOR)); } // X-Trail 0x15c, Leaf 0x239 @@ -136,9 +136,8 @@ static int nissan_tx_hook(CANPacket_t *to_send) { } -static int nissan_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int nissan_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); if (bus_num == 0) { int block_msg = (addr == 0x280); // CANCEL_MSG diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index 3c782e5f5..4c83aed85 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -24,7 +24,8 @@ const CanMsg SUBARU_TX_MSGS[] = { {0x122, 0, 8}, {0x221, 0, 8}, {0x321, 0, 8}, - {0x322, 0, 8} + {0x322, 0, 8}, + {0x323, 0, 8}, }; #define SUBARU_TX_MSGS_LEN (sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0])) @@ -32,7 +33,8 @@ const CanMsg SUBARU_GEN2_TX_MSGS[] = { {0x122, 0, 8}, {0x221, 1, 8}, {0x321, 0, 8}, - {0x322, 0, 8} + {0x322, 0, 8}, + {0x323, 0, 8} }; #define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0])) @@ -82,7 +84,7 @@ static uint32_t subaru_compute_checksum(CANPacket_t *to_push) { static int subaru_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &subaru_rx_checks, - subaru_get_checksum, subaru_compute_checksum, subaru_get_counter); + subaru_get_checksum, subaru_compute_checksum, subaru_get_counter, NULL); if (valid) { const int bus = GET_BUS(to_push); @@ -91,7 +93,7 @@ static int subaru_rx_hook(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); if ((addr == 0x119) && (bus == 0)) { int torque_driver_new; - torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FFU); + torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 16) & 0x7FFU); torque_driver_new = -1 * to_signed(torque_driver_new, 11); update_sample(&torque_driver, torque_driver_new); } @@ -104,7 +106,7 @@ static int subaru_rx_hook(CANPacket_t *to_push) { // update vehicle moving with any non-zero wheel speed if ((addr == 0x13a) && (bus == alt_bus)) { - vehicle_moving = ((GET_BYTES_04(to_push) >> 12) != 0U) || (GET_BYTES_48(to_push) != 0U); + vehicle_moving = ((GET_BYTES(to_push, 0, 4) >> 12) != 0U) || (GET_BYTES(to_push, 4, 4) != 0U); } if ((addr == 0x13c) && (bus == alt_bus)) { @@ -133,7 +135,7 @@ static int subaru_tx_hook(CANPacket_t *to_send) { // steer cmd checks if (addr == 0x122) { - int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x1FFFU); + int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x1FFFU); desired_torque = -1 * to_signed(desired_torque, 13); const SteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS; @@ -145,7 +147,7 @@ static int subaru_tx_hook(CANPacket_t *to_send) { return tx; } -static int subaru_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int subaru_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; if (bus_num == 0) { @@ -157,8 +159,8 @@ static int subaru_fwd_hook(int bus_num, CANPacket_t *to_fwd) { // 0x122 ES_LKAS // 0x321 ES_DashStatus // 0x322 ES_LKAS_State - int addr = GET_ADDR(to_fwd); - bool block_lkas = (addr == 0x122) || (addr == 0x321) || (addr == 0x322); + // 0x323 INFOTAINMENT_STATUS + bool block_lkas = (addr == 0x122) || (addr == 0x321) || (addr == 0x322) || (addr == 0x323); if (!block_lkas) { bus_fwd = 0; // Main CAN } diff --git a/panda/board/safety/safety_subaru_legacy.h b/panda/board/safety/safety_subaru_preglobal.h similarity index 58% rename from panda/board/safety/safety_subaru_legacy.h rename to panda/board/safety/safety_subaru_preglobal.h index a0b6b120d..673e3655c 100644 --- a/panda/board/safety/safety_subaru_legacy.h +++ b/panda/board/safety/safety_subaru_preglobal.h @@ -1,4 +1,4 @@ -const SteeringLimits SUBARU_L_STEERING_LIMITS = { +const SteeringLimits SUBARU_PG_STEERING_LIMITS = { .max_steer = 2047, .max_rt_delta = 940, .max_rt_interval = 250000, @@ -9,24 +9,24 @@ const SteeringLimits SUBARU_L_STEERING_LIMITS = { .type = TorqueDriverLimited, }; -const CanMsg SUBARU_L_TX_MSGS[] = { +const CanMsg SUBARU_PG_TX_MSGS[] = { {0x161, 0, 8}, {0x164, 0, 8} }; -#define SUBARU_L_TX_MSGS_LEN (sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_MSGS[0])) +#define SUBARU_PG_TX_MSGS_LEN (sizeof(SUBARU_PG_TX_MSGS) / sizeof(SUBARU_PG_TX_MSGS[0])) // TODO: do checksum and counter checks after adding the signals to the outback dbc file -AddrCheckStruct subaru_l_addr_checks[] = { +AddrCheckStruct subaru_preglobal_addr_checks[] = { {.msg = {{0x140, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, {.msg = {{0x371, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, {.msg = {{0x144, 0, 8, .expected_timestep = 50000U}, { 0 }, { 0 }}}, }; -#define SUBARU_L_ADDR_CHECK_LEN (sizeof(subaru_l_addr_checks) / sizeof(subaru_l_addr_checks[0])) -addr_checks subaru_l_rx_checks = {subaru_l_addr_checks, SUBARU_L_ADDR_CHECK_LEN}; +#define SUBARU_PG_ADDR_CHECK_LEN (sizeof(subaru_preglobal_addr_checks) / sizeof(subaru_preglobal_addr_checks[0])) +addr_checks subaru_preglobal_rx_checks = {subaru_preglobal_addr_checks, SUBARU_PG_ADDR_CHECK_LEN}; -static int subaru_legacy_rx_hook(CANPacket_t *to_push) { +static int subaru_preglobal_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &subaru_l_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &subaru_preglobal_rx_checks, NULL, NULL, NULL, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); @@ -45,11 +45,11 @@ static int subaru_legacy_rx_hook(CANPacket_t *to_push) { // update vehicle moving with any non-zero wheel speed if (addr == 0xD4) { - vehicle_moving = ((GET_BYTES_04(to_push) >> 12) != 0U) || (GET_BYTES_48(to_push) != 0U); + vehicle_moving = ((GET_BYTES(to_push, 0, 4) >> 12) != 0U) || (GET_BYTES(to_push, 4, 4) != 0U); } if (addr == 0xD1) { - brake_pressed = ((GET_BYTES_04(to_push) >> 16) & 0xFFU) > 0U; + brake_pressed = ((GET_BYTES(to_push, 0, 4) >> 16) & 0xFFU) > 0U; } if (addr == 0x140) { @@ -61,21 +61,21 @@ static int subaru_legacy_rx_hook(CANPacket_t *to_push) { return valid; } -static int subaru_legacy_tx_hook(CANPacket_t *to_send) { +static int subaru_preglobal_tx_hook(CANPacket_t *to_send) { int tx = 1; int addr = GET_ADDR(to_send); - if (!msg_allowed(to_send, SUBARU_L_TX_MSGS, SUBARU_L_TX_MSGS_LEN)) { + if (!msg_allowed(to_send, SUBARU_PG_TX_MSGS, SUBARU_PG_TX_MSGS_LEN)) { tx = 0; } // steer cmd checks if (addr == 0x164) { - int desired_torque = ((GET_BYTES_04(to_send) >> 8) & 0x1FFFU); + int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 8) & 0x1FFFU); desired_torque = -1 * to_signed(desired_torque, 13); - if (steer_torque_cmd_checks(desired_torque, -1, SUBARU_L_STEERING_LIMITS)) { + if (steer_torque_cmd_checks(desired_torque, -1, SUBARU_PG_STEERING_LIMITS)) { tx = 0; } @@ -83,7 +83,7 @@ static int subaru_legacy_tx_hook(CANPacket_t *to_send) { return tx; } -static int subaru_legacy_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int subaru_preglobal_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; if (bus_num == 0) { @@ -94,7 +94,6 @@ static int subaru_legacy_fwd_hook(int bus_num, CANPacket_t *to_fwd) { // Preglobal platform // 0x161 is ES_CruiseThrottle // 0x164 is ES_LKAS - int addr = GET_ADDR(to_fwd); int block_msg = ((addr == 0x161) || (addr == 0x164)); if (!block_msg) { bus_fwd = 0; // Main CAN @@ -104,15 +103,15 @@ static int subaru_legacy_fwd_hook(int bus_num, CANPacket_t *to_fwd) { return bus_fwd; } -static const addr_checks* subaru_legacy_init(uint16_t param) { +static const addr_checks* subaru_preglobal_init(uint16_t param) { UNUSED(param); - return &subaru_l_rx_checks; + return &subaru_preglobal_rx_checks; } -const safety_hooks subaru_legacy_hooks = { - .init = subaru_legacy_init, - .rx = subaru_legacy_rx_hook, - .tx = subaru_legacy_tx_hook, +const safety_hooks subaru_preglobal_hooks = { + .init = subaru_preglobal_init, + .rx = subaru_preglobal_rx_hook, + .tx = subaru_preglobal_tx_hook, .tx_lin = nooutput_tx_lin_hook, - .fwd = subaru_legacy_fwd_hook, + .fwd = subaru_preglobal_fwd_hook, }; diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index a0a82f4bc..484cb8412 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -1,12 +1,12 @@ const SteeringLimits TESLA_STEERING_LIMITS = { .angle_deg_to_can = 10, .angle_rate_up_lookup = { - {2., 7., 17.}, - {5., .8, .25} + {0., 5., 15.}, + {5., .8, .15} }, .angle_rate_down_lookup = { - {2., 7., 17.}, - {5., 3.5, .8} + {0., 5., 15.}, + {5., 3.5, .4} }, }; @@ -34,6 +34,7 @@ const CanMsg TESLA_PT_TX_MSGS[] = { #define TESLA_PT_TX_LEN (sizeof(TESLA_PT_TX_MSGS) / sizeof(TESLA_PT_TX_MSGS[0])) AddrCheckStruct tesla_addr_checks[] = { + {.msg = {{0x2b9, 2, 8, .expected_timestep = 40000U}, { 0 }, { 0 }}}, // DAS_control (25Hz) {.msg = {{0x370, 0, 8, .expected_timestep = 40000U}, { 0 }, { 0 }}}, // EPAS_sysStatus (25Hz) {.msg = {{0x108, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque1 (100Hz) {.msg = {{0x118, 0, 6, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque2 (100Hz) @@ -48,6 +49,7 @@ AddrCheckStruct tesla_pt_addr_checks[] = { {.msg = {{0x106, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque1 (100Hz) {.msg = {{0x116, 0, 6, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque2 (100Hz) {.msg = {{0x1f8, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, // BrakeMessage (50Hz) + {.msg = {{0x2bf, 2, 8, .expected_timestep = 40000U}, { 0 }, { 0 }}}, // DAS_control (25Hz) {.msg = {{0x256, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // DI_state (10Hz) }; #define TESLA_PT_ADDR_CHECK_LEN (sizeof(tesla_pt_addr_checks) / sizeof(tesla_pt_addr_checks[0])) @@ -60,7 +62,7 @@ bool tesla_stock_aeb = false; static int tesla_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, tesla_powertrain ? (&tesla_pt_rx_checks) : (&tesla_rx_checks), - NULL, NULL, NULL); + NULL, NULL, NULL, NULL); if(valid) { int bus = GET_BUS(to_push); @@ -78,8 +80,9 @@ static int tesla_rx_hook(CANPacket_t *to_push) { if(addr == (tesla_powertrain ? 0x116 : 0x118)) { // Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS - vehicle_speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447; - vehicle_moving = ABS(vehicle_speed) > 0.1; + float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447; + vehicle_moving = ABS(speed) > 0.1; + update_sample(&vehicle_speed, ROUND(speed * VEHICLE_SPEED_FACTOR)); } if(addr == (tesla_powertrain ? 0x106 : 0x108)) { @@ -104,6 +107,14 @@ static int tesla_rx_hook(CANPacket_t *to_push) { } } + if (bus == 2) { + int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9); + if (tesla_longitudinal && (addr == das_control_addr)) { + // "AEB_ACTIVE" + tesla_stock_aeb = ((GET_BYTE(to_push, 2) & 0x03U) == 1U); + } + } + if (tesla_powertrain) { // 0x2bf: DAS_control should not be received on bus 0 generic_rx_checks((addr == 0x2bf) && (bus == 0)); @@ -182,9 +193,8 @@ static int tesla_tx_hook(CANPacket_t *to_send) { return tx; } -static int tesla_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int tesla_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); if(bus_num == 0) { // Chassis/PT to autopilot @@ -200,13 +210,8 @@ static int tesla_fwd_hook(int bus_num, CANPacket_t *to_fwd) { block_msg = true; } - if (tesla_longitudinal && (addr == das_control_addr)) { - // "AEB_ACTIVE" - tesla_stock_aeb = ((GET_BYTE(to_fwd, 2) & 0x03U) == 1U); - - if (!tesla_stock_aeb) { - block_msg = true; - } + if (tesla_longitudinal && (addr == das_control_addr) && !tesla_stock_aeb) { + block_msg = true; } if(!block_msg) { diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 5aef744a8..5ea6b6e10 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -48,9 +48,11 @@ const uint32_t TOYOTA_PARAM_OFFSET = 8U; const uint32_t TOYOTA_EPS_FACTOR = (1U << TOYOTA_PARAM_OFFSET) - 1U; const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1U << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2U << TOYOTA_PARAM_OFFSET; +const uint32_t TOYOTA_PARAM_LTA = 4U << TOYOTA_PARAM_OFFSET; bool toyota_alt_brake = false; bool toyota_stock_longitudinal = false; +bool toyota_lta = false; int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file static uint32_t toyota_compute_checksum(CANPacket_t *to_push) { @@ -71,7 +73,7 @@ static uint32_t toyota_get_checksum(CANPacket_t *to_push) { static int toyota_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &toyota_rx_checks, - toyota_get_checksum, toyota_compute_checksum, NULL); + toyota_get_checksum, toyota_compute_checksum, NULL, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); @@ -107,7 +109,7 @@ static int toyota_rx_hook(CANPacket_t *to_push) { if (addr == 0xaa) { // check that all wheel speeds are at zero value with offset - bool standstill = (GET_BYTES_04(to_push) == 0x6F1A6F1AU) && (GET_BYTES_48(to_push) == 0x6F1A6F1AU); + bool standstill = (GET_BYTES(to_push, 0, 4) == 0x6F1A6F1AU) && (GET_BYTES(to_push, 4, 4) == 0x6F1A6F1AU); vehicle_moving = !standstill; } @@ -179,7 +181,7 @@ static int toyota_tx_hook(CANPacket_t *to_send) { // AEB: block all actuation. only used when DSU is unplugged if (addr == 0x283) { // only allow the checksum, which is the last byte - bool block = (GET_BYTES_04(to_send) != 0U) || (GET_BYTE(to_send, 4) != 0U) || (GET_BYTE(to_send, 5) != 0U); + bool block = (GET_BYTES(to_send, 0, 4) != 0U) || (GET_BYTE(to_send, 4) != 0U) || (GET_BYTE(to_send, 5) != 0U); if (block) { tx = 0; } @@ -188,14 +190,15 @@ static int toyota_tx_hook(CANPacket_t *to_send) { // LTA steering check // only sent to prevent dash errors, no actuation is accepted if (addr == 0x191) { - // check the STEER_REQUEST, STEER_REQUEST_2, and STEER_ANGLE_CMD signals + // check the STEER_REQUEST, STEER_REQUEST_2, SETME_X64 STEER_ANGLE_CMD signals bool lta_request = (GET_BYTE(to_send, 0) & 1U) != 0U; bool lta_request2 = ((GET_BYTE(to_send, 3) >> 1) & 1U) != 0U; + int setme_x64 = GET_BYTE(to_send, 5); int lta_angle = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2); lta_angle = to_signed(lta_angle, 16); // block LTA msgs with actuation requests - if (lta_request || lta_request2 || (lta_angle != 0)) { + if (lta_request || lta_request2 || (lta_angle != 0) || (setme_x64 != 0)) { tx = 0; } } @@ -208,6 +211,10 @@ static int toyota_tx_hook(CANPacket_t *to_send) { if (steer_torque_cmd_checks(desired_torque, steer_req, TOYOTA_STEERING_LIMITS)) { tx = 0; } + // When using LTA (angle control), assert no actuation on LKA message + if (toyota_lta && ((desired_torque != 0) || steer_req)) { + tx = 0; + } } } @@ -219,10 +226,16 @@ static const addr_checks* toyota_init(uint16_t param) { toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR; + +#ifdef ALLOW_DEBUG + toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA); +#else + toyota_lta = false; +#endif return &toyota_rx_checks; } -static int toyota_fwd_hook(int bus_num, CANPacket_t *to_fwd) { +static int toyota_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; @@ -231,7 +244,6 @@ static int toyota_fwd_hook(int bus_num, CANPacket_t *to_fwd) { } if (bus_num == 2) { - int addr = GET_ADDR(to_fwd); // block stock lkas messages and stock acc messages (if OP is doing ACC) // in TSS2, 0x191 is LTA which we need to block to avoid controls collision int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191)); diff --git a/panda/board/safety/safety_volkswagen_mqb.h b/panda/board/safety/safety_volkswagen_mqb.h index 7c22e9f0f..4cd9c2ec3 100644 --- a/panda/board/safety/safety_volkswagen_mqb.h +++ b/panda/board/safety/safety_volkswagen_mqb.h @@ -115,7 +115,7 @@ static const addr_checks* volkswagen_mqb_init(uint16_t param) { static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &volkswagen_mqb_rx_checks, - volkswagen_mqb_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter); + volkswagen_mqb_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); @@ -183,7 +183,7 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) { // Signal: Motor_20.MO_Fahrpedalrohwert_01 if (addr == MSG_MOTOR_20) { - gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFFU) != 0U; + gas_pressed = ((GET_BYTES(to_push, 0, 4) >> 12) & 0xFFU) != 0U; } // Signal: Motor_14.MO_Fahrer_bremst (ECU detected brake pedal switch F63) @@ -214,12 +214,12 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send) { } // Safety check for HCA_01 Heading Control Assist torque - // Signal: HCA_01.Assist_Torque (absolute torque) - // Signal: HCA_01.Assist_VZ (direction) + // Signal: HCA_01.HCA_01_LM_Offset (absolute torque) + // Signal: HCA_01.HCA_01_LM_OffSign (direction) if (addr == MSG_HCA_01) { - int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3FU) << 8); - int sign = (GET_BYTE(to_send, 3) & 0x80U) >> 7; - if (sign == 1) { + int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x1U) << 8); + bool sign = GET_BIT(to_send, 31U); + if (sign) { desired_torque *= -1; } @@ -265,8 +265,7 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send) { return tx; } -static int volkswagen_mqb_fwd_hook(int bus_num, CANPacket_t *to_fwd) { - int addr = GET_ADDR(to_fwd); +static int volkswagen_mqb_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; switch (bus_num) { diff --git a/panda/board/safety/safety_volkswagen_pq.h b/panda/board/safety/safety_volkswagen_pq.h index 20b9da63d..7a909d4d5 100644 --- a/panda/board/safety/safety_volkswagen_pq.h +++ b/panda/board/safety/safety_volkswagen_pq.h @@ -28,14 +28,14 @@ const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = { #define MSG_MOTOR_3 0x380 // RX from ECU, for driver throttle input #define MSG_GRA_NEU 0x38A // TX by OP, ACC control buttons for cancel/resume #define MSG_MOTOR_5 0x480 // RX from ECU, for ACC main switch state -#define MSG_ACC_GRA_ANZIEGE 0x56A // TX by OP, ACC HUD +#define MSG_ACC_GRA_ANZEIGE 0x56A // TX by OP, ACC HUD #define MSG_LDW_1 0x5BE // TX by OP, Lane line recognition and text alerts // Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8}, {MSG_GRA_NEU, 0, 4}, {MSG_GRA_NEU, 2, 4}}; const CanMsg VOLKSWAGEN_PQ_LONG_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8}, - {MSG_ACC_SYSTEM, 0, 8}, {MSG_ACC_GRA_ANZIEGE, 0, 8}}; + {MSG_ACC_SYSTEM, 0, 8}, {MSG_ACC_GRA_ANZEIGE, 0, 8}}; AddrCheckStruct volkswagen_pq_addr_checks[] = { {.msg = {{MSG_LENKHILFE_3, 0, 6, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, @@ -100,7 +100,7 @@ static const addr_checks* volkswagen_pq_init(uint16_t param) { static int volkswagen_pq_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &volkswagen_pq_rx_checks, - volkswagen_pq_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter); + volkswagen_pq_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); @@ -227,8 +227,7 @@ static int volkswagen_pq_tx_hook(CANPacket_t *to_send) { return tx; } -static int volkswagen_pq_fwd_hook(int bus_num, CANPacket_t *to_fwd) { - int addr = GET_ADDR(to_fwd); +static int volkswagen_pq_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; switch (bus_num) { @@ -240,7 +239,7 @@ static int volkswagen_pq_fwd_hook(int bus_num, CANPacket_t *to_fwd) { if ((addr == MSG_HCA_1) || (addr == MSG_LDW_1)) { // openpilot takes over LKAS steering control and related HUD messages from the camera bus_fwd = -1; - } else if (volkswagen_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZIEGE))) { + } else if (volkswagen_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZEIGE))) { // openpilot takes over acceleration/braking control and related HUD messages from the stock ACC radar } else { // Forward all remaining traffic from Extended CAN devices to J533 gateway diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 7bc117515..393e55467 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -2,12 +2,22 @@ #define GET_BIT(msg, b) (((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U) #define GET_BYTE(msg, b) ((msg)->data[(b)]) -#define GET_BYTES_04(msg) ((msg)->data[0] | ((msg)->data[1] << 8U) | ((msg)->data[2] << 16U) | ((msg)->data[3] << 24U)) -#define GET_BYTES_48(msg) ((msg)->data[4] | ((msg)->data[5] << 8U) | ((msg)->data[6] << 16U) | ((msg)->data[7] << 24U)) #define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask)) +uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) { + uint32_t ret = 0U; + for (int i = 0; i < len; i++) { + const uint8_t shift = i * 8; + ret |= (((uint32_t)msg->data[start + i]) << shift); + } + return ret; +} + const int MAX_WRONG_COUNTERS = 5; const uint8_t MAX_MISSED_MSGS = 10U; +#define MAX_ADDR_CHECK_MSGS 3U +// used to represent floating point vehicle speed in a sample_t +#define VEHICLE_SPEED_FACTOR 100.0 // sample struct that keeps 6 samples in memory struct sample_t { @@ -60,6 +70,11 @@ typedef struct { const int angle_deg_to_can; const struct lookup_t angle_rate_up_lookup; const struct lookup_t angle_rate_down_lookup; + const int max_angle_error; // used to limit error between meas and cmd while enabled + const float angle_error_min_speed; // minimum speed to start limiting angle error + + const bool enforce_angle_error; // enables max_angle_error check + const bool inactive_angle_is_zero; // if false, enforces angle near meas when disabled (default) } SteeringLimits; typedef struct { @@ -85,18 +100,20 @@ typedef struct { const int len; const bool check_checksum; // true is checksum check is performed const uint8_t max_counter; // maximum value of the counter. 0 means that the counter check is skipped + const bool quality_flag; // true is quality flag check is performed const uint32_t expected_timestep; // expected time between message updates [us] } CanMsgCheck; // params and flags about checksum, counter and frequency checks for each monitored address typedef struct { // const params - const CanMsgCheck msg[3]; // check either messages (e.g. honda steer). Array MUST terminate with an empty struct to know its length. + const CanMsgCheck msg[MAX_ADDR_CHECK_MSGS]; // check either messages (e.g. honda steer) // dynamic flags bool msg_seen; int index; // if multiple messages are allowed to be checked, this stores the index of the first one seen. only msg[msg_index] will be used bool valid_checksum; // true if and only if checksum check is passed int wrong_counters; // counter of wrong counters, saturated between 0 and MAX_WRONG_COUNTERS + bool valid_quality_flag; // true if the message's quality/health/status signals are valid uint8_t last_counter; // last counter value uint32_t last_timestamp; // micro-s bool lagging; // true if and only if the time between updates is excessive @@ -114,6 +131,8 @@ uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last); int to_signed(int d, int bits); void update_sample(struct sample_t *sample, int sample_new); bool max_limit_check(int val, const int MAX, const int MIN); +bool angle_dist_to_meas_check(int val, struct sample_t *val_meas, + const int MAX_ERROR, const int MAX_VAL); bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR); bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, @@ -122,6 +141,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, bool get_longitudinal_allowed(void); bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); float interpolate(struct lookup_t xy, float x); +int ROUND(float val); void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]); void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]); bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len); @@ -133,7 +153,8 @@ bool addr_safety_check(CANPacket_t *to_push, const addr_checks *rx_checks, uint32_t (*get_checksum)(CANPacket_t *to_push), uint32_t (*compute_checksum)(CANPacket_t *to_push), - uint8_t (*get_counter)(CANPacket_t *to_push)); + uint8_t (*get_counter)(CANPacket_t *to_push), + bool (*get_quality_flag_valid)(CANPacket_t *to_push)); void generic_rx_checks(bool stock_ecu_detected); void relay_malfunction_set(void); void relay_malfunction_reset(void); @@ -150,7 +171,7 @@ typedef const addr_checks* (*safety_hook_init)(uint16_t param); typedef int (*rx_hook)(CANPacket_t *to_push); typedef int (*tx_hook)(CANPacket_t *to_send); typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len); -typedef int (*fwd_hook)(int bus_num, CANPacket_t *to_fwd); +typedef int (*fwd_hook)(int bus_num, int addr); typedef struct { safety_hook_init init; @@ -174,7 +195,7 @@ bool brake_pressed_prev = false; bool regen_braking = false; bool regen_braking_prev = false; bool cruise_engaged_prev = false; -float vehicle_speed = 0; +struct sample_t vehicle_speed; bool vehicle_moving = false; bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 int cruise_button_prev = 0; @@ -197,7 +218,7 @@ uint32_t heartbeat_engaged_mismatches = 0; // count of mismatches between heart // for safety modes with angle steering control uint32_t ts_angle_last = 0; int desired_angle_last = 0; -struct sample_t angle_meas; // last 6 steer angles +struct sample_t angle_meas; // last 6 steer angles/curvatures // This can be set with a USB command // It enables features that allow alternative experiences, like not disengaging on gas press diff --git a/panda/board/stm32fx/board.h b/panda/board/stm32fx/board.h index 878c1be9e..76593a529 100644 --- a/panda/board/stm32fx/board.h +++ b/panda/board/stm32fx/board.h @@ -5,6 +5,7 @@ #include "boards/unused_funcs.h" // ///// Board definition and detection ///// // +#include "stm32fx/lladc.h" #include "drivers/harness.h" #ifdef PANDA #include "drivers/fan.h" diff --git a/panda/board/stm32fx/clock.h b/panda/board/stm32fx/clock.h index fb918d312..19be57443 100644 --- a/panda/board/stm32fx/clock.h +++ b/panda/board/stm32fx/clock.h @@ -32,17 +32,3 @@ void clock_init(void) { // *** running on PLL *** } - -void watchdog_init(void) { - // setup watchdog - IWDG->KR = 0x5555U; - register_set(&(IWDG->PR), 0x0U, 0x7U); // divider/4 - - // 0 = 0.125 ms, let's have a 50ms watchdog - register_set(&(IWDG->RLR), (400U-1U), 0xFFFU); - IWDG->KR = 0xCCCCU; -} - -void watchdog_feed(void) { - IWDG->KR = 0xAAAAU; -} diff --git a/panda/board/stm32fx/inc/stm32f205xx.h b/panda/board/stm32fx/inc/stm32f205xx.h index 622faead6..368bcf39e 100644 --- a/panda/board/stm32fx/inc/stm32f205xx.h +++ b/panda/board/stm32fx/inc/stm32f205xx.h @@ -8,7 +8,7 @@ * This file contains : * - Data structures and the address mapping for all peripherals * - Peripherals registers declarations and bits definition - * - Macros to access peripherals registers hardware + * - Macros to access peripheral's registers hardware * ****************************************************************************** * @attention @@ -6728,6 +6728,8 @@ USB_OTG_HostChannelTypeDef; #define USB_OTG_DSTS_ENUMSPD_0 0x00000002U /*!SMPR1), ADC_SMPR1_SMP12 | ADC_SMPR1_SMP13, 0x7FFFFFFU); } -uint32_t adc_get(unsigned int channel) { +uint16_t adc_get_raw(uint8_t channel) { // Select channel - register_set(&(ADC1->JSQR), (channel << 15U), 0x3FFFFFU); + register_set(&(ADC1->JSQR), ((uint32_t) channel << 15U), 0x3FFFFFU); // Start conversion ADC1->SR &= ~(ADC_SR_JEOC); @@ -28,13 +28,6 @@ uint32_t adc_get(unsigned int channel) { return ADC1->JDR1; } -uint32_t adc_get_voltage(void) { - // REVC has a 10, 1 (1/11) voltage divider - // Here is the calculation for the scale (s) - // ADCV = VIN_S * (1/11) * (4095/3.3) - // RETVAL = ADCV * s = VIN_S*1000 - // s = 1000/((4095/3.3)*(1/11)) = 8.8623046875 - - // Avoid needing floating point math, so output in mV - return (adc_get(ADCCHAN_VOLTAGE) * 8862U) / 1000U; +uint16_t adc_get_mV(uint8_t channel) { + return (adc_get_raw(channel) * current_board->avdd_mV) / 4095U; } diff --git a/panda/board/stm32fx/llexti.h b/panda/board/stm32fx/llexti.h index d4362cd18..6de13ab7f 100644 --- a/panda/board/stm32fx/llexti.h +++ b/panda/board/stm32fx/llexti.h @@ -2,7 +2,7 @@ void EXTI_IRQ_Handler(void); void exti_irq_init(void) { SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI8_Msk); - if (car_harness_status == HARNESS_STATUS_FLIPPED) { + if (harness.status == HARNESS_STATUS_FLIPPED) { // CAN2_RX current_board->enable_can_transceiver(3U, false); SYSCFG->EXTICR[2] |= (SYSCFG_EXTICR3_EXTI8_PA); diff --git a/panda/board/stm32fx/llfan.h b/panda/board/stm32fx/llfan.h index 3e4b0fa3f..9e3f0c654 100644 --- a/panda/board/stm32fx/llfan.h +++ b/panda/board/stm32fx/llfan.h @@ -2,7 +2,7 @@ void EXTI2_IRQ_Handler(void) { volatile unsigned int pr = EXTI->PR & (1U << 2); if ((pr & (1U << 2)) != 0U) { - fan_state.tach_counter++; + fan_state.tach_counter++; } EXTI->PR = (1U << 2); } diff --git a/panda/board/stm32fx/llrtc.h b/panda/board/stm32fx/llrtc.h index b286ce72b..a9b61917e 100644 --- a/panda/board/stm32fx/llrtc.h +++ b/panda/board/stm32fx/llrtc.h @@ -1,6 +1,3 @@ -#define RCC_BDCR_MASK_LSE (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL | RCC_BDCR_LSEMOD | RCC_BDCR_LSEBYP | RCC_BDCR_LSEON) -#define RCC_BDCR_MASK_LSI (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL) - void enable_bdomain_protection(void) { register_clear_bits(&(PWR->CR), PWR_CR_DBP); } @@ -9,6 +6,38 @@ void disable_bdomain_protection(void) { register_set_bits(&(PWR->CR), PWR_CR_DBP); } +void rtc_init(void){ + uint32_t bdcr_opts = RCC_BDCR_RTCEN; + uint32_t bdcr_mask = (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL); + if (current_board->has_rtc_battery) { + bdcr_opts |= (RCC_BDCR_RTCSEL_0 | RCC_BDCR_LSEON); + bdcr_mask |= (RCC_BDCR_LSEMOD | RCC_BDCR_LSEBYP | RCC_BDCR_LSEON); + } else { + bdcr_opts |= RCC_BDCR_RTCSEL_1; + RCC->CSR |= RCC_CSR_LSION; + while((RCC->CSR & RCC_CSR_LSIRDY) == 0){} + } + + // Initialize RTC module and clock if not done already. + if((RCC->BDCR & bdcr_mask) != bdcr_opts){ + print("Initializing RTC\n"); + // Reset backup domain + register_set_bits(&(RCC->BDCR), RCC_BDCR_BDRST); + + // Disable write protection + disable_bdomain_protection(); + + // Clear backup domain reset + register_clear_bits(&(RCC->BDCR), RCC_BDCR_BDRST); + + // Set RTC options + register_set(&(RCC->BDCR), bdcr_opts, bdcr_mask); + + // Enable write protection + enable_bdomain_protection(); + } +} + void rtc_wakeup_init(void) { EXTI->IMR |= EXTI_IMR_MR22; EXTI->RTSR |= EXTI_RTSR_TR22; // rising edge diff --git a/panda/board/stm32fx/llspi.h b/panda/board/stm32fx/llspi.h index 3aa7236bd..31e419b6f 100644 --- a/panda/board/stm32fx/llspi.h +++ b/panda/board/stm32fx/llspi.h @@ -36,7 +36,7 @@ void DMA2_Stream2_IRQ_Handler(void) { ENTER_CRITICAL(); DMA2->LIFCR = DMA_LIFCR_CTCIF2; - spi_handle_rx(); + spi_rx_done(); EXIT_CRITICAL(); } @@ -60,14 +60,17 @@ void DMA2_Stream3_IRQ_Handler(void) { (void)dat; SPI1->DR = 0U; - spi_handle_tx(timed_out); + if (timed_out) { + print("SPI: TX timeout\n"); + } + + spi_tx_done(timed_out); } // ***************************** SPI init ***************************** void llspi_init(void) { - // We expect less than 50 transactions (including control messages and CAN buffers) at the 100Hz boardd interval. Can be raised if needed. - REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, 5000U, FAULT_INTERRUPT_RATE_SPI_DMA) - REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, 5000U, FAULT_INTERRUPT_RATE_SPI_DMA) + REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) + REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) // Setup MOSI DMA register_set(&(DMA2_Stream2->CR), (DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_TCIE), 0x1E077EFEU); diff --git a/panda/board/stm32fx/peripherals.h b/panda/board/stm32fx/peripherals.h index 40ef6cb57..3eeebf0eb 100644 --- a/panda/board/stm32fx/peripherals.h +++ b/panda/board/stm32fx/peripherals.h @@ -5,6 +5,15 @@ void gpio_usb_init(void) { GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12; } +void gpio_spi_init(void) { + // A4-A7: SPI + set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1); + set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1); + set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1); + set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1); + register_set_bits(&(GPIOA->OSPEEDR), GPIO_OSPEEDER_OSPEEDR4 | GPIO_OSPEEDER_OSPEEDR5 | GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7); +} + void gpio_usart2_init(void) { // A2,A3: USART 2 for debugging set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2); diff --git a/panda/board/stm32fx/stm32fx_config.h b/panda/board/stm32fx/stm32fx_config.h index 066160693..a73239749 100644 --- a/panda/board/stm32fx/stm32fx_config.h +++ b/panda/board/stm32fx/stm32fx_config.h @@ -23,9 +23,6 @@ #define MAX_LED_FADE 8192U -// Threshold voltage (mV) for either of the SBUs to be below before deciding harness is connected -#define HARNESS_CONNECTED_THRESHOLD 2500U - #define NUM_INTERRUPTS 102U // There are 102 external interrupt sources (see stm32f413.h) #define TICK_TIMER_IRQ TIM1_BRK_TIM9_IRQn @@ -36,6 +33,8 @@ #define INTERRUPT_TIMER_IRQ TIM6_DAC_IRQn #define INTERRUPT_TIMER TIM6 +#define IND_WDG IWDG + #define PROVISION_CHUNK_ADDRESS 0x1FFF79E0U #define DEVICE_SERIAL_NUMBER_ADDRESS 0x1FFF79C0U @@ -63,11 +62,11 @@ #include "stm32fx/peripherals.h" #include "stm32fx/interrupt_handlers.h" #include "drivers/timers.h" -#include "stm32fx/lladc.h" #include "stm32fx/board.h" #include "stm32fx/clock.h" +#include "drivers/watchdog.h" -#ifdef ENABLE_SPI +#if defined(PANDA) || defined(BOOTSTUB) #include "drivers/spi.h" #include "stm32fx/llspi.h" #endif diff --git a/panda/board/stm32h7/board.h b/panda/board/stm32h7/board.h index 086db86b3..d991e433b 100644 --- a/panda/board/stm32h7/board.h +++ b/panda/board/stm32h7/board.h @@ -5,6 +5,7 @@ #include "boards/unused_funcs.h" // ///// Board definition and detection ///// // +#include "stm32h7/lladc.h" #include "drivers/harness.h" #include "drivers/fan.h" #include "stm32h7/llfan.h" diff --git a/panda/board/stm32h7/interrupt_handlers.h b/panda/board/stm32h7/interrupt_handlers.h index 0811ffe52..fb1298d77 100644 --- a/panda/board/stm32h7/interrupt_handlers.h +++ b/panda/board/stm32h7/interrupt_handlers.h @@ -46,6 +46,7 @@ void TIM8_CC_IRQHandler(void) {handle_interrupt(TIM8_CC_IRQn);} void DMA1_Stream7_IRQHandler(void) {handle_interrupt(DMA1_Stream7_IRQn);} void TIM5_IRQHandler(void) {handle_interrupt(TIM5_IRQn);} void SPI3_IRQHandler(void) {handle_interrupt(SPI3_IRQn);} +void SPI4_IRQHandler(void) {handle_interrupt(SPI4_IRQn);} void UART4_IRQHandler(void) {handle_interrupt(UART4_IRQn);} void UART5_IRQHandler(void) {handle_interrupt(UART5_IRQn);} void TIM6_DAC_IRQHandler(void) {handle_interrupt(TIM6_DAC_IRQn);} diff --git a/panda/board/stm32h7/lladc.h b/panda/board/stm32h7/lladc.h index 795ca3533..29d5e19d6 100644 --- a/panda/board/stm32h7/lladc.h +++ b/panda/board/stm32h7/lladc.h @@ -1,6 +1,6 @@ // 5VOUT_S = ADC12_INP5 // VOLT_S = ADC1_INP2 -#define ADCCHAN_VOLTAGE 2 +#define ADCCHAN_VIN 2 void adc_init(void) { ADC1->CR &= ~(ADC_CR_DEEPPWD); //Reset deep-power-down mode @@ -17,11 +17,11 @@ void adc_init(void) { while(!(ADC1->ISR & ADC_ISR_ADRDY)); } -uint32_t adc_get(unsigned int channel) { +uint16_t adc_get_raw(uint8_t channel) { ADC1->SQR1 &= ~(ADC_SQR1_L); - ADC1->SQR1 = (channel << 6U); - + ADC1->SQR1 = ((uint32_t) channel << 6U); + ADC1->SMPR1 = (0x7U << (channel * 3U) ); ADC1->PCSEL_RES0 = (0x1U << channel); @@ -36,13 +36,6 @@ uint32_t adc_get(unsigned int channel) { return res; } -uint32_t adc_get_voltage(void) { - // REVC has a 10, 1 (1/11) voltage divider - // Here is the calculation for the scale (s) - // ADCV = VIN_S * (1/11) * (65535/3.3) - // RETVAL = ADCV * s = VIN_S*1000 - // s = 1000/((65535/3.3)*(1/11)) = 0.553902494 - - // Avoid needing floating point math, so output in mV - return (adc_get(ADCCHAN_VOLTAGE) * 5539U) / 10000U; +uint16_t adc_get_mV(uint8_t channel) { + return (adc_get_raw(channel) * current_board->avdd_mV) / 65535U; } diff --git a/panda/board/stm32h7/llexti.h b/panda/board/stm32h7/llexti.h index 7b7c260d8..46d0acab6 100644 --- a/panda/board/stm32h7/llexti.h +++ b/panda/board/stm32h7/llexti.h @@ -1,7 +1,7 @@ void EXTI_IRQ_Handler(void); void exti_irq_init(void) { - if (car_harness_status == HARNESS_STATUS_FLIPPED) { + if (harness.status == HARNESS_STATUS_FLIPPED) { // CAN2_RX IRQ on falling edge (EXTI10) current_board->enable_can_transceiver(3U, false); SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI10_Msk); diff --git a/panda/board/stm32h7/llfdcan.h b/panda/board/stm32h7/llfdcan.h index b2bb885b4..e5791fc52 100644 --- a/panda/board/stm32h7/llfdcan.h +++ b/panda/board/stm32h7/llfdcan.h @@ -13,12 +13,14 @@ // FDCAN core settings #define FDCAN_MESSAGE_RAM_SIZE 0x2800UL #define FDCAN_START_ADDRESS 0x4000AC00UL -#define FDCAN_OFFSET 3412UL // bytes for each FDCAN module -#define FDCAN_OFFSET_W 853UL // words for each FDCAN module -#define FDCAN_END_ADDRESS 0x4000D3FCUL // Message RAM has a width of 4 Bytes +#define FDCAN_OFFSET 3384UL // bytes for each FDCAN module, equally +#define FDCAN_OFFSET_W 846UL // words for each FDCAN module, equally +#define FDCAN_END_ADDRESS 0x4000D3FCUL // Message RAM has a width of 4 bytes + +// FDCAN_RX_FIFO_0_EL_CNT + FDCAN_TX_FIFO_EL_CNT can't exceed 47 elements (47 * 72 bytes = 3,384 bytes) per FDCAN module // RX FIFO 0 -#define FDCAN_RX_FIFO_0_EL_CNT 24UL +#define FDCAN_RX_FIFO_0_EL_CNT 30UL #define FDCAN_RX_FIFO_0_HEAD_SIZE 8UL // bytes #define FDCAN_RX_FIFO_0_DATA_SIZE 64UL // bytes #define FDCAN_RX_FIFO_0_EL_SIZE (FDCAN_RX_FIFO_0_HEAD_SIZE + FDCAN_RX_FIFO_0_DATA_SIZE) @@ -26,7 +28,7 @@ #define FDCAN_RX_FIFO_0_OFFSET 0UL // TX FIFO -#define FDCAN_TX_FIFO_EL_CNT 16UL +#define FDCAN_TX_FIFO_EL_CNT 17UL #define FDCAN_TX_FIFO_HEAD_SIZE 8UL // bytes #define FDCAN_TX_FIFO_DATA_SIZE 64UL // bytes #define FDCAN_TX_FIFO_EL_SIZE (FDCAN_TX_FIFO_HEAD_SIZE + FDCAN_TX_FIFO_DATA_SIZE) diff --git a/panda/board/stm32h7/llrtc.h b/panda/board/stm32h7/llrtc.h index d78a514b5..03787d0db 100644 --- a/panda/board/stm32h7/llrtc.h +++ b/panda/board/stm32h7/llrtc.h @@ -1,6 +1,3 @@ -#define RCC_BDCR_MASK_LSE (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL | RCC_BDCR_LSEDRV | RCC_BDCR_LSEBYP | RCC_BDCR_LSEON) -#define RCC_BDCR_MASK_LSI (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL) - void enable_bdomain_protection(void) { register_clear_bits(&(PWR->CR1), PWR_CR1_DBP); } @@ -9,6 +6,38 @@ void disable_bdomain_protection(void) { register_set_bits(&(PWR->CR1), PWR_CR1_DBP); } +void rtc_init(void){ + uint32_t bdcr_opts = RCC_BDCR_RTCEN; + uint32_t bdcr_mask = (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL); + if (current_board->has_rtc_battery) { + bdcr_opts |= (RCC_BDCR_LSEDRV_1 | RCC_BDCR_RTCSEL_0 | RCC_BDCR_LSEON); + bdcr_mask |= (RCC_BDCR_LSEDRV | RCC_BDCR_LSEBYP | RCC_BDCR_LSEON); + } else { + bdcr_opts |= RCC_BDCR_RTCSEL_1; + RCC->CSR |= RCC_CSR_LSION; + while((RCC->CSR & RCC_CSR_LSIRDY) == 0){} + } + + // Initialize RTC module and clock if not done already. + if((RCC->BDCR & bdcr_mask) != bdcr_opts){ + print("Initializing RTC\n"); + // Reset backup domain + register_set_bits(&(RCC->BDCR), RCC_BDCR_BDRST); + + // Disable write protection + disable_bdomain_protection(); + + // Clear backup domain reset + register_clear_bits(&(RCC->BDCR), RCC_BDCR_BDRST); + + // Set RTC options + register_set(&(RCC->BDCR), bdcr_opts, bdcr_mask); + + // Enable write protection + enable_bdomain_protection(); + } +} + void rtc_wakeup_init(void) { EXTI->IMR1 |= EXTI_IMR1_IM19; EXTI->RTSR1 |= EXTI_RTSR1_TR19; // rising edge diff --git a/panda/board/stm32h7/llspi.h b/panda/board/stm32h7/llspi.h index 9e21659b8..ad1dc4d7b 100644 --- a/panda/board/stm32h7/llspi.h +++ b/panda/board/stm32h7/llspi.h @@ -11,6 +11,10 @@ void llspi_mosi_dma(uint8_t *addr, int len) { (void)dat; } + // clear all pending + SPI4->IFCR |= (0x1FFU << 3U); + register_set(&(SPI4->IER), 0, 0x3FFU); + // setup destination and length register_set(&(DMA2_Stream2->M0AR), (uint32_t)addr, 0xFFFFFFFFU); DMA2_Stream2->NDTR = len; @@ -23,20 +27,25 @@ void llspi_mosi_dma(uint8_t *addr, int len) { // panda -> master DMA start void llspi_miso_dma(uint8_t *addr, int len) { - // disable DMA + // disable DMA + SPI DMA2_Stream3->CR &= ~DMA_SxCR_EN; register_clear_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); + register_clear_bits(&(SPI4->CR1), SPI_CR1_SPE); // setup source and length register_set(&(DMA2_Stream3->M0AR), (uint32_t)addr, 0xFFFFFFFFU); DMA2_Stream3->NDTR = len; // clear under-run while we were reading - SPI4->IFCR |= SPI_IFCR_UDRC; + SPI4->IFCR |= (0x1FFU << 3U); - // enable DMA + // setup interrupt on TXC + register_set(&(SPI4->IER), (1U << SPI_IER_EOTIE_Pos), 0x3FFU); + + // enable DMA + SPI register_set_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); DMA2_Stream3->CR |= DMA_SxCR_EN; + register_set_bits(&(SPI4->CR1), SPI_CR1_SPE); } // master -> panda DMA finished @@ -45,37 +54,41 @@ void DMA2_Stream2_IRQ_Handler(void) { ENTER_CRITICAL(); DMA2->LIFCR = DMA_LIFCR_CTCIF2; - spi_handle_rx(); + spi_rx_done(); EXIT_CRITICAL(); } // panda -> master DMA finished void DMA2_Stream3_IRQ_Handler(void) { - // Clear interrupt flag + ENTER_CRITICAL(); + DMA2->LIFCR = DMA_LIFCR_CTCIF3; + spi_tx_dma_done = true; - // Wait until the transaction is actually finished and clear the DR. - // Timeout to prevent hang when the master clock stops. - bool timed_out = false; - uint32_t start_time = microsecond_timer_get(); - while (!(SPI4->SR & SPI_SR_TXC)) { - if (get_ts_elapsed(microsecond_timer_get(), start_time) > SPI_TIMEOUT_US) { - timed_out = true; - break; - } + EXIT_CRITICAL(); +} + +// panda TX finished +void SPI4_IRQ_Handler(void) { + ENTER_CRITICAL(); + + // clear flag + SPI4->IFCR |= (0x1FFU << 3U); + + if (spi_tx_dma_done && ((SPI4->SR & SPI_SR_TXC) != 0)) { + spi_tx_dma_done = false; + spi_tx_done(false); } - volatile uint8_t dat = SPI4->TXDR; - (void)dat; - spi_handle_tx(timed_out); + EXIT_CRITICAL(); } void llspi_init(void) { - // We expect less than 50 transactions (including control messages and CAN buffers) at the 100Hz boardd interval. Can be raised if needed. - REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, 5000U, FAULT_INTERRUPT_RATE_SPI_DMA) - REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, 5000U, FAULT_INTERRUPT_RATE_SPI_DMA) + REGISTER_INTERRUPT(SPI4_IRQn, SPI4_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) + REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) + REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) // Setup MOSI DMA register_set(&(DMAMUX1_Channel10->CCR), 83U, 0xFFFFFFFFU); @@ -88,6 +101,7 @@ void llspi_init(void) { register_set(&(DMA2_Stream3->PAR), (uint32_t)&(SPI4->TXDR), 0xFFFFFFFFU); // Enable SPI + register_set(&(SPI4->IER), 0, 0x3FFU); register_set(&(SPI4->CFG1), (7U << SPI_CFG1_DSIZE_Pos), SPI_CFG1_DSIZE_Msk); register_set(&(SPI4->UDRDR), 0xcd, 0xFFFFU); // set under-run value for debugging register_set(&(SPI4->CR1), SPI_CR1_SPE, 0xFFFFU); @@ -95,4 +109,5 @@ void llspi_init(void) { NVIC_EnableIRQ(DMA2_Stream2_IRQn); NVIC_EnableIRQ(DMA2_Stream3_IRQn); + NVIC_EnableIRQ(SPI4_IRQn); } diff --git a/panda/board/stm32h7/llusb.h b/panda/board/stm32h7/llusb.h index 58646574a..c46a693b0 100644 --- a/panda/board/stm32h7/llusb.h +++ b/panda/board/stm32h7/llusb.h @@ -83,8 +83,8 @@ void usb_init(void) { // Enable interrupts matching to the Device mode ONLY USBx->GINTMSK = USB_OTG_GINTMSK_USBRST | USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_OTGINT | USB_OTG_GINTMSK_RXFLVLM | USB_OTG_GINTMSK_GONAKEFFM | USB_OTG_GINTMSK_GINAKEFFM | - USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT | USB_OTG_GINTMSK_USBSUSPM | - USB_OTG_GINTMSK_CIDSCHGM | USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_MMISM | USB_OTG_GINTMSK_EOPFM; + USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT | + USB_OTG_GINTMSK_CIDSCHGM | USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_MMISM; // Set USB Turnaround time USBx->GUSBCFG |= ((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT); diff --git a/panda/board/stm32h7/peripherals.h b/panda/board/stm32h7/peripherals.h index 754f157ae..7e3b2e5b1 100644 --- a/panda/board/stm32h7/peripherals.h +++ b/panda/board/stm32h7/peripherals.h @@ -1,10 +1,18 @@ void gpio_usb_init(void) { - // A11,A12: USB: + // A11,A12: USB set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG1_FS); set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG1_FS); GPIOA->OSPEEDR = GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12; } +void gpio_spi_init(void) { + set_gpio_alternate(GPIOE, 11, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 12, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 13, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 14, GPIO_AF5_SPI4); + register_set_bits(&(GPIOE->OSPEEDR), GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12 | GPIO_OSPEEDR_OSPEED13 | GPIO_OSPEEDR_OSPEED14); +} + void gpio_usart2_init(void) { // A2,A3: USART 2 for debugging set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2); @@ -85,7 +93,10 @@ void common_init_gpio(void) { void flasher_peripherals_init(void) { RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; + + // SPI + DMA RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; + RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; } // Peripheral initialization diff --git a/panda/board/stm32h7/stm32h7_config.h b/panda/board/stm32h7/stm32h7_config.h index d1b19fa34..41ae768a9 100644 --- a/panda/board/stm32h7/stm32h7_config.h +++ b/panda/board/stm32h7/stm32h7_config.h @@ -14,14 +14,22 @@ #define BOOTLOADER_ADDRESS 0x1FF09804U -// Around (1Mbps / 8 bits/byte / 12 bytes per message) -#define CAN_INTERRUPT_RATE 12000U // FIXME: should raise to 16000 ? +/* +An IRQ is received on message RX/TX (or RX errors), with +separate IRQs for RX and TX. + +0-byte CAN FD frame as the worst case: +- 17 slow bits = SOF + 11 ID + R1 + IDE + EDL + R0 + BRS +- 23 fast bits = ESI + 4 DLC + 0 DATA + 17 CRC + CRC delimeter +- 12 slow bits = ACK + DEL + 7 EOF + 3 IFS +- all currently supported cars are 0.5 Mbps / 2 Mbps + +1 / ((29 bits / 0.5Mbps) + (23 bits / 2Mbps)) = 14388Hz +*/ +#define CAN_INTERRUPT_RATE 16000U #define MAX_LED_FADE 10240U -// Threshold voltage (mV) for either of the SBUs to be below before deciding harness is connected -#define HARNESS_CONNECTED_THRESHOLD 40000U - // There are 163 external interrupt sources (see stm32f735xx.h) #define NUM_INTERRUPTS 163U @@ -33,6 +41,8 @@ #define INTERRUPT_TIMER_IRQ TIM6_DAC_IRQn #define INTERRUPT_TIMER TIM6 +#define IND_WDG IWDG1 + #define PROVISION_CHUNK_ADDRESS 0x080FFFE0U #define DEVICE_SERIAL_NUMBER_ADDRESS 0x080FFFC0U @@ -56,7 +66,7 @@ #include "stm32h7/peripherals.h" #include "stm32h7/interrupt_handlers.h" #include "drivers/timers.h" -#include "stm32h7/lladc.h" +#include "drivers/watchdog.h" #if !defined(BOOTSTUB) && defined(PANDA) #include "drivers/uart.h" diff --git a/panda/board/utils.h b/panda/board/utils.h index 4232e0a97..3ab6b30bc 100644 --- a/panda/board/utils.h +++ b/panda/board/utils.h @@ -1,16 +1,26 @@ -#define MIN(a,b) \ - ({ __typeof__ (a) _a = (a); \ - __typeof__ (b) _b = (b); \ - (_a < _b) ? _a : _b; }) +#define MIN(a, b) ({ \ + __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + (_a < _b) ? _a : _b; \ +}) -#define MAX(a,b) \ - ({ __typeof__ (a) _a = (a); \ - __typeof__ (b) _b = (b); \ - (_a > _b) ? _a : _b; }) +#define MAX(a, b) ({ \ + __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + (_a > _b) ? _a : _b; \ +}) -#define ABS(a) \ - ({ __typeof__ (a) _a = (a); \ - (_a > 0) ? _a : (-_a); }) +#define CLAMP(x, low, high) ({ \ + __typeof__(x) __x = (x); \ + __typeof__(low) __low = (low);\ + __typeof__(high) __high = (high);\ + (__x > __high) ? __high : ((__x < __low) ? __low : __x); \ +}) + +#define ABS(a) ({ \ + __typeof__ (a) _a = (a); \ + (_a > 0) ? _a : (-_a); \ +}) #ifndef NULL #define NULL ((void*)0) diff --git a/panda/crypto/sign.py b/panda/crypto/sign.py index e199a6b14..daea38630 100755 --- a/panda/crypto/sign.py +++ b/panda/crypto/sign.py @@ -9,28 +9,30 @@ import binascii # increment this to make new hardware not run old versions VERSION = 2 -rsa = RSA.importKey(open(sys.argv[3]).read()) +if __name__ == "__main__": + with open(sys.argv[3]) as k: + rsa = RSA.importKey(k.read()) -with open(sys.argv[1], "rb") as f: - dat = f.read() + with open(sys.argv[1], "rb") as f: + dat = f.read() -print("signing", len(dat), "bytes") + print("signing", len(dat), "bytes") -with open(sys.argv[2], "wb") as f: - if os.getenv("SETLEN") is not None: - # add the version at the end - dat += b"VERS" + struct.pack("I", VERSION) - # add the length at the beginning - x = struct.pack("I", len(dat)) + dat[4:] - # mock signature of dat[4:] - dd = hashlib.sha1(dat[4:]).digest() - else: - x = dat - dd = hashlib.sha1(dat).digest() + with open(sys.argv[2], "wb") as f: + if os.getenv("SETLEN") is not None: + # add the version at the end + dat += b"VERS" + struct.pack("I", VERSION) + # add the length at the beginning + x = struct.pack("I", len(dat)) + dat[4:] + # mock signature of dat[4:] + dd = hashlib.sha1(dat[4:]).digest() + else: + x = dat + dd = hashlib.sha1(dat).digest() - print("hash:", str(binascii.hexlify(dd), "utf-8")) - dd = b"\x00\x01" + b"\xff" * 0x69 + b"\x00" + dd - rsa_out = pow(int.from_bytes(dd, byteorder='big', signed=False), rsa.d, rsa.n) - sig = (hex(rsa_out)[2:].rjust(0x100, '0')) - x += binascii.unhexlify(sig) - f.write(x) + print("hash:", str(binascii.hexlify(dd), "utf-8")) + dd = b"\x00\x01" + b"\xff" * 0x69 + b"\x00" + dd + rsa_out = pow(int.from_bytes(dd, byteorder='big', signed=False), rsa.d, rsa.n) + sig = (hex(rsa_out)[2:].rjust(0x100, '0')) + x += binascii.unhexlify(sig) + f.write(x) diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 2e44b3db7..0849f0a75 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -13,6 +13,7 @@ from functools import wraps from typing import Optional from itertools import accumulate +from .base import BaseHandle from .constants import McuType from .dfu import PandaDFU from .isotp import isotp_send, isotp_recv @@ -25,12 +26,12 @@ __version__ = '0.0.10' LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper() logging.basicConfig(level=LOGLEVEL, format='%(message)s') - USBPACKET_MAX_SIZE = 0x40 CANPACKET_HEAD_SIZE = 0x6 DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64] LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)} + def calculate_checksum(data): res = 0 for b in data: @@ -149,7 +150,7 @@ class Panda: SAFETY_NOOUTPUT = 19 SAFETY_HONDA_BOSCH = 20 SAFETY_VOLKSWAGEN_PQ = 21 - SAFETY_SUBARU_LEGACY = 22 + SAFETY_SUBARU_PREGLOBAL = 22 SAFETY_HYUNDAI_LEGACY = 23 SAFETY_HYUNDAI_COMMUNITY = 24 SAFETY_STELLANTIS = 25 @@ -181,21 +182,32 @@ class Panda: HW_TYPE_TRES = b'\x09' CAN_PACKET_VERSION = 4 - HEALTH_PACKET_VERSION = 11 + HEALTH_PACKET_VERSION = 14 CAN_HEALTH_PACKET_VERSION = 4 - HEALTH_STRUCT = struct.Struct(" bool: + return self._handle_open + def reconnect(self): - if self._handle is not None: + if self._handle_open: self.close() time.sleep(1.0) @@ -410,7 +433,7 @@ class Panda: except Exception: logging.debug("reconnecting is taking %d seconds...", i + 1) try: - dfu = PandaDFU(PandaDFU.st_serial_to_dfu_serial(self._serial, self._mcu_type)) + dfu = PandaDFU(self.get_dfu_serial()) dfu.recover() except Exception: pass @@ -418,13 +441,17 @@ class Panda: if not success: raise Exception("reconnect failed") + @staticmethod + def flasher_present(handle: BaseHandle) -> bool: + fr = handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc) + return fr[4:8] == b"\xde\xad\xd0\x0d" + @staticmethod def flash_static(handle, code, mcu_type): assert mcu_type is not None, "must set valid mcu_type to flash" # confirm flasher is present - fr = handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc) - assert fr[4:8] == b"\xde\xad\xd0\x0d" + assert Panda.flasher_present(handle) # determine sectors to erase apps_sectors_cumsum = accumulate(mcu_type.config.sector_sizes[1:]) @@ -477,8 +504,8 @@ class Panda: if reconnect: self.reconnect() - def recover(self, timeout: Optional[int] = None, reset: bool = True) -> bool: - dfu_serial = PandaDFU.st_serial_to_dfu_serial(self._serial, self._mcu_type) + def recover(self, timeout: Optional[int] = 60, reset: bool = True) -> bool: + dfu_serial = self.get_dfu_serial() if reset: self.reset(enter_bootstub=True) @@ -505,6 +532,11 @@ class Panda: return False return True + def up_to_date(self) -> bool: + current = self.get_signature() + expected = Panda.get_signature_from_firmware(self.get_mcu_type().config.app_path) + return (current == expected) + def call_control_api(self, msg): self._handle.controlWrite(Panda.REQUEST_OUT, msg, 0, 0, b'') @@ -538,6 +570,10 @@ class Panda: "interrupt_load": a[20], "fan_power": a[21], "safety_rx_checks_invalid": a[22], + "spi_checksum_error_count": a[23], + "fan_stall_count": a[24], + "sbu1_voltage_mV": a[25], + "sbu2_voltage_mV": a[26], } @ensure_can_health_packet_version @@ -592,11 +628,11 @@ class Panda: @staticmethod def get_signature_from_firmware(fn) -> bytes: - f = open(fn, 'rb') - f.seek(-128, 2) # Seek from end of file - return f.read(128) + with open(fn, 'rb') as f: + f.seek(-128, 2) # Seek from end of file + return f.read(128) - def get_signature(self): + def get_signature(self) -> bytes: part_1 = self._handle.controlRead(Panda.REQUEST_IN, 0xd3, 0, 0, 0x40) part_2 = self._handle.controlRead(Panda.REQUEST_IN, 0xd4, 0, 0, 0x40) return bytes(part_1 + part_2) @@ -656,6 +692,9 @@ class Panda: """ return self._serial + def get_dfu_serial(self): + return PandaDFU.st_serial_to_dfu_serial(self._serial, self._mcu_type) + def get_uid(self): """ Returns the UID from the MCU diff --git a/panda/python/base.py b/panda/python/base.py index d051e8770..5bfa56489 100644 --- a/panda/python/base.py +++ b/panda/python/base.py @@ -1,25 +1,67 @@ from abc import ABC, abstractmethod from typing import List +from .constants import McuType + +TIMEOUT = int(15 * 1e3) # default timeout, in milliseconds -# This mimics the handle given by libusb1 for easy interoperability class BaseHandle(ABC): + """ + A handle to talk to a panda. + Borrows heavily from the libusb1 handle API. + """ + @abstractmethod def close(self) -> None: ... @abstractmethod - def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0) -> int: + def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT) -> int: ... @abstractmethod - def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0) -> bytes: + def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = TIMEOUT) -> bytes: ... @abstractmethod - def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int: + def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int: ... @abstractmethod - def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes: + def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes: ... + + +class BaseSTBootloaderHandle(ABC): + """ + A handle to talk to a panda while it's in the STM32 bootloader. + """ + + @abstractmethod + def get_mcu_type(self) -> McuType: + ... + + @abstractmethod + def close(self) -> None: + ... + + @abstractmethod + def clear_status(self) -> None: + ... + + @abstractmethod + def program(self, address: int, dat: bytes) -> None: + ... + + @abstractmethod + def erase_app(self) -> None: + ... + + @abstractmethod + def erase_bootstub(self) -> None: + ... + + @abstractmethod + def jump(self, address: int) -> None: + ... + diff --git a/panda/python/constants.py b/panda/python/constants.py index d964192c8..c55fd2c9b 100644 --- a/panda/python/constants.py +++ b/panda/python/constants.py @@ -8,6 +8,7 @@ BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") class McuConfig(NamedTuple): mcu: str mcu_idcode: int + uid_address: int block_size: int sector_sizes: List[int] serial_number_address: int @@ -17,6 +18,7 @@ class McuConfig(NamedTuple): bootstub_path: str Fx = ( + 0x1FFF7A10, 0x800, [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)], 0x1FFF79C0, @@ -31,6 +33,7 @@ F4Config = McuConfig("STM32F4", 0x463, *Fx) H7Config = McuConfig( "STM32H7", 0x483, + 0x1FF1E800, 0x400, # there is an 8th sector, but we use that for the provisioning chunk, so don't program over that! [0x20000 for _ in range(7)], @@ -50,3 +53,5 @@ class McuType(enum.Enum): @property def config(self): return self.value + +MCU_TYPE_BY_IDCODE = {m.config.mcu_idcode: m for m in McuType} diff --git a/panda/python/dfu.py b/panda/python/dfu.py index dfdec1fd3..f724bf15d 100644 --- a/panda/python/dfu.py +++ b/panda/python/dfu.py @@ -1,52 +1,96 @@ import usb1 import struct import binascii +from typing import List, Optional +from .base import BaseSTBootloaderHandle +from .spi import STBootloaderSPIHandle, PandaSpiException +from .usb import STBootloaderUSBHandle from .constants import McuType -# *** DFU mode *** -DFU_DNLOAD = 1 -DFU_UPLOAD = 2 -DFU_GETSTATUS = 3 -DFU_CLRSTATUS = 4 -DFU_ABORT = 6 - class PandaDFU: - def __init__(self, dfu_serial): - self._handle = None + def __init__(self, dfu_serial: Optional[str]): + # try USB, then SPI + handle: Optional[BaseSTBootloaderHandle] + handle = PandaDFU.usb_connect(dfu_serial) + if handle is None: + handle = PandaDFU.spi_connect(dfu_serial) + + if handle is None: + raise Exception(f"failed to open DFU device {dfu_serial}") + + self._handle: BaseSTBootloaderHandle = handle + self._mcu_type: McuType = self._handle.get_mcu_type() + + @staticmethod + def usb_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderUSBHandle]: + handle = None context = usb1.USBContext() + context.open() for device in context.getDeviceList(skip_on_error=True): if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11: try: this_dfu_serial = device.open().getASCIIStringDescriptor(3) except Exception: continue + if this_dfu_serial == dfu_serial or dfu_serial is None: - self._handle = device.open() - self._mcu_type = self.get_mcu_type(device) + handle = STBootloaderUSBHandle(device, device.open()) break - if self._handle is None: - raise Exception(f"failed to open DFU device {dfu_serial}") + return handle @staticmethod - def list(): - context = usb1.USBContext() + def spi_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderSPIHandle]: + handle = None + this_dfu_serial = None + + try: + handle = STBootloaderSPIHandle() + this_dfu_serial = PandaDFU.st_serial_to_dfu_serial(handle.get_uid(), handle.get_mcu_type()) + except PandaSpiException: + handle = None + + if dfu_serial is not None and dfu_serial != this_dfu_serial: + handle = None + + return handle + + @staticmethod + def list() -> List[str]: + ret = PandaDFU.usb_list() + ret += PandaDFU.spi_list() + return list(set(ret)) + + @staticmethod + def usb_list() -> List[str]: dfu_serials = [] try: - for device in context.getDeviceList(skip_on_error=True): - if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11: - try: - dfu_serials.append(device.open().getASCIIStringDescriptor(3)) - except Exception: - pass + with usb1.USBContext() as context: + for device in context.getDeviceList(skip_on_error=True): + if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11: + try: + dfu_serials.append(device.open().getASCIIStringDescriptor(3)) + except Exception: + pass except Exception: pass return dfu_serials @staticmethod - def st_serial_to_dfu_serial(st, mcu_type=McuType.F4): + def spi_list() -> List[str]: + try: + h = PandaDFU.spi_connect(None) + if h is not None: + dfu_serial = PandaDFU.st_serial_to_dfu_serial(h.get_uid(), h.get_mcu_type()) + return [dfu_serial, ] + except PandaSpiException: + pass + return [] + + @staticmethod + def st_serial_to_dfu_serial(st: str, mcu_type: McuType = McuType.F4): if st is None or st == "none": return None uid_base = struct.unpack("H" * 6, bytes.fromhex(st)) @@ -55,52 +99,17 @@ class PandaDFU: else: return binascii.hexlify(struct.pack("!HHH", uid_base[1] + uid_base[5], uid_base[0] + uid_base[4] + 0xA, uid_base[3])).upper().decode("utf-8") - def get_mcu_type(self, dev) -> McuType: - # TODO: Find a way to detect F4 vs F2 - # TODO: also check F4 BCD, don't assume in else - return McuType.H7 if dev.getbcdDevice() == 512 else McuType.F4 + def get_mcu_type(self) -> McuType: + return self._mcu_type - def status(self): - while 1: - dat = self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6) - if dat[1] == 0: - break - - def clear_status(self): - # Clear status - stat = self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6) - if stat[4] == 0xa: - self._handle.controlRead(0x21, DFU_CLRSTATUS, 0, 0, 0) - elif stat[4] == 0x9: - self._handle.controlWrite(0x21, DFU_ABORT, 0, 0, b"") - self.status() - stat = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6)) - - def erase(self, address): - self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x41" + struct.pack("I", address)) - self.status() - - def program(self, address, dat, block_size=None): - if block_size is None: - block_size = len(dat) - - # Set Address Pointer - self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address)) - self.status() - - # Program - dat += b"\xFF" * ((block_size - len(dat)) % block_size) - for i in range(0, len(dat) // block_size): - ldat = dat[i * block_size:(i + 1) * block_size] - print("programming %d with length %d" % (i, len(ldat))) - self._handle.controlWrite(0x21, DFU_DNLOAD, 2 + i, 0, ldat) - self.status() + def reset(self): + self._handle.jump(self._mcu_type.config.bootstub_address) def program_bootstub(self, code_bootstub): - self.clear_status() - self.erase(self._mcu_type.config.bootstub_address) - self.erase(self._mcu_type.config.app_address) - self.program(self._mcu_type.config.bootstub_address, code_bootstub, self._mcu_type.config.block_size) + self._handle.clear_status() + self._handle.erase_bootstub() + self._handle.erase_app() + self._handle.program(self._mcu_type.config.bootstub_address, code_bootstub) self.reset() def recover(self): @@ -108,11 +117,3 @@ class PandaDFU: code = f.read() self.program_bootstub(code) - def reset(self): - self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", self._mcu_type.config.bootstub_address)) - self.status() - try: - self._handle.controlWrite(0x21, DFU_DNLOAD, 2, 0, b"") - _ = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6)) - except Exception: - pass diff --git a/panda/python/spi.py b/panda/python/spi.py index 8b3cab105..e427bea86 100644 --- a/panda/python/spi.py +++ b/panda/python/spi.py @@ -1,3 +1,4 @@ +import binascii import os import fcntl import math @@ -7,9 +8,10 @@ import logging import threading from contextlib import contextmanager from functools import reduce -from typing import List +from typing import List, Optional -from .base import BaseHandle +from .base import BaseHandle, BaseSTBootloaderHandle, TIMEOUT +from .constants import McuType, MCU_TYPE_BY_IDCODE try: import spidev @@ -23,10 +25,10 @@ DACK = 0x85 NACK = 0x1F CHECKSUM_START = 0xAB -ACK_TIMEOUT_SECONDS = 0.1 +MIN_ACK_TIMEOUT_MS = 100 MAX_XFER_RETRY_COUNT = 5 -USB_MAX_SIZE = 0x40 +XFER_SIZE = 1000 DEV_PATH = "/dev/spidev0.0" @@ -56,7 +58,14 @@ class SpiDevice: """ Provides locked, thread-safe access to a panda's SPI interface. """ - def __init__(self, speed=30000000): + + # 50MHz is the max of the 845. older rev comma three + # may not support the full 50MHz + MAX_SPEED = 50000000 + + def __init__(self, speed=MAX_SPEED): + assert speed <= self.MAX_SPEED + if not os.path.exists(DEV_PATH): raise PandaSpiUnavailable(f"SPI device not found: {DEV_PATH}") if spidev is None: @@ -94,10 +103,12 @@ class PandaSpiHandle(BaseHandle): cksum ^= b return cksum - def _wait_for_ack(self, spi, ack_val: int) -> None: + def _wait_for_ack(self, spi, ack_val: int, timeout: int, tx: int) -> None: + timeout_s = max(MIN_ACK_TIMEOUT_MS, timeout) * 1e-3 + start = time.monotonic() - while (time.monotonic() - start) < ACK_TIMEOUT_SECONDS: - dat = spi.xfer2(b"\x12")[0] + while (timeout == 0) or ((time.monotonic() - start) < timeout_s): + dat = spi.xfer2([tx, ])[0] if dat == NACK: raise PandaSpiNackResponse elif dat == ack_val: @@ -105,7 +116,7 @@ class PandaSpiHandle(BaseHandle): raise PandaSpiMissingAck - def _transfer(self, spi, endpoint: int, data, max_rx_len: int = 1000) -> bytes: + def _transfer(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = 1000) -> bytes: logging.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len) logging.debug("==============================================") @@ -119,7 +130,7 @@ class PandaSpiHandle(BaseHandle): spi.xfer2(packet) logging.debug("- waiting for header ACK") - self._wait_for_ack(spi, HACK) + self._wait_for_ack(spi, HACK, timeout, 0x11) # send data logging.debug("- sending data") @@ -127,11 +138,13 @@ class PandaSpiHandle(BaseHandle): spi.xfer2(packet) logging.debug("- waiting for data ACK") - self._wait_for_ack(spi, DACK) + self._wait_for_ack(spi, DACK, timeout, 0x13) # get response length, then response response_len_bytes = bytes(spi.xfer2(b"\x00" * 2)) response_len = struct.unpack(" max_rx_len: + raise PandaSpiException("response length greater than max") logging.debug("- receiving response") dat = bytes(spi.xfer2(b"\x00" * (response_len + 1))) @@ -141,34 +154,177 @@ class PandaSpiHandle(BaseHandle): return dat[:-1] except PandaSpiException as e: exc = e - logging.debug("SPI transfer failed, %d retries left", n, exc_info=True) + logging.debug("SPI transfer failed, %d retries left", MAX_XFER_RETRY_COUNT - n - 1, exc_info=True) raise exc # libusb1 functions def close(self): self.dev.close() - def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0): + def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT): with self.dev.acquire() as spi: - return self._transfer(spi, 0, struct.pack(" int: + def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int: with self.dev.acquire() as spi: - for x in range(math.ceil(len(data) / USB_MAX_SIZE)): - self._transfer(spi, endpoint, data[USB_MAX_SIZE*x:USB_MAX_SIZE*(x+1)]) + for x in range(math.ceil(len(data) / XFER_SIZE)): + self._transfer(spi, endpoint, data[XFER_SIZE*x:XFER_SIZE*(x+1)], timeout) return len(data) - def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes: + def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes: ret: List[int] = [] with self.dev.acquire() as spi: - for _ in range(math.ceil(length / USB_MAX_SIZE)): - d = self._transfer(spi, endpoint, [], max_rx_len=USB_MAX_SIZE) + for _ in range(math.ceil(length / XFER_SIZE)): + d = self._transfer(spi, endpoint, [], timeout, max_rx_len=XFER_SIZE) ret += d - if len(d) < USB_MAX_SIZE: + if len(d) < XFER_SIZE: break return bytes(ret) + + +class STBootloaderSPIHandle(BaseSTBootloaderHandle): + """ + Implementation of the STM32 SPI bootloader protocol described in: + https://www.st.com/resource/en/application_note/an4286-spi-protocol-used-in-the-stm32-bootloader-stmicroelectronics.pdf + """ + + SYNC = 0x5A + ACK = 0x79 + NACK = 0x1F + + def __init__(self): + self.dev = SpiDevice(speed=1000000) + + # say hello + try: + with self.dev.acquire() as spi: + spi.xfer([self.SYNC, ]) + try: + self._get_ack(spi) + except (PandaSpiNackResponse, PandaSpiMissingAck): + # NACK ok here, will only ACK the first time + pass + + self._mcu_type = MCU_TYPE_BY_IDCODE[self.get_chip_id()] + except PandaSpiException: + raise PandaSpiException("failed to connect to panda") # pylint: disable=W0707 + + def _get_ack(self, spi, timeout=1.0): + data = 0x00 + start_time = time.monotonic() + while data not in (self.ACK, self.NACK) and (time.monotonic() - start_time < timeout): + data = spi.xfer([0x00, ])[0] + time.sleep(0.001) + spi.xfer([self.ACK, ]) + + if data == self.NACK: + raise PandaSpiNackResponse + elif data != self.ACK: + raise PandaSpiMissingAck + + def _cmd_no_retry(self, cmd: int, data: Optional[List[bytes]] = None, read_bytes: int = 0, predata=None) -> bytes: + ret = b"" + with self.dev.acquire() as spi: + # sync + command + spi.xfer([self.SYNC, ]) + spi.xfer([cmd, cmd ^ 0xFF]) + self._get_ack(spi, timeout=0.1) + + # "predata" - for commands that send the first data without a checksum + if predata is not None: + spi.xfer(predata) + self._get_ack(spi) + + # send data + if data is not None: + for d in data: + if predata is not None: + spi.xfer(d + self._checksum(predata + d)) + else: + spi.xfer(d + self._checksum(d)) + self._get_ack(spi, timeout=20) + + # receive + if read_bytes > 0: + ret = spi.xfer([0x00, ]*(read_bytes + 1))[1:] + if data is None or len(data) == 0: + self._get_ack(spi) + + return bytes(ret) + + def _cmd(self, cmd: int, data: Optional[List[bytes]] = None, read_bytes: int = 0, predata=None) -> bytes: + exc = PandaSpiException() + for n in range(MAX_XFER_RETRY_COUNT): + try: + return self._cmd_no_retry(cmd, data, read_bytes, predata) + except PandaSpiException as e: + exc = e + logging.debug("SPI transfer failed, %d retries left", MAX_XFER_RETRY_COUNT - n - 1, exc_info=True) + raise exc + + def _checksum(self, data: bytes) -> bytes: + if len(data) == 1: + ret = data[0] ^ 0xFF + else: + ret = reduce(lambda a, b: a ^ b, data) + return bytes([ret, ]) + + # *** Bootloader commands *** + + def read(self, address: int, length: int): + data = [struct.pack('>I', address), struct.pack('B', length - 1)] + return self._cmd(0x11, data=data, read_bytes=length) + + def get_chip_id(self) -> int: + r = self._cmd(0x02, read_bytes=3) + assert r[0] == 1 # response length - 1 + return ((r[1] << 8) + r[2]) + + def go_cmd(self, address: int) -> None: + self._cmd(0x21, data=[struct.pack('>I', address), ]) + + # *** helpers *** + + def get_uid(self): + dat = self.read(McuType.H7.config.uid_address, 12) + return binascii.hexlify(dat).decode() + + def erase_sector(self, sector: int): + p = struct.pack('>H', 0) # number of sectors to erase + d = struct.pack('>H', sector) + self._cmd(0x44, data=[d, ], predata=p) + + # *** PandaDFU API *** + + def erase_app(self): + self.erase_sector(1) + + def erase_bootstub(self): + self.erase_sector(0) + + def get_mcu_type(self): + return self._mcu_type + + def clear_status(self): + pass + + def close(self): + self.dev.close() + + def program(self, address, dat): + bs = 256 # max block size for writing to flash over SPI + dat += b"\xFF" * ((bs - len(dat)) % bs) + for i in range(0, len(dat) // bs): + block = dat[i * bs:(i + 1) * bs] + self._cmd(0x31, data=[ + struct.pack('>I', address + i*bs), + bytes([len(block) - 1]) + block, + ]) + + def jump(self, address): + self.go_cmd(self._mcu_type.config.bootstub_address) diff --git a/panda/python/spi_dfu.py b/panda/python/spi_dfu.py deleted file mode 100644 index 73d930528..000000000 --- a/panda/python/spi_dfu.py +++ /dev/null @@ -1,118 +0,0 @@ -import time -import struct -from functools import reduce - -from .constants import McuType -from .spi import SpiDevice - -SYNC = 0x5A -ACK = 0x79 -NACK = 0x1F - -# https://www.st.com/resource/en/application_note/an4286-spi-protocol-used-in-the-stm32-bootloader-stmicroelectronics.pdf -class PandaSpiDFU: - def __init__(self, dfu_serial): - self.dev = SpiDevice(speed=1000000) - - # say hello - with self.dev.acquire() as spi: - try: - spi.xfer([SYNC, ]) - self._get_ack(spi) - except Exception: - raise Exception("failed to connect to panda") # pylint: disable=W0707 - - self._mcu_type = self.get_mcu_type() - - def _get_ack(self, spi, timeout=1.0): - data = 0x00 - start_time = time.monotonic() - while data not in (ACK, NACK) and (time.monotonic() - start_time < timeout): - data = spi.xfer([0x00, ])[0] - time.sleep(0.001) - spi.xfer([ACK, ]) - - if data == NACK: - raise Exception("Got NACK response") - elif data != ACK: - raise Exception("Missing ACK") - - def _cmd(self, cmd, data=None, read_bytes=0) -> bytes: - ret = b"" - with self.dev.acquire() as spi: - # sync - spi.xfer([SYNC, ]) - - # send command - spi.xfer([cmd, cmd ^ 0xFF]) - self._get_ack(spi) - - # send data - if data is not None: - for d in data: - spi.xfer(self.add_checksum(d)) - self._get_ack(spi, timeout=20) - - # receive - if read_bytes > 0: - # send busy byte - ret = spi.xfer([0x00, ]*(read_bytes + 1))[1:] - self._get_ack(spi) - - return ret - - def add_checksum(self, data): - return data + bytes([reduce(lambda a, b: a ^ b, data)]) - - # ***** ST Bootloader functions ***** - - def get_bootloader_version(self) -> int: - return self._cmd(0x01, read_bytes=1)[0] - - def get_id(self) -> int: - ret = self._cmd(0x02, read_bytes=3) - assert ret[0] == 1 - return ((ret[1] << 8) + ret[2]) - - def go_cmd(self, address: int) -> None: - self._cmd(0x21, data=[struct.pack('>I', address), ]) - - def erase(self, address: int) -> None: - d = struct.pack('>H', address) - self._cmd(0x44, data=[d, ]) - - # ***** panda api ***** - - def get_mcu_type(self) -> McuType: - mcu_by_id = {mcu.config.mcu_idcode: mcu for mcu in McuType} - return mcu_by_id[self.get_id()] - - def global_erase(self): - self.erase(0xFFFF) - - def program_file(self, address, fn): - with open(fn, 'rb') as f: - code = f.read() - - i = 0 - while i < len(code): - #print(i, len(code)) - block = code[i:i+256] - if len(block) < 256: - block += b'\xFF' * (256 - len(block)) - - self._cmd(0x31, data=[ - struct.pack('>I', address + i), - bytes([len(block) - 1]) + block, - ]) - #print(f"Written {len(block)} bytes to {hex(address + i)}") - i += 256 - - def program_bootstub(self): - self.program_file(self._mcu_type.config.bootstub_address, self._mcu_type.config.bootstub_path) - - def program_app(self): - self.program_file(self._mcu_type.config.app_address, self._mcu_type.config.app_path) - - def reset(self): - self.go_cmd(self._mcu_type.config.bootstub_address) diff --git a/panda/python/uds.py b/panda/python/uds.py index 5bf35088e..06d8d8257 100644 --- a/panda/python/uds.py +++ b/panda/python/uds.py @@ -141,6 +141,12 @@ class DYNAMIC_DEFINITION_TYPE(IntEnum): DEFINE_BY_MEMORY_ADDRESS = 2 CLEAR_DYNAMICALLY_DEFINED_DATA_IDENTIFIER = 3 +class ISOTP_FRAME_TYPE(IntEnum): + SINGLE = 0 + FIRST = 1 + CONSECUTIVE = 2 + FLOW = 3 + class DynamicSourceDefinition(NamedTuple): data_identifier: int position: int @@ -438,38 +444,42 @@ class IsoTpMessage(): timeout = self.timeout start_time = time.monotonic() - updated = False + rx_in_progress = False try: while True: for msg in self._can_client.recv(): - self._isotp_rx_next(msg) + frame_type = self._isotp_rx_next(msg) start_time = time.monotonic() - updated = True + rx_in_progress = frame_type == ISOTP_FRAME_TYPE.CONSECUTIVE if self.tx_done and self.rx_done: - return self.rx_dat, updated + return self.rx_dat, False # no timeout indicates non-blocking if timeout == 0: - return None, updated + return None, rx_in_progress if time.monotonic() - start_time > timeout: raise MessageTimeoutError("timeout waiting for response") finally: if self.debug and self.rx_dat: print(f"ISO-TP: RESPONSE - {hex(self._can_client.rx_addr)} 0x{bytes.hex(self.rx_dat)}") - def _isotp_rx_next(self, rx_data: bytes) -> None: - # single rx_frame - if rx_data[0] >> 4 == 0x0: + def _isotp_rx_next(self, rx_data: bytes) -> ISOTP_FRAME_TYPE: + # TODO: Handle CAN frame data optimization, which is allowed with some frame types + # # ISO 15765-2 specifies an eight byte CAN frame for ISO-TP communication + # assert len(rx_data) == self.max_len, f"isotp - rx: invalid CAN frame length: {len(rx_data)}" + + if rx_data[0] >> 4 == ISOTP_FRAME_TYPE.SINGLE: self.rx_len = rx_data[0] & 0xFF + assert self.rx_len < self.max_len, f"isotp - rx: invalid single frame length: {self.rx_len}" self.rx_dat = rx_data[1:1 + self.rx_len] self.rx_idx = 0 self.rx_done = True if self.debug: print(f"ISO-TP: RX - single frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}") - return + return ISOTP_FRAME_TYPE.SINGLE - # first rx_frame - if rx_data[0] >> 4 == 0x1: + elif rx_data[0] >> 4 == ISOTP_FRAME_TYPE.FIRST: self.rx_len = ((rx_data[0] & 0x0F) << 8) + rx_data[1] + assert self.max_len <= self.rx_len, f"isotp - rx: invalid first frame length: {self.rx_len}" self.rx_dat = rx_data[2:] self.rx_idx = 0 self.rx_done = False @@ -479,10 +489,9 @@ class IsoTpMessage(): print(f"ISO-TP: TX - flow control continue - {hex(self._can_client.tx_addr)}") # send flow control message self._can_client.send([self.flow_control_msg]) - return + return ISOTP_FRAME_TYPE.FIRST - # consecutive rx frame - if rx_data[0] >> 4 == 0x2: + elif rx_data[0] >> 4 == ISOTP_FRAME_TYPE.CONSECUTIVE: assert not self.rx_done, "isotp - rx: consecutive frame with no active frame" self.rx_idx += 1 assert self.rx_idx & 0xF == rx_data[0] & 0xF, "isotp - rx: invalid consecutive frame index" @@ -495,10 +504,9 @@ class IsoTpMessage(): self._can_client.send([self.flow_control_msg]) if self.debug: print(f"ISO-TP: RX - consecutive frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}") - return + return ISOTP_FRAME_TYPE.CONSECUTIVE - # flow control - if rx_data[0] >> 4 == 0x3: + elif rx_data[0] >> 4 == ISOTP_FRAME_TYPE.FLOW: assert not self.tx_done, "isotp - rx: flow control with no active frame" assert rx_data[0] != 0x32, "isotp - rx: flow-control overflow/abort" assert rx_data[0] == 0x30 or rx_data[0] == 0x31, "isotp - rx: flow-control transfer state indicator invalid" @@ -512,7 +520,7 @@ class IsoTpMessage(): # first frame = 6 bytes, each consecutive frame = 7 bytes num_bytes = self.max_len - 1 - start = 6 + self.tx_idx * num_bytes + start = self.max_len - 2 + self.tx_idx * num_bytes count = rx_data[1] end = start + count * num_bytes if count > 0 else self.tx_len tx_msgs = [] @@ -531,9 +539,16 @@ class IsoTpMessage(): # wait (do nothing until next flow control message) if self.debug: print(f"ISO-TP: TX - flow control wait - {hex(self._can_client.tx_addr)}") + return ISOTP_FRAME_TYPE.FLOW + + # 4-15 - reserved + else: + raise Exception(f"isotp - rx: invalid frame type: {rx_data[0] >> 4}") + FUNCTIONAL_ADDRS = [0x7DF, 0x18DB33F1] + def get_rx_addr_for_tx_addr(tx_addr, rx_offset=0x8): if tx_addr in FUNCTIONAL_ADDRS: return None @@ -551,15 +566,16 @@ def get_rx_addr_for_tx_addr(tx_addr, rx_offset=0x8): class UdsClient(): - def __init__(self, panda, tx_addr: int, rx_addr: int = None, bus: int = 0, timeout: float = 1, debug: bool = False, - tx_timeout: float = 1, response_pending_timeout: float = 10): + def __init__(self, panda, tx_addr: int, rx_addr: int = None, bus: int = 0, sub_addr: int = None, timeout: float = 1, + debug: bool = False, tx_timeout: float = 1, response_pending_timeout: float = 10): self.bus = bus self.tx_addr = tx_addr self.rx_addr = rx_addr if rx_addr is not None else get_rx_addr_for_tx_addr(tx_addr) + self.sub_addr = sub_addr self.timeout = timeout self.debug = debug can_send_with_timeout = partial(panda.can_send, timeout=int(tx_timeout*1000)) - self._can_client = CanClient(can_send_with_timeout, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, debug=self.debug) + self._can_client = CanClient(can_send_with_timeout, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, self.sub_addr, debug=self.debug) self.response_pending_timeout = response_pending_timeout # generic uds request @@ -571,7 +587,8 @@ class UdsClient(): req += data # send request, wait for response - isotp_msg = IsoTpMessage(self._can_client, timeout=self.timeout, debug=self.debug) + max_len = 8 if self.sub_addr is None else 7 + isotp_msg = IsoTpMessage(self._can_client, timeout=self.timeout, debug=self.debug, max_len=max_len) isotp_msg.send(req) response_pending = False while True: diff --git a/panda/python/usb.py b/panda/python/usb.py index 179ebe53c..2e236a999 100644 --- a/panda/python/usb.py +++ b/panda/python/usb.py @@ -1,6 +1,8 @@ +import struct from typing import List -from .base import BaseHandle +from .base import BaseHandle, BaseSTBootloaderHandle, TIMEOUT +from .constants import McuType class PandaUsbHandle(BaseHandle): def __init__(self, libusb_handle): @@ -9,15 +11,85 @@ class PandaUsbHandle(BaseHandle): def close(self): self._libusb_handle.close() - def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0): + def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT): return self._libusb_handle.controlWrite(request_type, request, value, index, data, timeout) - def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0): + def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = TIMEOUT): return self._libusb_handle.controlRead(request_type, request, value, index, length, timeout) - def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int: + def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int: return self._libusb_handle.bulkWrite(endpoint, data, timeout) # type: ignore - def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes: + def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes: return self._libusb_handle.bulkRead(endpoint, length, timeout) # type: ignore + + +class STBootloaderUSBHandle(BaseSTBootloaderHandle): + DFU_DNLOAD = 1 + DFU_UPLOAD = 2 + DFU_GETSTATUS = 3 + DFU_CLRSTATUS = 4 + DFU_ABORT = 6 + + def __init__(self, libusb_device, libusb_handle): + self._libusb_handle = libusb_handle + + # TODO: Find a way to detect F4 vs F2 + # TODO: also check F4 BCD, don't assume in else + self._mcu_type = McuType.H7 if libusb_device.getbcdDevice() == 512 else McuType.F4 + + def _status(self) -> None: + while 1: + dat = self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6) + if dat[1] == 0: + break + + def _erase_page_address(self, address: int) -> None: + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x41" + struct.pack("I", address)) + self._status() + + def get_mcu_type(self): + return self._mcu_type + + def erase_app(self): + self._erase_page_address(self._mcu_type.config.app_address) + + def erase_bootstub(self): + self._erase_page_address(self._mcu_type.config.bootstub_address) + + def clear_status(self): + # Clear status + stat = self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6) + if stat[4] == 0xa: + self._libusb_handle.controlRead(0x21, self.DFU_CLRSTATUS, 0, 0, 0) + elif stat[4] == 0x9: + self._libusb_handle.controlWrite(0x21, self.DFU_ABORT, 0, 0, b"") + self._status() + stat = str(self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6)) + + def close(self): + self._libusb_handle.close() + + def program(self, address, dat): + # Set Address Pointer + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address)) + self._status() + + # Program + bs = self._mcu_type.config.block_size + dat += b"\xFF" * ((bs - len(dat)) % bs) + for i in range(0, len(dat) // bs): + ldat = dat[i * bs:(i + 1) * bs] + print("programming %d with length %d" % (i, len(ldat))) + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 2 + i, 0, ldat) + self._status() + + def jump(self, address): + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address)) + self._status() + try: + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 2, 0, b"") + _ = str(self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6)) + except Exception: + pass diff --git a/release/check-submodules.sh b/release/check-submodules.sh index bc85a43c5..5f4e307e4 100755 --- a/release/check-submodules.sh +++ b/release/check-submodules.sh @@ -1,7 +1,7 @@ #!/bin/bash while read hash submodule ref; do - git -C $submodule fetch --depth 200 origin master + git -C $submodule fetch --depth 1000 origin master git -C $submodule branch -r --contains $hash | grep "origin/master" if [ "$?" -eq 0 ]; then echo "$submodule ok" diff --git a/release/files_common b/release/files_common index 5b310d5c8..2fcbf90c9 100644 --- a/release/files_common +++ b/release/files_common @@ -219,14 +219,15 @@ system/hardware/pc/__init__.py system/hardware/pc/hardware.h system/hardware/pc/hardware.py -selfdrive/locationd/__init__.py -selfdrive/locationd/.gitignore -selfdrive/locationd/SConscript -selfdrive/locationd/ubloxd.cc -selfdrive/locationd/ublox_msg.cc -selfdrive/locationd/ublox_msg.h -selfdrive/locationd/generated/* +system/ubloxd/.gitignore +system/ubloxd/SConscript +system/ubloxd/generated/* +system/ubloxd/*.h +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 @@ -253,35 +254,36 @@ system/proclogd/main.cc system/proclogd/proclog.cc system/proclogd/proclog.h -selfdrive/loggerd/.gitignore -selfdrive/loggerd/SConscript -selfdrive/loggerd/encoder/encoder.cc -selfdrive/loggerd/encoder/encoder.h -selfdrive/loggerd/encoder/v4l_encoder.cc -selfdrive/loggerd/encoder/v4l_encoder.h -selfdrive/loggerd/video_writer.cc -selfdrive/loggerd/video_writer.h -selfdrive/loggerd/logger.cc -selfdrive/loggerd/logger.h -selfdrive/loggerd/loggerd.cc -selfdrive/loggerd/loggerd.h -selfdrive/loggerd/encoderd.cc -selfdrive/loggerd/bootlog.cc -selfdrive/loggerd/encoder/ffmpeg_encoder.cc -selfdrive/loggerd/encoder/ffmpeg_encoder.h +system/loggerd/.gitignore +system/loggerd/SConscript +system/loggerd/encoder/encoder.cc +system/loggerd/encoder/encoder.h +system/loggerd/encoder/v4l_encoder.cc +system/loggerd/encoder/v4l_encoder.h +system/loggerd/video_writer.cc +system/loggerd/video_writer.h +system/loggerd/logger.cc +system/loggerd/logger.h +system/loggerd/loggerd.cc +system/loggerd/loggerd.h +system/loggerd/encoderd.cc +system/loggerd/bootlog.cc +system/loggerd/encoder/ffmpeg_encoder.cc +system/loggerd/encoder/ffmpeg_encoder.h -selfdrive/loggerd/__init__.py -selfdrive/loggerd/config.py -selfdrive/loggerd/uploader.py -selfdrive/loggerd/deleter.py -selfdrive/loggerd/xattr_cache.py +system/loggerd/__init__.py +system/loggerd/config.py +system/loggerd/uploader.py +system/loggerd/deleter.py +system/loggerd/xattr_cache.py -selfdrive/sensord/SConscript -selfdrive/sensord/sensors_qcom2.cc -selfdrive/sensord/sensors/*.cc -selfdrive/sensord/sensors/*.h -selfdrive/sensord/sensord -selfdrive/sensord/pigeond.py +system/sensord/.gitignore +system/sensord/SConscript +system/sensord/sensors_qcom2.cc +system/sensord/sensors/*.cc +system/sensord/sensors/*.h +system/sensord/sensord +system/sensord/pigeond.py selfdrive/thermald/thermald.py selfdrive/thermald/power_monitoring.py @@ -394,9 +396,9 @@ selfdrive/modeld/runners/run.h selfdrive/monitoring/dmonitoringd.py selfdrive/monitoring/driver_monitor.py +selfdrive/navd/.gitignore selfdrive/navd/__init__.py -selfdrive/navd/navd.py -selfdrive/navd/helpers.py +selfdrive/navd/** selfdrive/assets/.gitignore selfdrive/assets/assets.qrc @@ -409,6 +411,7 @@ selfdrive/assets/images/* selfdrive/assets/offroad/* selfdrive/assets/sounds/* selfdrive/assets/training/* +selfdrive/assets/navigation/* third_party/.gitignore third_party/SConscript @@ -577,13 +580,14 @@ opendbc/tesla_can.dbc opendbc/tesla_radar.dbc opendbc/tesla_powertrain.dbc -tinygrad_repo/accel/opencl/* -tinygrad_repo/tinygrad/llops/ops_opencl.py tinygrad_repo/openpilot/compile.py tinygrad_repo/extra/onnx.py +tinygrad_repo/extra/onnx_ops.py tinygrad_repo/extra/thneed.py tinygrad_repo/extra/utils.py -tinygrad_repo/tinygrad/llops/ops_gpu.py -tinygrad_repo/tinygrad/shape/* +tinygrad_repo/tinygrad/codegen/ast.py +tinygrad_repo/tinygrad/codegen/gpu.py tinygrad_repo/tinygrad/nn/* +tinygrad_repo/tinygrad/runtime/ops_gpu.py +tinygrad_repo/tinygrad/shape/* tinygrad_repo/tinygrad/*.py diff --git a/release/files_tici b/release/files_tici index c8abd720d..892b7cd2f 100644 --- a/release/files_tici +++ b/release/files_tici @@ -4,16 +4,13 @@ third_party/mapbox-gl-native-qt/include/* system/timezoned.py -selfdrive/assets/navigation/* -selfdrive/assets/training_wide/* - system/camerad/cameras/camera_qcom2.cc system/camerad/cameras/camera_qcom2.h system/camerad/cameras/camera_util.cc system/camerad/cameras/camera_util.h system/camerad/cameras/real_debayer.cl -selfdrive/sensord/rawgps/* +system/sensord/rawgps/* selfdrive/ui/qt/spinner_larch64 selfdrive/ui/qt/text_larch64 diff --git a/selfdrive/assets/img_driver_face.png b/selfdrive/assets/img_driver_face.png index 03765a037..e2d943e53 100644 Binary files a/selfdrive/assets/img_driver_face.png and b/selfdrive/assets/img_driver_face.png differ diff --git a/selfdrive/assets/img_driver_face_static.png b/selfdrive/assets/img_driver_face_static.png new file mode 100644 index 000000000..d8bc5f137 Binary files /dev/null and b/selfdrive/assets/img_driver_face_static.png differ diff --git a/selfdrive/assets/training/step0.png b/selfdrive/assets/training/step0.png index b942703b5..3c2c5c72a 100644 Binary files a/selfdrive/assets/training/step0.png and b/selfdrive/assets/training/step0.png differ diff --git a/selfdrive/assets/training/step1.png b/selfdrive/assets/training/step1.png index e2c9f9f60..085789311 100644 Binary files a/selfdrive/assets/training/step1.png and b/selfdrive/assets/training/step1.png differ diff --git a/selfdrive/assets/training/step10.png b/selfdrive/assets/training/step10.png index c5ed8fd62..2941316d1 100644 Binary files a/selfdrive/assets/training/step10.png and b/selfdrive/assets/training/step10.png differ diff --git a/selfdrive/assets/training/step11.png b/selfdrive/assets/training/step11.png index 477659392..7a7c72e3d 100644 Binary files a/selfdrive/assets/training/step11.png and b/selfdrive/assets/training/step11.png differ diff --git a/selfdrive/assets/training/step12.png b/selfdrive/assets/training/step12.png index 497170c97..0d6f64eb8 100644 Binary files a/selfdrive/assets/training/step12.png and b/selfdrive/assets/training/step12.png differ diff --git a/selfdrive/assets/training/step13.png b/selfdrive/assets/training/step13.png index 228d7549d..565e02fa3 100644 Binary files a/selfdrive/assets/training/step13.png and b/selfdrive/assets/training/step13.png differ diff --git a/selfdrive/assets/training/step14.png b/selfdrive/assets/training/step14.png index 7f8da0552..225231cba 100644 Binary files a/selfdrive/assets/training/step14.png and b/selfdrive/assets/training/step14.png differ diff --git a/selfdrive/assets/training/step15.png b/selfdrive/assets/training/step15.png index 9aa861c9f..929c759b2 100644 Binary files a/selfdrive/assets/training/step15.png and b/selfdrive/assets/training/step15.png differ diff --git a/selfdrive/assets/training/step16.png b/selfdrive/assets/training/step16.png index e0b36b033..161af863a 100644 Binary files a/selfdrive/assets/training/step16.png and b/selfdrive/assets/training/step16.png differ diff --git a/selfdrive/assets/training/step17.png b/selfdrive/assets/training/step17.png index c6b33c237..1b0cdb6fb 100644 Binary files a/selfdrive/assets/training/step17.png and b/selfdrive/assets/training/step17.png differ diff --git a/selfdrive/assets/training/step18.png b/selfdrive/assets/training/step18.png index bd062d4cc..0e3b64bab 100644 Binary files a/selfdrive/assets/training/step18.png and b/selfdrive/assets/training/step18.png differ diff --git a/selfdrive/assets/training/step2.png b/selfdrive/assets/training/step2.png index 97c2eb0f4..55814b8ef 100644 Binary files a/selfdrive/assets/training/step2.png and b/selfdrive/assets/training/step2.png differ diff --git a/selfdrive/assets/training/step3.png b/selfdrive/assets/training/step3.png index 748972231..831095b0a 100644 Binary files a/selfdrive/assets/training/step3.png and b/selfdrive/assets/training/step3.png differ diff --git a/selfdrive/assets/training/step4.png b/selfdrive/assets/training/step4.png index 8139349ff..543303493 100644 Binary files a/selfdrive/assets/training/step4.png and b/selfdrive/assets/training/step4.png differ diff --git a/selfdrive/assets/training/step5.png b/selfdrive/assets/training/step5.png index 714162ae1..7191b63a0 100644 Binary files a/selfdrive/assets/training/step5.png and b/selfdrive/assets/training/step5.png differ diff --git a/selfdrive/assets/training/step6.png b/selfdrive/assets/training/step6.png index 356d76a3e..8eafd4a19 100644 Binary files a/selfdrive/assets/training/step6.png and b/selfdrive/assets/training/step6.png differ diff --git a/selfdrive/assets/training/step7.png b/selfdrive/assets/training/step7.png index ac09faffe..502f5f1b2 100644 Binary files a/selfdrive/assets/training/step7.png and b/selfdrive/assets/training/step7.png differ diff --git a/selfdrive/assets/training/step8.png b/selfdrive/assets/training/step8.png index f081ac6e4..77ff9d736 100644 Binary files a/selfdrive/assets/training/step8.png and b/selfdrive/assets/training/step8.png differ diff --git a/selfdrive/assets/training/step9.png b/selfdrive/assets/training/step9.png index 540dafe78..84eae3a06 100644 Binary files a/selfdrive/assets/training/step9.png and b/selfdrive/assets/training/step9.png differ diff --git a/selfdrive/assets/training_wide/step0.png b/selfdrive/assets/training_wide/step0.png deleted file mode 100644 index 3c2c5c72a..000000000 Binary files a/selfdrive/assets/training_wide/step0.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step1.png b/selfdrive/assets/training_wide/step1.png deleted file mode 100644 index 085789311..000000000 Binary files a/selfdrive/assets/training_wide/step1.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step10.png b/selfdrive/assets/training_wide/step10.png deleted file mode 100644 index 2941316d1..000000000 Binary files a/selfdrive/assets/training_wide/step10.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step11.png b/selfdrive/assets/training_wide/step11.png deleted file mode 100644 index 7a7c72e3d..000000000 Binary files a/selfdrive/assets/training_wide/step11.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step12.png b/selfdrive/assets/training_wide/step12.png deleted file mode 100644 index 0d6f64eb8..000000000 Binary files a/selfdrive/assets/training_wide/step12.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step13.png b/selfdrive/assets/training_wide/step13.png deleted file mode 100644 index 565e02fa3..000000000 Binary files a/selfdrive/assets/training_wide/step13.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step14.png b/selfdrive/assets/training_wide/step14.png deleted file mode 100644 index 225231cba..000000000 Binary files a/selfdrive/assets/training_wide/step14.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step15.png b/selfdrive/assets/training_wide/step15.png deleted file mode 100644 index 929c759b2..000000000 Binary files a/selfdrive/assets/training_wide/step15.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step16.png b/selfdrive/assets/training_wide/step16.png deleted file mode 100644 index 161af863a..000000000 Binary files a/selfdrive/assets/training_wide/step16.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step17.png b/selfdrive/assets/training_wide/step17.png deleted file mode 100644 index 1b0cdb6fb..000000000 Binary files a/selfdrive/assets/training_wide/step17.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step18.png b/selfdrive/assets/training_wide/step18.png deleted file mode 100644 index 0e3b64bab..000000000 Binary files a/selfdrive/assets/training_wide/step18.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step2.png b/selfdrive/assets/training_wide/step2.png deleted file mode 100644 index 55814b8ef..000000000 Binary files a/selfdrive/assets/training_wide/step2.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step3.png b/selfdrive/assets/training_wide/step3.png deleted file mode 100644 index 831095b0a..000000000 Binary files a/selfdrive/assets/training_wide/step3.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step4.png b/selfdrive/assets/training_wide/step4.png deleted file mode 100644 index 543303493..000000000 Binary files a/selfdrive/assets/training_wide/step4.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step5.png b/selfdrive/assets/training_wide/step5.png deleted file mode 100644 index 7191b63a0..000000000 Binary files a/selfdrive/assets/training_wide/step5.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step6.png b/selfdrive/assets/training_wide/step6.png deleted file mode 100644 index 8eafd4a19..000000000 Binary files a/selfdrive/assets/training_wide/step6.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step7.png b/selfdrive/assets/training_wide/step7.png deleted file mode 100644 index 502f5f1b2..000000000 Binary files a/selfdrive/assets/training_wide/step7.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step8.png b/selfdrive/assets/training_wide/step8.png deleted file mode 100644 index c4e866833..000000000 Binary files a/selfdrive/assets/training_wide/step8.png and /dev/null differ diff --git a/selfdrive/assets/training_wide/step9.png b/selfdrive/assets/training_wide/step9.png deleted file mode 100644 index 84eae3a06..000000000 Binary files a/selfdrive/assets/training_wide/step9.png and /dev/null differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index d1cc4cea8..8e335c2b5 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -36,8 +36,8 @@ from common.file_helpers import CallbackReader from common.params import Params from common.realtime import sec_since_boot, set_core_affinity from system.hardware import HARDWARE, PC, AGNOS -from selfdrive.loggerd.config import ROOT -from selfdrive.loggerd.xattr_cache import getxattr, setxattr +from system.loggerd.config import ROOT +from system.loggerd.xattr_cache import getxattr, setxattr from selfdrive.statsd import STATS_DIR from system.swaglog import SWAGLOG_DIR, cloudlog from system.version import get_commit, get_origin, get_short_branch, get_version @@ -517,6 +517,11 @@ def getSshAuthorizedKeys() -> str: return Params().get("GithubSshKeys", encoding='utf8') or '' +@dispatcher.add_method +def getGithubUsername() -> str: + return Params().get("GithubUsername", encoding='utf8') or '' + + @dispatcher.add_method def getSimInfo(): return HARDWARE.get_sim_info() diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index d99e67a9f..2fe459148 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,9 +1,11 @@ Import('env', 'envCython', 'common', 'cereal', 'messaging') libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] -env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=libs) +panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) + +env.Program('boardd', ['main.cc', 'boardd.cc'], LIBS=[panda] + libs) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) if GetOption('test'): - env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=libs) + env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc'], LIBS=[panda] + libs) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 5d885c2c7..6248da7d4 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -113,32 +113,35 @@ bool safety_setter_thread(std::vector pandas) { return false; } - // set to ELM327 for fingerprinting + // initialize to ELM327 without OBD multiplexing for fingerprinting + bool obd_multiplexing_enabled = false; for (int i = 0; i < pandas.size(); i++) { - const uint16_t safety_param = (i > 0) ? 1U : 0U; - pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); + pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); } - // wait for FW query at OBD port to finish + // openpilot can switch between multiplexing modes for different FW queries while (true) { if (do_exit || !check_all_connected(pandas) || !ignition) { return false; } - if (p.getBool("FirmwareObdQueryDone")) { - LOGW("finished FW query at OBD port"); + bool obd_multiplexing_requested = p.getBool("ObdMultiplexingEnabled"); + if (obd_multiplexing_requested != obd_multiplexing_enabled) { + for (int i = 0; i < pandas.size(); i++) { + const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U; + pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); + } + obd_multiplexing_enabled = obd_multiplexing_requested; + p.putBool("ObdMultiplexingChanged", true); + } + + if (p.getBool("FirmwareQueryDone")) { + LOGW("finished FW query"); break; } util::sleep_for(20); } - // set to ELM327 to finish fingerprinting and for potential ECU knockouts - for (Panda *panda : pandas) { - panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); - } - - p.putBool("ObdMultiplexingDisabled", true); - std::string params; LOGW("waiting for params to set safety model"); while (true) { @@ -229,6 +232,8 @@ void can_send_thread(std::vector pandas, bool fake_send) { panda->can_send(event.getSendcan()); LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str()); } + } else { + LOGE("sendcan too old to send: %llu, %llu", nanos_since_boot(), event.getLogMonoTime()); } } } @@ -360,6 +365,8 @@ std::optional send_panda_states(PubMaster *pm, const std::vector } auto ps = pss[i]; + ps.setVoltage(health.voltage_pkt); + ps.setCurrent(health.current_pkt); ps.setUptime(health.uptime_pkt); ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt); ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt); @@ -380,7 +387,11 @@ std::optional send_panda_states(PubMaster *pm, const std::vector ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); ps.setInterruptLoad(health.interrupt_load); ps.setFanPower(health.fan_power); + ps.setFanStallCount(health.fan_stall_count); ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid)); + ps.setSpiChecksumErrorCount(health.spi_checksum_error_count); + ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); + ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); std::array cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; @@ -415,7 +426,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector size_t j = 0; for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION); - f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_EXTI); f++) { + f <= size_t(cereal::PandaState::FaultType::HEARTBEAT_LOOP_WATCHDOG); f++) { if (fault_bits.test(f)) { faults.set(j, cereal::PandaState::FaultType(f)); j++; @@ -457,7 +468,8 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin SubMaster sm({"controlsState"}); Panda *peripheral_panda = pandas[0]; - bool ignition_last = false; + bool is_onroad = false; + bool is_onroad_last = false; std::future safety_future; LOGD("start panda state thread"); @@ -476,27 +488,40 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin ignition = *ignition_opt; - // TODO: make this check fast, currently takes 16ms - // check if we have new pandas and are offroad - if (!ignition && (pandas.size() != Panda::list().size())) { - LOGW("Reconnecting to changed amount of pandas!"); - do_exit = true; - break; + // check if we should have pandad reconnect + if (!ignition) { + bool comms_healthy = true; + for (const auto &panda : pandas) { + comms_healthy &= panda->comms_healthy(); + } + + if (!comms_healthy) { + LOGE("Reconnecting, communication to pandas not healthy"); + do_exit = true; + + // TODO: list is slow, takes 16ms + } else if (pandas.size() != Panda::list().size()) { + LOGW("Reconnecting to changed amount of pandas!"); + do_exit = true; + } + + if (do_exit) { + break; + } } - // clear ignition-based params and set new safety on car start - if (ignition && !ignition_last) { - params.clearAll(CLEAR_ON_IGNITION_ON); + is_onroad = params.getBool("IsOnroad"); + + // set new safety on onroad transition, after params are cleared + if (is_onroad && !is_onroad_last) { if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) { safety_future = std::async(std::launch::async, safety_setter_thread, pandas); } else { LOGW("Safety setter thread already running"); } - } else if (!ignition && ignition_last) { - params.clearAll(CLEAR_ON_IGNITION_OFF); } - ignition_last = ignition; + is_onroad_last = is_onroad; sm.update(0); const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled(); diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 647a0d9c7..980a31b83 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -10,20 +10,11 @@ #include "common/util.h" Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { - // try USB first, then SPI - try { - handle = std::make_unique(serial); - } catch (std::exception &e) { -#ifndef __APPLE__ - handle = std::make_unique(serial); -#endif - } + handle = std::make_unique(serial); + LOGW("conntected to %s over USB", serial.c_str()); hw_type = get_hw_type(); - assert((hw_type != cereal::PandaState::PandaType::WHITE_PANDA) && - (hw_type != cereal::PandaState::PandaType::GREY_PANDA)); - has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) || (hw_type == cereal::PandaState::PandaType::DOS) || (hw_type == cereal::PandaState::PandaType::TRES); @@ -48,7 +39,7 @@ std::string Panda::hw_serial() { std::vector Panda::list() { std::vector serials = PandaUsbHandle::list(); -#ifndef __APPLE__ +#if 0 for (auto s : PandaSpiHandle::list()) { if (std::find(serials.begin(), serials.end(), s) == serials.end()) { serials.push_back(s); @@ -203,7 +194,7 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data assert(can_data.size() <= 64); assert(can_data.size() == dlc_to_len[data_len_code]); - can_header header; + can_header header = {}; header.addr = cmsg.getAddress(); header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; header.data_len_code = data_len_code; diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index 506b96b37..f93d29217 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -1,8 +1,9 @@ #pragma once -#include #include #include +#include +#include #include #ifndef __APPLE__ diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index f61d9ee1a..50027f340 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -26,7 +26,7 @@ def flash_panda(panda_serial: str) -> Panda: panda = Panda(panda_serial) fw_signature = get_expected_signature(panda) - internal_panda = panda.is_internal() and not panda.bootstub + internal_panda = panda.is_internal() panda_version = "bootstub" if panda.bootstub else panda.get_version() panda_signature = b"" if panda.bootstub else panda.get_signature() @@ -39,7 +39,7 @@ def flash_panda(panda_serial: str) -> Panda: if panda.bootstub: bootstub_version = panda.get_version() - cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. Bootstub version: {bootstub_version}") + cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. {bootstub_version=}, {internal_panda=}") if internal_panda: HARDWARE.recover_internal_panda() panda.recover(reset=(not internal_panda)) @@ -76,23 +76,28 @@ def panda_sort_cmp(a: Panda, b: Panda): def main() -> NoReturn: + count = 0 first_run = True params = Params() while True: try: + count += 1 + cloudlog.event("pandad.flash_and_connect", count=count) params.remove("PandaSignatures") # Flash all Pandas in DFU mode - for p in PandaDFU.list(): - cloudlog.info(f"Panda in DFU mode found, flashing recovery {p}") - PandaDFU(p).recover() - time.sleep(1) + dfu_serials = PandaDFU.list() + if len(dfu_serials) > 0: + for serial in dfu_serials: + cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}") + PandaDFU(serial).recover() + time.sleep(1) panda_serials = Panda.list() if len(panda_serials) == 0: if first_run: - cloudlog.info("Resetting internal panda") + cloudlog.info("No pandas found, resetting internal panda") HARDWARE.reset_internal_panda() time.sleep(2) # wait to come back up continue @@ -104,16 +109,13 @@ def main() -> NoReturn: for serial in panda_serials: pandas.append(flash_panda(serial)) - # check health for lost heartbeat - for panda in pandas: - health = panda.health() - if health["heartbeat_lost"]: - params.put_bool("PandaHeartbeatLost", True) - cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) - - if first_run: - cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") - panda.reset() + # Ensure internal panda is present if expected + internal_pandas = [panda for panda in pandas if panda.is_internal()] + if HARDWARE.has_internal_panda() and len(internal_pandas) == 0: + cloudlog.error("Internal panda is missing, resetting") + HARDWARE.reset_internal_panda() + time.sleep(2) # wait to come back up + continue # sort pandas to have deterministic order pandas.sort(key=cmp_to_key(panda_sort_cmp)) @@ -122,13 +124,30 @@ def main() -> NoReturn: # log panda fw versions params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) - # close all pandas + for panda in pandas: + # check health for lost heartbeat + health = panda.health() + if health["heartbeat_lost"]: + params.put_bool("PandaHeartbeatLost", True) + cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) + + if first_run: + cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") + if panda.is_internal(): + HARDWARE.reset_internal_panda() + else: + panda.reset(reconnect=False) + for p in pandas: p.close() + # TODO: wrap all panda exceptions in a base panda exception except (usb1.USBErrorNoDevice, usb1.USBErrorPipe): # a panda was disconnected while setting everything up. let's try again cloudlog.exception("Panda USB exception while setting up") continue + except Exception: + cloudlog.exception("pandad.uncaught_exception") + continue first_run = False diff --git a/selfdrive/boardd/set_time.py b/selfdrive/boardd/set_time.py index 7d17be4de..2159eba5e 100755 --- a/selfdrive/boardd/set_time.py +++ b/selfdrive/boardd/set_time.py @@ -1,11 +1,9 @@ #!/usr/bin/env python3 -import datetime import os -import struct -import usb1 +import datetime +from panda import Panda -REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE -MIN_DATE = datetime.datetime(year=2021, month=4, day=1) +MIN_DATE = datetime.datetime(year=2023, month=4, day=1) def set_time(logger): sys_time = datetime.datetime.today() @@ -14,24 +12,27 @@ def set_time(logger): return try: - ctx = usb1.USBContext() - dev = ctx.openByVendorIDAndProductID(0xbbaa, 0xddcc) - if dev is None: - logger.info("No panda found") + ps = Panda.list() + if len(ps) == 0: + logger.error("Failed to set time, no pandas found") return - # Set system time from panda RTC time - dat = dev.controlRead(REQUEST_IN, 0xa0, 0, 0, 8) - a = struct.unpack("HBBBBBB", dat) - panda_time = datetime.datetime(a[0], a[1], a[2], a[4], a[5], a[6]) - if panda_time > MIN_DATE: - logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'") - os.system(f"TZ=UTC date -s '{panda_time}'") + for s in ps: + with Panda(serial=s) as p: + if not p.is_internal(): + continue + + # Set system time from panda RTC time + panda_time = p.get_datetime() + if panda_time > MIN_DATE: + logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'") + os.system(f"TZ=UTC date -s '{panda_time}'") + break except Exception: - logger.warn("Failed to fetch time from panda") + logger.exception("Failed to fetch time from panda") if __name__ == "__main__": import logging - logging.basicConfig(level=logging.INFO) + logging.basicConfig(level=logging.DEBUG) set_time(logging) diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index 9a10e30f9..82a95f775 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -57,9 +57,12 @@ PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { uint8_t uid[uid_len] = {0}; uint32_t spi_mode = SPI_MODE_0; - uint32_t spi_speed = 30000000; uint8_t spi_bits_per_word = 8; + // 50MHz is the max of the 845. note that some older + // revs of the comma three may not support this speed + uint32_t spi_speed = 50000000; + spi_fd = open(SPI_DEVICE.c_str(), O_RDWR); if (spi_fd < 0) { LOGE("failed opening SPI device %d", spi_fd); @@ -280,7 +283,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx } // Wait for (N)ACK - tx_buf[0] = 0x12; + tx_buf[0] = 0x11; transfer.len = 1; ret = wait_for_ack(transfer, SPI_HACK); if (ret < 0) { @@ -300,7 +303,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx } // Wait for (N)ACK - tx_buf[0] = 0xab; + tx_buf[0] = 0x13; transfer.len = 1; ret = wait_for_ack(transfer, SPI_DACK); if (ret < 0) { diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index b8ebbd88a..4be5b3f7e 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -6,7 +6,7 @@ import unittest from collections import defaultdict import cereal.messaging as messaging -from cereal import car +from cereal import car, log from common.params import Params from common.spinner import Spinner from common.timeout import Timeout @@ -31,33 +31,35 @@ class TestBoardd(unittest.TestCase): @phone_only @with_processes(['pandad']) def test_loopback(self): - # wait for boardd to init - time.sleep(2) + params = Params() + params.put_bool("IsOnroad", False) with Timeout(60, "boardd didn't start"): sm = messaging.SubMaster(['pandaStates']) - while sm.rcv_frame['pandaStates'] < 1 and len(sm['pandaStates']) == 0: + while sm.rcv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \ + any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']): sm.update(1000) num_pandas = len(sm['pandaStates']) - if TICI: - self.assertGreater(num_pandas, 1, "connect another panda for multipanda tests") + expected_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1 + self.assertEqual(num_pandas, expected_pandas, "connected pandas ({num_pandas}) doesn't match expected panda count ({expected_pandas}). \ + connect another panda for multipanda tests.") - # boardd blocks on CarVin and CarParams + # boardd safety setting relies on these params cp = car.CarParams.new_message() safety_config = car.CarParams.SafetyConfig.new_message() safety_config.safetyModel = car.CarParams.SafetyModel.allOutput cp.safetyConfigs = [safety_config]*num_pandas - params = Params() - params.put_bool("FirmwareObdQueryDone", True) + params.put_bool("IsOnroad", True) + params.put_bool("FirmwareQueryDone", True) params.put_bool("ControlsReady", True) params.put("CarParams", cp.to_bytes()) sendcan = messaging.pub_sock('sendcan') can = messaging.sub_sock('can', conflate=False, timeout=100) - time.sleep(0.2) + time.sleep(0.5) n = 200 for i in range(n): diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index c056f7ca3..58cde8581 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -73,7 +73,7 @@ def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, st return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc} -def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): +def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): # limits due to driver torque driver_max_torque = LIMITS.STEER_MAX + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER @@ -93,29 +93,37 @@ def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque return int(round(float(apply_torque))) -def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS): - # limits due to comparison of commanded torque VS motor reported torque - max_lim = min(max(motor_torque + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_ERROR_MAX), LIMITS.STEER_MAX) - min_lim = max(min(motor_torque - LIMITS.STEER_ERROR_MAX, -LIMITS.STEER_ERROR_MAX), -LIMITS.STEER_MAX) +def apply_dist_to_meas_limits(val, val_last, val_meas, + STEER_DELTA_UP, STEER_DELTA_DOWN, + STEER_ERROR_MAX, STEER_MAX): + # limits due to comparison of commanded val VS measured val (torque/angle/curvature) + max_lim = min(max(val_meas + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX) + min_lim = max(min(val_meas - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX) - apply_torque = clip(apply_torque, min_lim, max_lim) + val = clip(val, min_lim, max_lim) - # slow rate if steer torque increases in magnitude - if apply_torque_last > 0: - apply_torque = clip(apply_torque, - max(apply_torque_last - LIMITS.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP), - apply_torque_last + LIMITS.STEER_DELTA_UP) + # slow rate if val increases in magnitude + if val_last > 0: + val = clip(val, + max(val_last - STEER_DELTA_DOWN, -STEER_DELTA_UP), + val_last + STEER_DELTA_UP) else: - apply_torque = clip(apply_torque, - apply_torque_last - LIMITS.STEER_DELTA_UP, - min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) + val = clip(val, + val_last - STEER_DELTA_UP, + min(val_last + STEER_DELTA_DOWN, STEER_DELTA_UP)) - return int(round(float(apply_torque))) + return float(val) + + +def apply_meas_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS): + return int(round(apply_dist_to_meas_limits(apply_torque, apply_torque_last, motor_torque, + LIMITS.STEER_DELTA_UP, LIMITS.STEER_DELTA_DOWN, + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_MAX))) def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS): # pick angle rate limits based on wind up/down - steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) + steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last) rate_limits = LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else LIMITS.ANGLE_RATE_LIMIT_DOWN angle_rate_lim = interp(v_ego, rate_limits.speed_bp, rate_limits.angle_v) diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 850a3538a..4d583bada 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -8,7 +8,7 @@ from selfdrive.car.body.values import SPEED_FROM_RPM class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.notCar = True ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 370772c90..3d8ea22ef 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -8,7 +8,7 @@ from system.version import is_comma_remote, is_tested_branch from selfdrive.car.interfaces import get_interface_attr from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN -from selfdrive.car.fw_versions import disable_obd_multiplexing, get_fw_versions_ordered, match_fw_to_car, get_present_ecus +from selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing from system.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint @@ -80,12 +80,13 @@ def fingerprint(logcan, sendcan, num_pandas): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) ecu_rx_addrs = set() + params = Params() if not skip_fw_query: # Vin query only reliably works through OBDII bus = 1 - cached_params = Params().get("CarParamsCache") + cached_params = params.get("CarParamsCache") if cached_params is not None: cached_params = car.CarParams.from_bytes(cached_params) if cached_params.carName == "mock": @@ -98,6 +99,7 @@ def fingerprint(logcan, sendcan, num_pandas): cached = True else: cloudlog.warning("Getting VIN & FW versions") + set_obd_multiplexing(params, True) vin_rx_addr, vin = get_vin(logcan, sendcan, bus) ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas) car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas) @@ -113,10 +115,11 @@ def fingerprint(logcan, sendcan, num_pandas): cloudlog.event("Malformed VIN", vin=vin, error=True) vin = VIN_UNKNOWN cloudlog.warning("VIN %s", vin) - - params = Params() params.put("CarVin", vin) - disable_obd_multiplexing(params) + + # disable OBD multiplexing for potential ECU knockouts + set_obd_multiplexing(params, False) + params.put_bool("FirmwareQueryDone", True) finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 @@ -180,11 +183,11 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: - cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) + cloudlog.event("car doesn't match any fingerprints", fingerprints=fingerprints, error=True) candidate = "mock" CarInterface, CarController, CarState = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed) + CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 20a44bce2..b418179e0 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,6 +1,6 @@ from opendbc.can.packer import CANPacker from common.realtime import DT_CTRL -from selfdrive.car import apply_toyota_steer_torque_limits +from selfdrive.car import apply_meas_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons from selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags @@ -67,7 +67,7 @@ class CarController: # steer torque new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params) + apply_steer = apply_meas_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params) if not lkas_active or not lkas_control_bit: apply_steer = 0 self.apply_steer_last = apply_steer diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 961684f39..22b207388 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -8,10 +8,11 @@ from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "chrysler" ret.dashcamOnly = candidate in RAM_HD + # radar parsing needs some work, see https://github.com/commaai/openpilot/issues/26842 ret.radarUnavailable = True # DBC[candidate]['radar'] is None ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 16ebb4fa1..17ba59f96 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,12 +1,11 @@ from enum import IntFlag from dataclasses import dataclass -from enum import Enum from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -50,6 +49,7 @@ class CarControllerParams: self.STEER_DELTA_DOWN = 3 self.STEER_MAX = 261 # higher than this faults the EPS + STEER_THRESHOLD = 120 RAM_DT = {CAR.RAM_1500, } @@ -60,12 +60,13 @@ RAM_CARS = RAM_DT | RAM_HD @dataclass class ChryslerCarInfo(CarInfo): package: str = "Adaptive Cruise Control (ACC)" - harness: Enum = Harness.fca + harness_kit: HarnessKit = HarnessKit(Harness.fca) + CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"), CAR.PACIFICA_2018_HYBRID: None, # same platforms - CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-22"), + CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-23"), CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"), CAR.PACIFICA_2020: [ ChryslerCarInfo("Chrysler Pacifica 2019-20"), @@ -73,10 +74,10 @@ CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { ], CAR.JEEP_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"), CAR.JEEP_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"), - CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-22", harness=Harness.ram), + CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-23", harness_kit=HarnessKit(Harness.ram)), CAR.RAM_HD: [ - ChryslerCarInfo("Ram 2500 2020-22", harness=Harness.ram), - ChryslerCarInfo("Ram 3500 2020-22", harness=Harness.ram), + ChryslerCarInfo("Ram 2500 2020-22", harness_kit=HarnessKit(Harness.ram)), + ChryslerCarInfo("Ram 3500 2019-22", harness_kit=HarnessKit(Harness.ram)), ], } @@ -93,13 +94,13 @@ CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { FINGERPRINTS = { CAR.PACIFICA_2017_HYBRID: [{ - 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788:3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8 + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 840: 8, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8 }], CAR.PACIFICA_2018: [{ 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 }, { - 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 + 55: 8, 58: 6, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 956: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 }], CAR.PACIFICA_2020: [{ 55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8 @@ -125,6 +126,9 @@ FINGERPRINTS = { # Based on "8190c7275a24557b|2020-02-24--09-57-23" { 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1536: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + }, + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 450: 8, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2018: 8, 2020: 8, 2026: 8, 2028: 8 }], CAR.JEEP_CHEROKEE: [{ 55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 @@ -135,7 +139,7 @@ FINGERPRINTS = { }], CAR.JEEP_CHEROKEE_2019: [{ # Jeep Grand Cherokee 2019, including most 2020 models - 55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + 55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1538: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }], } @@ -180,79 +184,158 @@ FW_QUERY_CONFIG = FwQueryConfig( ) FW_VERSIONS = { - CAR.RAM_1500: { + CAR.JEEP_CHEROKEE_2019: { (Ecu.combinationMeter, 0x742, None): [ - b'68294063AH', - b'68294063AG', - b'68434860AC', - b'68527375AD', - b'68453503AC', + b'68402971AD', ], (Ecu.srs, 0x744, None): [ - b'68441329AB', - b'68490898AA', - b'68428609AB', - b'68500728AA', + b'68355363AB', ], (Ecu.abs, 0x747, None): [ - b'68432418AD', - b'68432418AB', - b'68436004AE', - b'68438454AD', - b'68436004AD', - b'68535469AB', - b'68438454AC', + b'68408639AD', ], (Ecu.fwdRadar, 0x753, None): [ - b'68320950AL', - b'68320950AJ', - b'68454268AB', - b'68475160AG', - b'04672892AB', - b'68475160AE', + b'68456722AC', ], (Ecu.eps, 0x75A, None): [ - b'68273275AG', - b'68469901AA', - b'68552788AA', + b'68453431AA', ], (Ecu.engine, 0x7e0, None): [ - b'68448163AJ', - b'68500630AD', - b'68539650AD', + b'05035674AB ', + ], + (Ecu.transmission, 0x7e1, None): [ + b'05035707AA', + ], + }, + + CAR.RAM_1500: { + (Ecu.combinationMeter, 0x742, None): [ + b'68294051AG', + b'68294051AI', + b'68294052AG', + b'68294063AG', + b'68294063AH', + b'68294063AI', + b'68434846AC', + b'68434858AC', + b'68434860AC', + b'68453503AC', + b'68453505AC', + b'68453511AC', + b'68453513AD', + b'68453514AD', + b'68510283AG', + b'68527375AD', + b'68527346AE', + ], + (Ecu.srs, 0x744, None): [ + b'68428609AB', + b'68441329AB', + b'68473844AB', + b'68490898AA', + b'68500728AA', + b'68615033AA', + ], + (Ecu.abs, 0x747, None): [ + b'68292406AH', + b'68432418AB', + b'68432418AD', + b'68436004AD', + b'68436004AE', + b'68438454AC', + b'68438454AD', + b'68438456AE', + b'68438456AF', + b'68535469AB', + b'68535470AC', + b'68586307AB', + b'68548900AB', + ], + (Ecu.fwdRadar, 0x753, None): [ + b'04672892AB', + b'04672932AB', + b'68320950AH', + b'68320950AI', + b'68320950AJ', + b'68320950AL', + b'68320950AM', + b'68454268AB', + b'68475160AE', + b'68475160AF', + b'68475160AG', + ], + (Ecu.eps, 0x75A, None): [ + b'68273275AF', + b'68273275AG', + b'68312176AE', + b'68312176AG', + b'68440789AC', + b'68466110AB', + b'68469901AA', + b'68522583AB', + b'68522585AB', + b'68552788AA', + b'68552790AA', + b'68585112AB', + b'68552789AA', + ], + (Ecu.engine, 0x7e0, None): [ + b'05036065AE ', + b'05036066AE ', + b'68378701AI ', b'68378758AM ', + b'68448163AJ', + b'68448165AK', + b'68500630AD', + b'68500630AE', + b'68539650AD', + b'05149846AA ', ], (Ecu.transmission, 0x7e1, None): [ b'68360078AL', - b'68384328AD', - b'68360085AL', + b'68360080AM', b'68360081AM', - b'68502994AD', + b'68360085AL', + b'68384328AD', + b'68384332AD', b'68445533AB', - b'68540431AB', b'68484467AC', + b'68502994AD', + b'68540431AB', + b'68520867AE', ], }, CAR.RAM_HD: { (Ecu.combinationMeter, 0x742, None): [ b'68361606AH', + b'68437735AC', b'68492693AD', + b'68525485AB', + b'68525487AB', + b'68525498AB', ], (Ecu.srs, 0x744, None): [ b'68399794AC', b'68428503AA', b'68428505AA', + b'68428507AA', ], (Ecu.abs, 0x747, None): [ b'68334977AH', + b'68455481AC', + b'68504022AA', b'68504022AB', - b'68530686AB', b'68504022AC', + b'68530686AB', + b'68530686AC', ], (Ecu.fwdRadar, 0x753, None): [ b'04672895AB', b'56029827AG', + b'56029827AH', + b'68462657AE', + b'68484694AD', b'68484694AE', ], (Ecu.eps, 0x761, None): [ @@ -260,7 +343,13 @@ FW_VERSIONS = { b'68507906AB', ], (Ecu.engine, 0x7e0, None): [ + b'52370131AF', + b'52370231AF', + b'52370231AG', + b'52370931CT', + b'52401032AE', b'52421132AF', + b'68527616AD ', b'M2370131MB', b'M2421132MB', ], diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py index cd3e93fa8..ed98e14dc 100755 --- a/selfdrive/car/disable_ecu.py +++ b/selfdrive/car/disable_ecu.py @@ -31,8 +31,8 @@ def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01' except Exception: cloudlog.exception("ecu disable exception") - print(f"ecu disable retry ({i+1}) ...") - cloudlog.warning("ecu disable failed") + cloudlog.error(f"ecu disable retry ({i + 1}) ...") + cloudlog.error("ecu disable failed") return False diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index c0fb4420d..bfb182c5e 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -1,5 +1,6 @@ import re from collections import namedtuple +import copy from dataclasses import dataclass, field from enum import Enum from typing import Dict, List, Optional, Tuple, Union @@ -20,7 +21,7 @@ class Column(Enum): FSR_STEERING = "No ALC below" STEERING_TORQUE = "Steering Torque" AUTO_RESUME = "Resume from stop" - HARNESS = "Harness" + HARNESS = "Harness Kit" VIDEO = "Video" @@ -69,6 +70,23 @@ class Harness(Enum): none = "None" +class HarnessPart(Enum): + harness_box = "harness box" + comma_power_v2 = "comma power v2" + rj45_cable = "RJ45 cable (7 ft)" + long_obdc_cable = "long OBD-C cable" + usbc_coupler = "USB-C coupler" + + +DEFAULT_HARNESS_PARTS: List[HarnessPart] = [HarnessPart.harness_box, HarnessPart.comma_power_v2, HarnessPart.rj45_cable] + + +@dataclass +class HarnessKit: + connector: Harness = Harness.none + parts: List[HarnessPart] = field(default_factory=lambda: copy.copy(DEFAULT_HARNESS_PARTS)) + + CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "shop_footnote"], defaults=(False, False)) @@ -117,13 +135,27 @@ def split_name(name: str) -> Tuple[str, str, str]: @dataclass class CarInfo: + # make + model + model years name: str + + # Example for Toyota Corolla MY20 + # requirements: Lane Tracing Assist (LTA) and Dynamic Radar Cruise Control (DRCC) + # US Market reference: "All", since all Corolla in the US come standard with LTA and DRCC + + # the simplest description of the requirements for the US market package: str + + # the minimum compatibility requirements for this model, regardless + # of market. can be a package, trim, or list of features + requirements: Optional[str] = None + video_link: Optional[str] = None footnotes: List[Enum] = field(default_factory=list) min_steer_speed: Optional[float] = None min_enable_speed: Optional[float] = None - harness: Enum = Harness.none + + # harness connectors + all the parts needed + harness_kit: HarnessKit = HarnessKit() def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): self.car_name = CP.carName @@ -153,12 +185,14 @@ class CarInfo: self.min_enable_speed = CP.minEnableSpeed # harness column - harness_col = self.harness.value - if self.harness is not Harness.none: + harness_col = self.harness_kit.connector.value + if self.harness_kit.connector is not Harness.none: model_years = self.model + (' ' + self.years if self.years else '') - harness_col = f'{harness_col}' + harness_connector = f'- 1 {harness_col} connector' + harness_parts = '
'.join([f"- {self.harness_kit.parts.count(part)} {part.value}" for part in sorted(set(self.harness_kit.parts), key=lambda part: part.value)]) + harness_col = f'
View{harness_connector}
{harness_parts}
' - self.row = { + self.row: Dict[Enum, Union[str, Star]] = { Column.MAKE: self.make, Column.MODEL: self.model, Column.PACKAGE: self.package, diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index e5d550fac..868f12cdb 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -9,6 +9,8 @@ from selfdrive.car import make_can_msg from selfdrive.boardd.boardd import can_list_to_can_capnp from system.swaglog import cloudlog +EcuAddrBusType = Tuple[int, Optional[int], int] + def make_tester_present_msg(addr, bus, subaddr=None): dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0] @@ -33,16 +35,16 @@ def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subadd return False -def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[Tuple[int, Optional[int], int]]: +def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[EcuAddrBusType]: addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)] - queries: Set[Tuple[int, Optional[int], int]] = {(addr, None, bus) for addr in addr_list} + queries: Set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list} responses = queries return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug) -def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[Tuple[int, Optional[int], int]], - responses: Set[Tuple[int, Optional[int], int]], timeout: float = 1, debug: bool = False) -> Set[Tuple[int, Optional[int], int]]: - ecu_responses: Set[Tuple[int, Optional[int], int]] = set() # set((addr, subaddr, bus),) +def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[EcuAddrBusType], + responses: Set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> Set[EcuAddrBusType]: + ecu_responses: Set[EcuAddrBusType] = set() # set((addr, subaddr, bus),) try: msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries] @@ -53,6 +55,10 @@ def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, que can_packets = messaging.drain_sock(logcan, wait_for_one=True) for packet in can_packets: for msg in packet.can: + if not len(msg.dat): + cloudlog.warning("ECU addr scan: skipping empty remote frame") + continue + subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0] if (msg.address, subaddr, msg.src) in responses and is_tester_present_response(msg, subaddr): if debug: diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 99072ae97..d9a9ae6bc 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -2,13 +2,26 @@ from cereal import car from common.numpy_fast import clip from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_angle_limits -from selfdrive.car.ford.fordcan import create_acc_command, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \ +from selfdrive.car.ford.fordcan import create_acc_msg, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \ create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams +LongCtrlState = car.CarControl.Actuators.LongControlState VisualAlert = car.CarControl.HUDControl.VisualAlert +def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw): + # No blending at low speed due to lack of torque wind-up and inaccurate current curvature + if v_ego_raw > 9: + apply_curvature = clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, + current_curvature + CarControllerParams.CURVATURE_ERROR) + + # Curvature rate limit after driver torque limit + apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams) + + return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) + + class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -43,17 +56,16 @@ class CarController: can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True)) ### lateral control ### - # send steering commands at 20Hz + # send steer msg at 20Hz if (self.frame % CarControllerParams.STEER_STEP) == 0: if CC.latActive: - # apply limits to curvature and clip to signal range - apply_curvature = apply_std_steer_angle_limits(actuators.curvature, self.apply_curvature_last, CS.out.vEgo, CarControllerParams) - apply_curvature = clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) + # apply rate limits, curvature error limit, and clip to signal range + current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + apply_curvature = apply_ford_curvature_limits(actuators.curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw) else: apply_curvature = 0. self.apply_curvature_last = apply_curvature - can_sends.append(create_lka_msg(self.packer)) if self.CP.carFingerprint in CANFD_CARS: # TODO: extended mode @@ -63,31 +75,32 @@ class CarController: else: can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + # send lka msg at 33Hz + if (self.frame % CarControllerParams.LKA_STEP) == 0: + can_sends.append(create_lka_msg(self.packer)) + ### longitudinal control ### - # send acc command at 50Hz + # send acc msg at 50Hz if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: + # Both gas and accel are in m/s^2, accel is used solely for braking accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + gas = accel + if not CC.longActive or gas < CarControllerParams.MIN_GAS: + gas = CarControllerParams.INACTIVE_GAS - precharge_brake = accel < -0.1 - if accel > -0.5: - gas = accel - decel = False - else: - gas = -5.0 - decel = True - - can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel)) + stopping = CC.actuators.longControlState == LongCtrlState.stopping + can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, stopping)) ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) - - # send lkas ui command at 1Hz or if ui state changes + # send lkas ui msg at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) - - # send acc ui command at 20Hz or if ui state changes + # send acc ui msg at 5Hz or if ui state changes if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) + can_sends.append(create_acc_ui_msg(self.packer, self.CP, main_on, CC.latActive, + CS.out.cruiseState.standstill, hud_control, + CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index f97225e9f..9be2c7637 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -16,9 +16,21 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] + self.vehicle_sensors_valid = False + self.hybrid_platform = False + def update(self, cp, cp_cam): ret = car.CarState.new_message() + # Hybrid variants experience a bug where a message from the PCM sends invalid checksums, + # we do not support these cars at this time. + # TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants + self.hybrid_platform = cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0 + + # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement + # The vehicle usually recovers out of this state within a minute of normal driving + self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3 + # car speed ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) @@ -37,7 +49,7 @@ class CarState(CarStateBase): # steering wheel ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] - ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE + ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5) ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1 ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) # ret.espDisabled = False # TODO: find traction control signal @@ -48,6 +60,7 @@ class CarState(CarStateBase): ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0 ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3 + ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2) # gear if self.CP.transmissionType == TransmissionType.automatic: @@ -62,7 +75,7 @@ class CarState(CarStateBase): # safety ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) - ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled + ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"]) # button presses ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 @@ -92,6 +105,8 @@ class CarState(CarStateBase): def get_can_parser(CP): signals = [ # sig_name, sig_address + ("TrnAinTq_D_Qf", "VehicleOperatingModes"), # Used to detect hybrid or ICE platform variant + ("Veh_V_ActlBrk", "BrakeSysFeatures"), # ABS vehicle speed (kph) ("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) ("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped @@ -105,6 +120,7 @@ class CarState(CarStateBase): ("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill ("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) + ("StePinCompAnEst_D_Qf", "SteeringPinion_Data"), # PSCM estimated steering angle (quality flag) # Calculates steering angle (and offset) from pinion # angle and driving measurements. # StePinRelInit_An_Sns is the pinion angle, initialised @@ -125,7 +141,6 @@ class CarState(CarStateBase): ("AccButtnGapIncPress", "Steering_Data_FD1"), ("AslButtnOnOffCnclPress", "Steering_Data_FD1"), ("AslButtnOnOffPress", "Steering_Data_FD1"), - ("CcAslButtnCnclPress", "Steering_Data_FD1"), ("LaSwtchPos_D_Stat", "Steering_Data_FD1"), ("CcAslButtnCnclResPress", "Steering_Data_FD1"), ("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"), @@ -139,7 +154,6 @@ class CarState(CarStateBase): ("CcAslButtnSetDecPress", "Steering_Data_FD1"), ("CcAslButtnSetIncPress", "Steering_Data_FD1"), ("CcAslButtnSetPress", "Steering_Data_FD1"), - ("CcAsllButtnResPress", "Steering_Data_FD1"), ("CcButtnOffPress", "Steering_Data_FD1"), ("CcButtnOnOffCnclPress", "Steering_Data_FD1"), ("CcButtnOnOffPress", "Steering_Data_FD1"), @@ -154,6 +168,7 @@ class CarState(CarStateBase): checks = [ # sig_address, frequency + ("VehicleOperatingModes", 100), ("BrakeSysFeatures", 50), ("Yaw_Data_FD1", 100), ("DesiredTorqBrk", 50), @@ -202,6 +217,8 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): signals = [ # sig_name, sig_address + ("CmbbBrkDecel_B_Rq", "ACCDATA_2"), # AEB actuation request bit + ("HaDsply_No_Cs", "ACCDATA_3"), ("HaDsply_No_Cnt", "ACCDATA_3"), ("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message @@ -216,7 +233,7 @@ class CarState(CarStateBase): ("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting ("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting ("CadsAlignIncplt_B_Actl", "ACCDATA_3"), - ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting + ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC lead indicator ("CadsRadrBlck_B_Actl", "ACCDATA_3"), ("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status ("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting @@ -233,9 +250,7 @@ class CarState(CarStateBase): ("FeatNoIpmaActl", "IPMA_Data"), ("PersIndexIpma_D_Actl", "IPMA_Data"), ("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping - ("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines) ("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error - ("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime ("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater? ("CamraStats_D_Dsply", "IPMA_Data"), # Camera status ("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level @@ -248,6 +263,7 @@ class CarState(CarStateBase): checks = [ # sig_address, frequency + ("ACCDATA_2", 50), ("ACCDATA_3", 5), ("IPMA_Data", 1), ] diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 594d50f59..97a8c025d 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -5,11 +5,15 @@ HUDControl = car.CarControl.HUDControl def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): + curvature = (dat[2] << 3) | ((dat[3]) >> 5) + curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5) + path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2) + path_offset = ((dat[4] & 0x3) << 8) | dat[5] + checksum = mode + counter - checksum += dat[2] + ((dat[3] & 0xE0) >> 5) # curvature - checksum += dat[6] + ((dat[7] & 0xE0) >> 5) # curvature rate - checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2) # path angle - checksum += (dat[4] & 0x3) + dat[5] # path offset + for sig_val in (curvature, curvature_rate, path_angle, path_offset): + checksum += sig_val + (sig_val >> 8) + return 0xFF - (checksum & 0xFF) @@ -19,7 +23,7 @@ def create_lka_msg(packer): This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout. - Frequency is 20Hz. + Frequency is 33Hz. """ return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) @@ -97,7 +101,7 @@ def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values) -def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool): +def create_acc_msg(packer, long_active: bool, gas: float, accel: float, stopping: bool): """ Creates a CAN message for the Ford ACC Command. @@ -107,16 +111,95 @@ def create_acc_command(packer, long_active: bool, gas: float, accel: float, prec Frequency is 50Hz. """ + decel = accel < 0 and long_active values = { "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 - "AccBrkPrchg_B_Rq": 1 if precharge_brake else 0, # Pre-charge brake request: 0=No, 1=Yes + "AccResumEnbl_B_Rq": 1 if long_active else 0, + # TODO: we may be able to improve braking response by utilizing pre-charging better + "AccBrkPrchg_B_Rq": 1 if decel else 0, # Pre-charge brake request: 0=No, 1=Yes "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active + "AccStopStat_B_Rq": 1 if stopping else 0, } return packer.make_can_msg("ACCDATA", CANBUS.main, values) +def create_acc_ui_msg(packer, CP, main_on: bool, enabled: bool, standstill: bool, hud_control, + stock_values: dict): + """ + Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam + assist status. + + Stock functionality is maintained by passing through unmodified signals. + + Frequency is 5Hz. + """ + + # Tja_D_Stat + if enabled: + if hud_control.leftLaneDepart: + status = 3 # ActiveInterventionLeft + elif hud_control.rightLaneDepart: + status = 4 # ActiveInterventionRight + else: + status = 2 # Active + elif main_on: + if hud_control.leftLaneDepart: + status = 5 # ActiveWarningLeft + elif hud_control.rightLaneDepart: + status = 6 # ActiveWarningRight + else: + status = 1 # Standby + else: + status = 0 # Off + + values = {s: stock_values[s] for s in [ + "HaDsply_No_Cs", + "HaDsply_No_Cnt", + "AccStopStat_D_Dsply", # ACC stopped status message + "AccTrgDist2_D_Dsply", # ACC target distance + "AccStopRes_B_Dsply", + "TjaWarn_D_Rq", # TJA warning + "TjaMsgTxt_D_Dsply", # TJA text + "IaccLamp_D_Rq", # iACC status icon + "AccMsgTxt_D2_Rq", # ACC text + "FcwDeny_B_Dsply", # FCW disabled + "FcwMemStat_B_Actl", # FCW enabled setting + "AccTGap_B_Dsply", # ACC time gap display setting + "CadsAlignIncplt_B_Actl", + "AccFllwMde_B_Dsply", # ACC follow mode display setting + "CadsRadrBlck_B_Actl", + "CmbbPostEvnt_B_Dsply", # AEB event status + "AccStopMde_B_Dsply", # ACC stop mode display setting + "FcwMemSens_D_Actl", # FCW sensitivity setting + "FcwMsgTxt_D_Rq", # FCW text + "AccWarn_D_Dsply", # ACC warning + "FcwVisblWarn_B_Rq", # FCW visible alert + "FcwAudioWarn_B_Rq", # FCW audio alert + "AccTGap_D_Dsply", # ACC time gap + "AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting + "FdaMem_B_Stat", # FDA enabled setting + ]} + + values.update({ + "Tja_D_Stat": status, # TJA status + }) + + if CP.openpilotLongitudinalControl: + values.update({ + "AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text + "AccMsgTxt_D2_Rq": 0, # ACC text + "AccTGap_B_Dsply": 0, # Show time gap control UI + "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator + "AccStopMde_B_Dsply": 1 if standstill else 0, + "AccWarn_D_Dsply": 0, # ACC warning + "AccTGap_D_Dsply": 4, # Fixed time gap in UI + }) + + return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) + + def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC IPMA/LKAS status. @@ -158,63 +241,78 @@ def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, else: lines = 30 # LA_Off - # TODO: use level 1 for no sound when less severe? - hands_on_wheel_dsply = 2 if steer_alert else 0 + hands_on_wheel_dsply = 1 if steer_alert else 0 - values = { - **stock_values, + values = {s: stock_values[s] for s in [ + "FeatConfigIpmaActl", + "FeatNoIpmaActl", + "PersIndexIpma_D_Actl", + "AhbcRampingV_D_Rq", # AHB ramping + "LaDenyStats_B_Dsply", # LKAS error + "CamraDefog_B_Req", # Windshield heater? + "CamraStats_D_Dsply", # Camera status + "DasAlrtLvl_D_Dsply", # DAS alert level + "DasStats_D_Dsply", # DAS status + "DasWarn_D_Dsply", # DAS warning + "AhbHiBeam_D_Rq", # AHB status + "Passthru_63", + "Passthru_48", + ]} + + values.update({ "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] "LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed - } + }) return packer.make_can_msg("IPMA_Data", CANBUS.main, values) -def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): - """ - Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status. - - Stock functionality is maintained by passing through unmodified signals. - - Frequency is 20Hz. - """ - - # Tja_D_Stat - if enabled: - if hud_control.leftLaneDepart: - status = 3 # ActiveInterventionLeft - elif hud_control.rightLaneDepart: - status = 4 # ActiveInterventionRight - else: - status = 2 # Active - elif main_on: - if hud_control.leftLaneDepart: - status = 5 # ActiveWarningLeft - elif hud_control.rightLaneDepart: - status = 6 # ActiveWarningRight - else: - status = 1 # Standby - else: - status = 0 # Off - - values = { - **stock_values, - "Tja_D_Stat": status, - } - return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) - - def create_button_msg(packer, stock_values: dict, cancel=False, resume=False, tja_toggle=False, bus: int = CANBUS.camera): """ Creates a CAN message for the Ford SCCM buttons/switches. Includes cruise control buttons, turn lights and more. + + Frequency is 10Hz. """ - values = { - **stock_values, + values = {s: stock_values[s] for s in [ + "HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons + "TurnLghtSwtch_D_Stat", # SCCM Turn signal switch + "WiprFront_D_Stat", + "LghtAmb_D_Sns", + "AccButtnGapDecPress", + "AccButtnGapIncPress", + "AslButtnOnOffCnclPress", + "AslButtnOnOffPress", + "LaSwtchPos_D_Stat", + "CcAslButtnCnclResPress", + "CcAslButtnDeny_B_Actl", + "CcAslButtnIndxDecPress", + "CcAslButtnIndxIncPress", + "CcAslButtnOffCnclPress", + "CcAslButtnOnOffCncl", + "CcAslButtnOnPress", + "CcAslButtnResDecPress", + "CcAslButtnResIncPress", + "CcAslButtnSetDecPress", + "CcAslButtnSetIncPress", + "CcAslButtnSetPress", + "CcButtnOffPress", + "CcButtnOnOffCnclPress", + "CcButtnOnOffPress", + "CcButtnOnPress", + "HeadLghtHiFlash_D_Actl", + "HeadLghtHiOn_B_StatAhb", + "AhbStat_B_Dsply", + "AccButtnGapTogglePress", + "WiprFrontSwtch_D_Stat", + "HeadLghtHiCtrl_D_RqAhb", + ]} + + values.update({ "CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button "CcAsllButtnResPress": 1 if resume else 0, # CC resume button - "TjaButtnOnOffPress": 1 if tja_toggle else 0, # TJA toggle button - } + "TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button + }) return packer.make_can_msg("Steering_Data_FD1", bus, values) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 9e1366618..937980899 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -11,12 +11,14 @@ GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "ford" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] - # These cars are dashcam only until the port is finished - ret.dashcamOnly = True + # These cars are dashcam only for lack of test coverage. + # Once a user confirms each car works and a test route is + # added to selfdrive/car/tests/routes.py, we can remove it from this list. + ret.dashcamOnly = candidate in {CAR.FOCUS_MK4} ret.radarUnavailable = True ret.steerControlType = car.CarParams.SteerControlType.angle @@ -53,7 +55,7 @@ class CarInterface(CarInterfaceBase): # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 found_ecus = [fw.ecu for fw in car_fw] - if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0]: + if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0] or docs: ret.transmissionType = TransmissionType.automatic else: ret.transmissionType = TransmissionType.manual @@ -74,6 +76,11 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) + if not self.CS.vehicle_sensors_valid: + events.add(car.CarEvent.EventName.vehicleSensorsInvalid) + if self.CS.hybrid_platform: + events.add(car.CarEvent.EventName.startupNoControl) + ret.events = events.to_msg() return ret diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 526b74b16..6425179a2 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,38 +1,38 @@ from collections import defaultdict from dataclasses import dataclass -from enum import Enum from typing import Dict, List, Set, Union from cereal import car from selfdrive.car import AngleRateLimit, dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu class CarControllerParams: - # Messages: Lane_Assist_Data1, LateralMotionControl - STEER_STEP = 5 - # Message: ACCDATA - ACC_CONTROL_STEP = 2 - # Message: IPMA_Data - LKAS_UI_STEP = 100 - # Message: ACCDATA_3 - ACC_UI_STEP = 5 - # Message: Steering_Data_FD1, but send twice as fast - BUTTONS_STEP = 10 / 2 + STEER_STEP = 5 # LateralMotionControl, 20Hz + LKA_STEP = 3 # Lane_Assist_Data1, 33Hz + ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz + LKAS_UI_STEP = 100 # IPMA_Data, 1Hz + ACC_UI_STEP = 20 # ACCDATA_3, 5Hz + BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1 - STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold, Nm + STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm # Curvature rate limits - # TODO: unify field names used by curvature and angle control cars - ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.005, 0.00056, 0.0002]) - ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.008, 0.00089, 0.00032]) + # The curvature signal is limited to 0.003 to 0.009 m^-1/sec by the EPS depending on speed and direction + # Limit to ~2 m/s^3 up, ~3 m/s^3 down at 75 mph + # Worst case, the low speed limits will allow 4.3 m/s^3 up, 4.9 m/s^3 down at 75 mph + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.0002, 0.0001]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.000225, 0.00015]) + CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s ACCEL_MAX = 2.0 # m/s^s max acceleration ACCEL_MIN = -3.5 # m/s^s max deceleration + MIN_GAS = -0.5 + INACTIVE_GAS = -5.0 def __init__(self, CP): pass @@ -66,21 +66,21 @@ DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base @dataclass class FordCarInfo(CarInfo): package: str = "Co-Pilot360 Assist+" - harness: Enum = Harness.ford_q3 + harness_kit: HarnessKit = HarnessKit(Harness.ford_q3) CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"), CAR.ESCAPE_MK4: [ - FordCarInfo("Ford Escape 2020-21"), - FordCarInfo("Ford Kuga 2020-21", "Driver Assistance Pack"), + FordCarInfo("Ford Escape 2020-22"), + FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"), ], CAR.EXPLORER_MK6: [ FordCarInfo("Ford Explorer 2020-22"), FordCarInfo("Lincoln Aviator 2021", "Co-Pilot360 Plus"), ], - CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), - CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022", "Co-Pilot360 Assist"), + CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2018", "Adaptive Cruise Control with Lane Centering"), + CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022-23", "Co-Pilot360 Assist"), } FW_QUERY_CONFIG = FwQueryConfig( @@ -128,6 +128,7 @@ FW_VERSIONS = { (Ecu.eps, 0x730, None): [ b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -145,19 +146,25 @@ FW_VERSIONS = { b'LX6A-14C204-BJV\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'MX6A-14C204-BEF\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'MX6A-14C204-BEJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NX6A-14C204-BLE\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.shiftByWire, 0x732, None): [ b'LX6P-14G395-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6P-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ1P-14G395-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.EXPLORER_MK6: { (Ecu.eps, 0x730, None): [ + b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ + b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-KB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -166,11 +173,14 @@ FW_VERSIONS = { b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ + b'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7E0, None): [ + b'LB5A-14C204-ATJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LB5A-14C204-BUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'MB5A-14C204-RC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -208,6 +218,7 @@ FW_VERSIONS = { ], (Ecu.abs, 0x760, None): [ b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -219,6 +230,7 @@ FW_VERSIONS = { b'NZ6A-14C204-AAA\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NZ6A-14C204-PA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NZ6A-14C204-ZA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ6A-14C204-JC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.shiftByWire, 0x732, None): [ b'NZ6P-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py index dd3b19f6d..7ae9bee40 100755 --- a/selfdrive/car/fw_query_definitions.py +++ b/selfdrive/car/fw_query_definitions.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import capnp +import copy from dataclasses import dataclass, field import struct from typing import Dict, List, Optional, Tuple @@ -57,10 +58,12 @@ class Request: whitelist_ecus: List[int] = field(default_factory=list) rx_offset: int = 0x8 bus: int = 1 + # Whether this query should be run on the first auxiliary panda (CAN FD cars for example) + auxiliary: bool = False # FW responses from these queries will not be used for fingerprinting logging: bool = False - # These requests are done once OBD multiplexing is disabled, after all others - non_obd: bool = False + # boardd toggles OBD multiplexing on/off as needed + obd_multiplexing: bool = True @dataclass @@ -71,3 +74,10 @@ class FwQueryConfig: non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict) # Ecus added for data collection, not to be fingerprinted on extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list) + + def __post_init__(self): + for i in range(len(self.requests)): + if self.requests[i].auxiliary: + new_request = copy.deepcopy(self.requests[i]) + new_request.bus += 4 + self.requests.append(new_request) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 8092ac0b7..1c0d5003e 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 from collections import defaultdict -from typing import Any, Optional, Set, Tuple +from typing import Any, Dict, List, Set from tqdm import tqdm import panda.python.uds as uds from cereal import car from common.params import Params -from selfdrive.car.ecu_addrs import get_ecu_addrs +from selfdrive.car.ecu_addrs import EcuAddrBusType, get_ecu_addrs from selfdrive.car.interfaces import get_interface_attr from selfdrive.car.fingerprints import FW_VERSIONS from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery @@ -19,7 +19,7 @@ FW_QUERY_CONFIGS = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True) VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True) MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e} -REQUESTS = [(brand, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests] +REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests] def chunks(l, n=128): @@ -39,6 +39,8 @@ def build_fw_dict(fw_versions, filter_brand=None): def get_brand_addrs(): brand_addrs = defaultdict(set) for brand, cars in VERSIONS.items(): + # Add ecus in database + extra ecus to match against + brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in FW_QUERY_CONFIGS[brand].extra_ecus} for fw in cars.values(): brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in fw.keys()} return brand_addrs @@ -146,38 +148,43 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True): return True, set() -def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[Tuple[int, Optional[int], int]]: - queries = list() - parallel_queries = list() +def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]: + params = Params() + # queries are split by OBD multiplexing mode + queries: Dict[bool, List[List[EcuAddrBusType]]] = {True: [], False: []} + parallel_queries: Dict[bool, List[EcuAddrBusType]] = {True: [], False: []} responses = set() - for brand, r in REQUESTS: + for brand, config, r in REQUESTS: # Skip query if no panda available if r.bus > num_pandas * 4 - 1: continue for brand_versions in VERSIONS[brand].values(): - for ecu_type, addr, sub_addr in brand_versions: + for ecu_type, addr, sub_addr in list(brand_versions) + config.extra_ecus: # Only query ecus in whitelist if whitelist is not empty if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus: a = (addr, sub_addr, r.bus) # Build set of queries if sub_addr is None: - if a not in parallel_queries: - parallel_queries.append(a) + if a not in parallel_queries[r.obd_multiplexing]: + parallel_queries[r.obd_multiplexing].append(a) else: # subaddresses must be queried one by one - if [a] not in queries: - queries.append([a]) + if [a] not in queries[r.obd_multiplexing]: + queries[r.obd_multiplexing].append([a]) # Build set of expected responses to filter response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset) responses.add((response_addr, sub_addr, r.bus)) - queries.insert(0, parallel_queries) + for obd_multiplexing in queries: + queries[obd_multiplexing].insert(0, parallel_queries[obd_multiplexing]) ecu_responses = set() - for query in queries: - ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) + for obd_multiplexing in queries: + set_obd_multiplexing(params, obd_multiplexing) + for query in queries[obd_multiplexing]: + ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) return ecu_responses @@ -185,9 +192,9 @@ def get_brand_ecu_matches(ecu_rx_addrs): """Returns dictionary of brands and matches with ECUs in their FW versions""" brand_addrs = get_brand_addrs() - brand_matches = {brand: set() for brand, _ in REQUESTS} + brand_matches = {brand: set() for brand, _, _ in REQUESTS} - brand_rx_offsets = set((brand, r.rx_offset) for brand, r in REQUESTS) + brand_rx_offsets = set((brand, r.rx_offset) for brand, _, r in REQUESTS) for addr, sub_addr, _ in ecu_rx_addrs: # Since we can't know what request an ecu responded to, add matches for all possible rx offsets for brand, rx_offset in brand_rx_offsets: @@ -198,13 +205,13 @@ def get_brand_ecu_matches(ecu_rx_addrs): return brand_matches -def disable_obd_multiplexing(params): - if not params.get_bool("ObdMultiplexingDisabled"): - params.put_bool("FirmwareObdQueryDone", True) - - cloudlog.warning("Waiting for OBD multiplexing to be disabled") - params.get_bool("ObdMultiplexingDisabled", block=True) - cloudlog.warning("OBD multiplexing disabled") +def set_obd_multiplexing(params: Params, obd_multiplexing: bool): + if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing: + cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}") + params.remove("ObdMultiplexingChanged") + params.put_bool("ObdMultiplexingEnabled", obd_multiplexing) + params.get_bool("ObdMultiplexingChanged", block=True) + cloudlog.warning("OBD multiplexing set successfully") def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False): @@ -212,29 +219,25 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand all_car_fw = [] brand_matches = get_brand_ecu_matches(ecu_rx_addrs) - matched_brand: Optional[str] = None for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True): + # Skip this brand if there are no matching present ECUs + if not len(brand_matches[brand]): + continue + car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress) all_car_fw.extend(car_fw) # Try to match using FW returned from this brand only matches = match_fw_to_car_exact(build_fw_dict(car_fw)) if len(matches) == 1: - matched_brand = brand break - disable_obd_multiplexing(Params()) - - # Do non-OBD queries for matched brand, or all if no match is found - for brand in FW_QUERY_CONFIGS.keys(): - if brand == matched_brand or matched_brand is None: - all_car_fw.extend(get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, obd_multiplexed=False, debug=debug, progress=progress)) - return all_car_fw -def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, obd_multiplexed=True, debug=False, progress=False): +def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False): versions = VERSIONS.copy() + params = Params() # Each brand can define extra ECUs to query for data collection for brand, config in FW_QUERY_CONFIGS.items(): @@ -250,19 +253,15 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, # ECUs using a subaddress need be queried one by one, the rest can be done in parallel addrs = [] parallel_addrs = [] - logging_addrs = [] ecu_types = {} for brand, brand_versions in versions.items(): - for candidate, ecu in brand_versions.items(): + for ecu in brand_versions.values(): for ecu_type, addr, sub_addr in ecu.keys(): a = (brand, addr, sub_addr) if a not in ecu_types: ecu_types[a] = ecu_type - if a not in logging_addrs and candidate == "debug": - logging_addrs.append(a) - if sub_addr is None: if a not in parallel_addrs: parallel_addrs.append(a) @@ -274,16 +273,17 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, # Get versions and build capnp list to put into CarParams car_fw = [] - requests = [(brand, r) for brand, r in REQUESTS if query_brand is None or brand == query_brand] + requests = [(brand, config, r) for brand, config, r in REQUESTS if query_brand is None or brand == query_brand] for addr in tqdm(addrs, disable=not progress): for addr_chunk in chunks(addr): - for brand, r in requests: + for brand, config, r in requests: # Skip query if no panda available if r.bus > num_pandas * 4 - 1: continue - # Or if request is not designated for current multiplexing mode - elif r.non_obd == obd_multiplexed: - continue + + # Toggle OBD multiplexing for each request + if r.bus % 4 == 1: + set_obd_multiplexing(params, r.obd_multiplexing) try: addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and @@ -294,15 +294,15 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, for (tx_addr, sub_addr), version in query.get_data(timeout).items(): f = car.CarParams.CarFw.new_message() - ecu_key = (brand, tx_addr, sub_addr) - f.ecu = ecu_types.get(ecu_key, Ecu.unknown) + f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown) f.fwVersion = version f.address = tx_addr f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset) f.request = r.request f.brand = brand f.bus = r.bus - f.logging = r.logging or ecu_key in logging_addrs + f.logging = r.logging or (f.ecu, tx_addr, sub_addr) in config.extra_ecus + f.obdMultiplexing = r.obd_multiplexing if sub_addr is not None: f.subAddress = sub_addr diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 73085d30b..2a996c0ff 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -3,7 +3,7 @@ from common.conversions import Conversions as CV from common.numpy_fast import interp from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.gm import gmcan from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons @@ -30,7 +30,6 @@ class CarController: self.cancel_counter = 0 self.lka_steering_cmd_counter = 0 - self.sent_lka_steering_cmd = False self.lka_icon_status_last = (False, False) self.params = CarControllerParams(self.CP) @@ -59,24 +58,22 @@ class CarController: # - until we're in sync with camera so counters align when relay closes, preventing a fault. # openpilot can subtly drift, so this is activated throughout a drive to stay synced out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.cam_lka_steering_cmd_counter + 1) % 4 - if not self.sent_lka_steering_cmd or out_of_sync: + if CS.loopback_lka_steering_cmd_ts_nanos == 0 or out_of_sync: steer_step = self.params.STEER_STEP - if CS.loopback_lka_steering_cmd_updated: - self.lka_steering_cmd_counter += 1 - self.sent_lka_steering_cmd = True + self.lka_steering_cmd_counter += 1 if CS.loopback_lka_steering_cmd_updated else 0 # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we # received the ASCMLKASteeringCmd loopback confirmation too recently last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6 if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS: # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus - if not self.sent_lka_steering_cmd: + if CS.loopback_lka_steering_cmd_ts_nanos == 0: self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1 if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) else: apply_steer = 0 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 3c7d35f2d..2f9c95287 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -37,7 +37,8 @@ class CarState(CarStateBase): # Variables used for avoiding LKAS faults self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 - self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"] + if self.loopback_lka_steering_cmd_updated: + self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"] if self.CP.networkLocation == NetworkLocation.fwdCamera: self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 864db19f7..ff578da98 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,12 +1,14 @@ #!/usr/bin/env python3 from cereal import car -from math import fabs +from math import fabs, exp from panda import Panda from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config -from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR -from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG +from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus +from selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD +from selfdrive.controls.lib.drive_helpers import get_friction ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -44,7 +46,30 @@ class CarInterface(CarInterfaceBase): return CarInterfaceBase.get_steer_feedforward_default @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def torque_from_lateral_accel_bolt(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, + lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float: + friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) + + def sig(val): + return 1 / (1 + exp(-val)) - 0.5 + + # The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves + # An important thing to consider is that the slope at 0 should be > 0 (ideally >1) + # This has big effect on the stability about 0 (noise when going straight) + # ToDo: To generalize to other GMs, explore tanh function as the nonlinear + a, b, c, _ = [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178] # weights computed offline + + steer_torque = (sig(lateral_accel_value * a) * b) + (lateral_accel_value * c) + return float(steer_torque) + friction + + def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: + if self.CP.carFingerprint == CAR.BOLT_EUV: + return self.torque_from_lateral_accel_bolt + else: + return self.torque_from_lateral_accel_linear + + @staticmethod + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -72,11 +97,9 @@ class CarInterface(CarInterfaceBase): # Tuning for experimental long ret.longitudinalTuning.kpV = [2.0, 1.5] ret.longitudinalTuning.kiV = [0.72] - ret.stopAccel = -2.0 ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 - ret.longitudinalActuatorDelayUpperBound = 0.5 if experimental_long: ret.pcmCruise = False @@ -86,7 +109,7 @@ class CarInterface(CarInterfaceBase): else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway - ret.radarUnavailable = False + ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS @@ -99,7 +122,8 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/car/tests/routes.py, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX} + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX} or \ + (ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable) # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -110,6 +134,7 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz + ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking if candidate == CAR.VOLT: ret.mass = 1607. + STD_CARGO_KG @@ -145,7 +170,13 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.4 # end to end is 13.46 ret.centerToFront = ret.wheelbase * 0.4 ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() - ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking + + elif candidate == CAR.BUICK_LACROSSE: + ret.mass = 1712. + STD_CARGO_KG + ret.wheelbase = 2.91 + ret.steerRatio = 15.8 + ret.centerToFront = ret.wheelbase * 0.4 # wild guess + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.BUICK_REGAL: ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 @@ -202,6 +233,15 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.TRAILBLAZER: + ret.mass = 1345. + STD_CARGO_KG + ret.wheelbase = 2.64 + ret.steerRatio = 16.8 + ret.centerToFront = ret.wheelbase * 0.4 + tire_stiffness_factor = 1.0 + ret.steerActuatorDelay = 0.2 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index de5b7be55..865ba8ed0 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -5,7 +5,7 @@ from typing import Dict, List, Union from cereal import car from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit, HarnessPart Ecu = car.CarParams.Ecu @@ -14,8 +14,8 @@ class CarControllerParams: STEER_STEP = 3 # Active control frames per command (~33hz) INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz) STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness - STEER_DELTA_DOWN = 25 - STEER_DRIVER_ALLOWANCE = 50 + STEER_DELTA_DOWN = 15 + STEER_DRIVER_ALLOWANCE = 65 STEER_DRIVER_MULTIPLIER = 4 STEER_DRIVER_FACTOR = 100 NEAR_STOP_BRAKE_PHASE = 0.5 # m/s @@ -66,12 +66,14 @@ class CAR: CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" MALIBU = "CHEVROLET MALIBU PREMIER 2017" ACADIA = "GMC ACADIA DENALI 2018" + BUICK_LACROSSE = "BUICK LACROSSE 2017" BUICK_REGAL = "BUICK REGAL ESSENCE 2018" ESCALADE = "CADILLAC ESCALADE 2017" ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" BOLT_EUV = "CHEVROLET BOLT EUV 2022" SILVERADO = "CHEVROLET SILVERADO 1500 2020" EQUINOX = "CHEVROLET EQUINOX 2019" + TRAILBLAZER = "CHEVROLET TRAILBLAZER 2021" class Footnote(Enum): @@ -87,9 +89,9 @@ class GMCarInfo(CarInfo): def init_make(self, CP: car.CarParams): if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera: - self.harness = Harness.gm + self.harness_kit = HarnessKit(Harness.gm) else: - self.harness = Harness.obd_ii + self.harness_kit = HarnessKit(Harness.obd_ii, parts=[HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler]) self.footnotes.append(Footnote.OBD_II) @@ -99,18 +101,20 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"), CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"), CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"), + CAR.BUICK_LACROSSE: GMCarInfo("Buick LaCrosse 2017-19", "Driver Confidence Package 2"), 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.BOLT_EUV: [ - GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210"), + 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"), ], CAR.SILVERADO: [ GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II"), - GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", "https://youtu.be/5HbNoBLzRwE"), + GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"), ], CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22"), + CAR.TRAILBLAZER: GMCarInfo("Chevrolet Trailblazer 2021-22"), } @@ -151,6 +155,15 @@ FINGERPRINTS = { # Volt Premier w/ ACC 2018 { 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 578: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1516: 8, 1601: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2018: 8, 2020: 8, 2024: 8, 2028: 8 + }, + # Volt Premier 2018 w/ flashed firmware, no radar + { + 170: 8, 171: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 209: 7, 211: 2, 241: 6, 288: 5, 289: 1, 290: 1, 298: 2, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 368: 8, 381: 2, 384: 8, 386: 5, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 3, 508: 8, 512: 3, 528: 4, 530: 8, 532: 6, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 5, 844: 8, 853: 8, 866: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7 + }], + CAR.BUICK_LACROSSE: [ + # LaCrosse Premium AWD 2017 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 5, 707: 8, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 872: 1, 882: 8, 890: 1, 892: 2, 893: 1, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1916: 7, 1918: 7, 1919: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8 }], CAR.BUICK_REGAL : [ # Regal TourX Essence w/ ACC 2018 @@ -196,6 +209,12 @@ FINGERPRINTS = { { 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 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, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1930: 7 }], + # Trailblazer also matches as a Silverado, so comment out to avoid conflicts. + # TODO: split with FW versions + CAR.TRAILBLAZER: [ + { + # 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8 + }], } DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) @@ -203,6 +222,6 @@ DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_power EV_CAR = {CAR.VOLT, CAR.BOLT_EUV} # We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness) -CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX} +CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX, CAR.TRAILBLAZER} STEER_THRESHOLD = 1.0 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 4dc1dc813..347c16c86 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,7 +1,6 @@ from collections import namedtuple from cereal import car -from common.conversions import Conversions as CV from common.numpy_fast import clip, interp from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker @@ -117,6 +116,7 @@ class CarController: self.brake_last = 0. self.apply_brake_last = 0 self.last_pump_ts = 0. + self.stopping_counter = 0 self.accel = 0.0 self.speed = 0.0 @@ -127,7 +127,8 @@ class CarController: def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl - hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible else 255 + conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric) + hud_v_cruise = hud_control.setSpeed / conversion if hud_control.speedVisible else 255 pcm_cancel_cmd = CC.cruiseControl.cancel if CC.longActive: @@ -161,7 +162,7 @@ class CarController: can_sends = [] # tester present - w/ no response (keeps radar disabled) - if self.CP.carFingerprint in HONDA_BOSCH and self.CP.openpilotLongitudinalControl: + if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and self.CP.openpilotLongitudinalControl: if self.frame % 10 == 0: can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) @@ -217,8 +218,9 @@ class CarController: self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) stopping = actuators.longControlState == LongCtrlState.stopping + self.stopping_counter = self.stopping_counter + 1 if stopping else 0 can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas, - stopping, self.CP.carFingerprint)) + self.stopping_counter, self.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1)) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 16880d1b1..bcc239c2d 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -5,7 +5,7 @@ from common.conversions import Conversions as CV from common.numpy_fast import interp from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from selfdrive.car.honda.hondacan import get_pt_bus +from selfdrive.car.honda.hondacan import get_cruise_speed_conversion, get_pt_bus from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS from selfdrive.car.interfaces import CarStateBase @@ -103,7 +103,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): else: checks.append(("CRUISE_PARAMS", 50)) - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022): + if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK")) elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS")) @@ -120,7 +120,10 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) checks.append(("GAS_SENSOR", 50)) - if CP.openpilotLongitudinalControl: + if CP.carFingerprint in HONDA_BOSCH_RADARLESS: + signals.append(("CRUISE_FAULT", "CRUISE_FAULT_STATUS")) + checks.append(("CRUISE_FAULT_STATUS", 50)) + elif CP.openpilotLongitudinalControl: signals += [ ("BRAKE_ERROR_1", "STANDSTILL"), ("BRAKE_ERROR_2", "STANDSTILL") @@ -176,7 +179,7 @@ class CarState(CarStateBase): # panda checks if the signal is non-zero ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5 # TODO: find a common signal across all cars - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022): + if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"]) elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"]) @@ -191,7 +194,9 @@ class CarState(CarStateBase): # NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2") - if self.CP.openpilotLongitudinalControl: + if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: + self.brake_error = cp.vl["CRUISE_FAULT_STATUS"]["CRUISE_FAULT"] + elif self.CP.openpilotLongitudinalControl: self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"] ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0 @@ -246,8 +251,7 @@ class CarState(CarStateBase): ret.cruiseState.nonAdaptive = acc_hud["CRUISE_CONTROL_LABEL"] != 0 ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252. - # on certain cars, CRUISE_SPEED changes to imperial with car's unit setting - conversion = CV.MPH_TO_MS if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS and not self.is_metric else CV.KPH_TO_MS + conversion = get_cruise_speed_conversion(self.CP.carFingerprint, self.is_metric) # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. ret.cruiseState.speed = self.v_cruise_pcm_prev if acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * conversion self.v_cruise_pcm_prev = ret.cruiseState.speed @@ -294,7 +298,7 @@ class CarState(CarStateBase): if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: self.lkas_hud = cp_cam.vl["LKAS_HUD"] - if self.CP.enableBsm and self.CP.carFingerprint in (CAR.CRV_5G, ): + if self.CP.enableBsm: # BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0 # more info here: https://github.com/commaai/openpilot/pull/1867 ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1 @@ -341,7 +345,7 @@ class CarState(CarStateBase): @staticmethod def get_body_can_parser(CP): - if CP.enableBsm and CP.carFingerprint == CAR.CRV_5G: + if CP.enableBsm: signals = [("BSM_ALERT", "BSM_STATUS_RIGHT"), ("BSM_ALERT", "BSM_STATUS_LEFT")] diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 17681444a..1fe0a1376 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -21,6 +21,11 @@ def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False): return 0 +def get_cruise_speed_conversion(car_fingerprint: str, is_metric: bool) -> float: + # on certain cars, CRUISE_SPEED changes to imperial with car's unit setting + return CV.MPH_TO_MS if car_fingerprint in HONDA_BOSCH_RADARLESS and not is_metric else CV.KPH_TO_MS + + def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake): # TODO: do we loose pressure if we keep pump off for long? brakelights = apply_brake > 0 @@ -46,7 +51,7 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ return packer.make_can_msg("BRAKE_COMMAND", bus, values) -def create_acc_commands(packer, enabled, active, accel, gas, stopping, car_fingerprint): +def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, car_fingerprint): commands = [] bus = get_pt_bus(car_fingerprint) min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0] @@ -55,30 +60,39 @@ def create_acc_commands(packer, enabled, active, accel, gas, stopping, car_finge gas_command = gas if active and accel > min_gas_accel else -30000 accel_command = accel if active else 0 braking = 1 if active and accel < min_gas_accel else 0 - standstill = 1 if active and stopping else 0 - standstill_release = 1 if active and not stopping else 0 + standstill = 1 if active and stopping_counter > 0 else 0 + standstill_release = 1 if active and stopping_counter == 0 else 0 + # common ACC_CONTROL values acc_control_values = { - # setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1 - "CONTROL_ON": control_on, - "GAS_COMMAND": gas_command, # used for gas - "ACCEL_COMMAND": accel_command, # used for brakes - "BRAKE_LIGHTS": braking, - "BRAKE_REQUEST": braking, - "STANDSTILL": standstill, - "STANDSTILL_RELEASE": standstill_release, + 'ACCEL_COMMAND': accel_command, + 'STANDSTILL': standstill, } + + if car_fingerprint in HONDA_BOSCH_RADARLESS: + acc_control_values.update({ + "CONTROL_ON": enabled, + "IDLESTOP_ALLOW": stopping_counter > 200, # allow idle stop after 4 seconds (50 Hz) + }) + else: + acc_control_values.update({ + # setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1 + "CONTROL_ON": control_on, + "GAS_COMMAND": gas_command, # used for gas + "BRAKE_LIGHTS": braking, + "BRAKE_REQUEST": braking, + "STANDSTILL_RELEASE": standstill_release, + }) + acc_control_on_values = { + "SET_TO_3": 0x03, + "CONTROL_ON": enabled, + "SET_TO_FF": 0xff, + "SET_TO_75": 0x75, + "SET_TO_30": 0x30, + } + commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values)) + commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values)) - - acc_control_on_values = { - "SET_TO_3": 0x03, - "CONTROL_ON": enabled, - "SET_TO_FF": 0xff, - "SET_TO_75": 0x75, - "SET_TO_30": 0x30, - } - commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values)) - return commands diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index d3cf9fa89..65deab740 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -31,19 +31,17 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "honda" if candidate in HONDA_BOSCH: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)] ret.radarUnavailable = True - - if candidate not in HONDA_BOSCH_RADARLESS: - # Disable the radar and let openpilot control longitudinal - # WARNING: THIS DISABLES AEB! - ret.experimentalLongitudinalAvailable = True - ret.openpilotLongitudinalControl = experimental_long - + # Disable the radar and let openpilot control longitudinal + # WARNING: THIS DISABLES AEB! + # If Bosch radarless, this blocks ACC messages from the camera + ret.experimentalLongitudinalAvailable = True + ret.openpilotLongitudinalControl = experimental_long ret.pcmCruise = not ret.openpilotLongitudinalControl else: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)] @@ -75,6 +73,8 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kpV = [0.25] ret.longitudinalTuning.kiV = [0.05] ret.longitudinalActuatorDelayUpperBound = 0.5 # s + if candidate in HONDA_BOSCH_RADARLESS: + ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model else: # default longitudinal tuning for all hondas ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -193,15 +193,18 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] - elif candidate == CAR.HRV: + elif candidate in (CAR.HRV, CAR.HRV_3G): ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.61 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.2 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] - ret.wheelSpeedFactor = 1.025 + if candidate == CAR.HRV: + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] + ret.wheelSpeedFactor = 1.025 + else: + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning elif candidate == CAR.ACURA_RDX: ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index c085c3fe8..05c6f2543 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -6,7 +6,7 @@ from cereal import car from common.conversions import Conversions as CV from panda.python import uds from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -87,6 +87,7 @@ class CAR: FIT = "HONDA FIT 2018" FREED = "HONDA FREED 2020" HRV = "HONDA HRV 2019" + HRV_3G = "HONDA HR-V 2023" ODYSSEY = "HONDA ODYSSEY 2018" ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019" ACURA_RDX = "ACURA RDX 2018" @@ -109,26 +110,26 @@ class HondaCarInfo(CarInfo): def init_make(self, CP: car.CarParams): if CP.carFingerprint in HONDA_BOSCH: - self.harness = Harness.bosch_b if CP.carFingerprint in HONDA_BOSCH_RADARLESS else Harness.bosch_a + self.harness_kit = HarnessKit(Harness.bosch_b) if CP.carFingerprint in HONDA_BOSCH_RADARLESS else HarnessKit(Harness.bosch_a) else: - self.harness = Harness.nidec + self.harness_kit = HarnessKit(Harness.nidec) CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { CAR.ACCORD: [ - HondaCarInfo("Honda Accord 2018-22", "All", "https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), + HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS), ], CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"), CAR.CIVIC_BOSCH: [ - HondaCarInfo("Honda Civic 2019-21", "All", "https://www.youtube.com/watch?v=4Iz1Mz5LGF8", [Footnote.CIVIC_DIESEL], 2. * CV.MPH_TO_MS), + HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS), HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS), ], CAR.CIVIC_BOSCH_DIESEL: None, # same platform CAR.CIVIC_2022: [ - HondaCarInfo("Honda Civic 2022", "All"), - HondaCarInfo("Honda Civic Hatchback 2022", "All"), + HondaCarInfo("Honda Civic 2022", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), + HondaCarInfo("Honda Civic Hatchback 2022", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), ], CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS), CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS), @@ -138,15 +139,16 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS), CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS), CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.HRV_3G: HondaCarInfo("Honda HR-V 2023", "All"), CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"), CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS), CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), CAR.PILOT: [ HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), - HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS), + HondaCarInfo("Honda Passport 2019-22", "All", min_steer_speed=12. * CV.MPH_TO_MS), ], - CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-23", min_steer_speed=12. * CV.MPH_TO_MS), CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS), } @@ -186,7 +188,7 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.UDS_VERSION_RESPONSE], bus=1, logging=True, - non_obd=True, + obd_multiplexing=False, ), ], extra_ecus=[ @@ -368,10 +370,12 @@ FW_VERSIONS = { b'57114-TWA-A050\x00\x00', b'57114-TWA-A530\x00\x00', b'57114-TWA-B520\x00\x00', + b'57114-TWB-H030\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TWA-A440\x00\x00', b'77959-TWA-L420\x00\x00', + b'77959-TWB-H220\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TWA-A010\x00\x00', @@ -385,6 +389,7 @@ FW_VERSIONS = { b'78109-TWA-A230\x00\x00', b'78109-TWA-L010\x00\x00', b'78109-TWA-L210\x00\x00', + b'78109-TWA-H210\x00\x00', ], (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TWA-A910\x00\x00', @@ -396,16 +401,19 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TWA-A070\x00\x00', b'36161-TWA-A330\x00\x00', + b'36161-TWB-H040\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TWA-A070\x00\x00', b'36802-TWA-A080\x00\x00', b'36802-TWA-A330\x00\x00', + b'36802-TWB-H060\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TVA-A160\x00\x00', b'39990-TVA-A150\x00\x00', b'39990-TVA-A340\x00\x00', + b'39990-TWB-H120\x00\x00', ], }, CAR.CIVIC: { @@ -1106,6 +1114,7 @@ FW_VERSIONS = { b'28101-5EZ-A210\x00\x00', b'28101-5EZ-A600\x00\x00', b'28101-5EZ-A430\x00\x00', + b'28101-5EZ-A700\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-RLV-4060\x00\x00', @@ -1120,6 +1129,7 @@ FW_VERSIONS = { b'37805-RLV-B220\x00\x00', b'37805-RLV-B210\x00\x00', b'37805-RLV-L160\x00\x00', + b'37805-RLV-B420\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TG7-A030\x00\x00', @@ -1133,6 +1143,7 @@ FW_VERSIONS = { b'39990-TG7-A060\x00\x00', b'39990-TG7-A070\x00\x00', b'39990-TGS-A230\x00\x00', + b'39990-TGS-A320\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-TG7-A310\x00\x00', @@ -1153,6 +1164,7 @@ FW_VERSIONS = { b'36161-TGT-A030\x00\x00', b'36161-TGT-A130\x00\x00', b'36161-TGS-A030\x00\x00', + b'36161-TGS-A220\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TG7-A020\x00\x00', @@ -1160,6 +1172,7 @@ FW_VERSIONS = { b'77959-TG7-A210\x00\x00', b'77959-TG7-Y210\x00\x00', b'77959-TGS-A010\x00\x00', + b'77959-TGS-A110\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TG7-A040\x00\x00', @@ -1193,6 +1206,7 @@ FW_VERSIONS = { b'78109-TGS-AT20\x00\x00', b'78109-TGS-AX20\x00\x00', b'78109-TGS-AJ20\x00\x00', + b'78109-TGS-AC10\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TG7-A130\x00\x00', @@ -1240,6 +1254,7 @@ FW_VERSIONS = { b'37805-5YF-A750\x00\x00', b'37805-5YF-A850\x00\x00', b'37805-5YF-A870\x00\x00', + b'37805-5YF-AD20\x00\x00', b'37805-5YF-C210\x00\x00', b'37805-5YF-C220\x00\x00', b'37805-5YF-C410\000\000', @@ -1248,16 +1263,20 @@ FW_VERSIONS = { (Ecu.vsa, 0x18da28f1, None): [ b'57114-TJB-A030\x00\x00', b'57114-TJB-A040\x00\x00', + b'57114-TJB-A120\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TJB-A040\x00\x00', b'36802-TJB-A050\x00\x00', + b'36802-TJB-A540\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TJB-A040\x00\x00', + b'36161-TJB-A530\x00\x00', ], (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TJB-A520\x00\x00', + b'54008-TJB-A530\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28102-5YK-A610\x00\x00', @@ -1265,6 +1284,7 @@ FW_VERSIONS = { b'28102-5YK-A630\x00\x00', b'28102-5YK-A700\x00\x00', b'28102-5YK-A711\x00\x00', + b'28102-5YK-A800\x00\x00', b'28102-5YL-A620\x00\x00', b'28102-5YL-A700\x00\x00', b'28102-5YL-A711\x00\x00', @@ -1276,6 +1296,7 @@ FW_VERSIONS = { b'78109-TJB-AB10\x00\x00', b'78109-TJB-AD10\x00\x00', b'78109-TJB-AF10\x00\x00', + b'78109-TJB-AQ20\x00\x00', b'78109-TJB-AR10\x00\x00', b'78109-TJB-AS10\000\000', b'78109-TJB-AU10\x00\x00', @@ -1287,22 +1308,26 @@ FW_VERSIONS = { ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TJB-A040\x00\x00', + b'77959-TJB-A120\x00\x00', b'77959-TJB-A210\x00\x00', ], (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ b'46114-TJB-A040\x00\x00', b'46114-TJB-A050\x00\x00', b'46114-TJB-A060\x00\x00', + b'46114-TJB-A120\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TJB-A040\x00\x00', b'38897-TJB-A110\x00\x00', b'38897-TJB-A120\x00\x00', + b'38897-TJB-A220\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TJB-A030\x00\x00', b'39990-TJB-A040\x00\x00', - b'39990-TJB-A130\x00\x00' + b'39990-TJB-A070\x00\x00', + b'39990-TJB-A130\x00\x00', ], }, CAR.RIDGELINE: { @@ -1402,6 +1427,32 @@ FW_VERSIONS = { b'78109-THW-A110\x00\x00', ], }, + CAR.HRV_3G: { + (Ecu.eps, 0x18DA30F1, None): [ + b'39990-3W0-A030\x00\x00', + ], + (Ecu.gateway, 0x18DAEFF1, None): [ + b'38897-3W1-A010\x00\x00', + ], + (Ecu.srs, 0x18DA53F1, None): [ + b'77959-3V0-A820\x00\x00', + ], + (Ecu.combinationMeter, 0x18DA60F1, None): [ + b'78108-3V1-A220\x00\x00', + ], + (Ecu.vsa, 0x18DA28F1, None): [ + b'57114-3W0-A040\x00\x00', + ], + (Ecu.transmission, 0x18DA1EF1, None): [ + b'28101-6EH-A010\x00\x00', + ], + (Ecu.programmedFuelInjection, 0x18DA10F1, None): [ + b'37805-6CT-A710\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18DA2BF1, None): [ + b'46114-3W0-A020\x00\x00', + ], + }, CAR.ACURA_ILX: { (Ecu.gateway, 0x18daeff1, None): [ b'38897-TX6-A010\x00\x00', @@ -1520,6 +1571,7 @@ DBC = { CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.HRV_3G: dbc_dict('honda_civic_ex_2022_can_generated', None), CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), @@ -1539,6 +1591,6 @@ HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, CAR.PILOT, CAR.RIDGELINE} HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, - CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022} -HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G} -HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022} + CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G} +HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G, CAR.HRV_3G} +HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022, CAR.HRV_3G} diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 1e6f78af2..ac74d2cc5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -3,8 +3,9 @@ from common.conversions import Conversions as CV from common.numpy_fast import clip from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.hyundai import hyundaicanfd, hyundaican +from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -44,6 +45,7 @@ def process_hud_alert(enabled, fingerprint, hud_control): class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP + self.CAN = CanBus(CP) self.params = CarControllerParams(CP) self.packer = CANPacker(dbc_name) self.angle_limit_counter = 0 @@ -60,7 +62,7 @@ class CarController: # steering torque new_steer = int(round(actuators.steer * self.params.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) if not CC.latActive: apply_steer = 0 @@ -85,12 +87,12 @@ class CarController: # for longitudinal control, either radar or ADAS driving ECU addr, bus = 0x7d0, 0 if self.CP.flags & HyundaiFlags.CANFD_HDA2.value: - addr, bus = 0x730, 5 + addr, bus = 0x730, self.CAN.ECAN can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus]) # for blinkers if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: - can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 5]) + can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN]) # >90 degree steering fault prevention # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault @@ -112,25 +114,25 @@ class CarController: hda2_long = hda2 and self.CP.openpilotLongitudinalControl # steering control - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer)) + can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, lat_active, apply_steer)) # disable LFA on HDA2 if self.frame % 5 == 0 and hda2: - can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, CS.cam_0x2a4)) + can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, self.CAN, CS.cam_0x2a4)) # LFA and HDA icons if self.frame % 5 == 0 and (not hda2 or hda2_long): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled)) + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled)) # blinkers if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: - can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.frame, CC.leftBlinker, CC.rightBlinker)) + can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker)) if self.CP.openpilotLongitudinalControl: if hda2: - can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) + can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units)) self.accel_last = accel else: @@ -139,11 +141,11 @@ class CarController: # cruise cancel if CC.cruiseControl.cancel: if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, CS.cruise_info)) + can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CAN, CS.cruise_info)) self.last_button_frame = self.frame else: for _ in range(20): - can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, CS.buttons_counter+1, Buttons.CANCEL)) + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL)) self.last_button_frame = self.frame # cruise standstill resume @@ -153,7 +155,7 @@ class CarController: pass else: for _ in range(20): - can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, CS.buttons_counter+1, Buttons.RES_ACCEL)) + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL)) self.last_button_frame = self.frame else: can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active, diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 22934c05b..cec2c3e69 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -6,8 +6,8 @@ from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.hyundai.hyundaicanfd import get_e_can_bus -from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams +from selfdrive.car.hyundai.hyundaicanfd import CanBus +from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from selfdrive.car.interfaces import CarStateBase PREV_BUTTON_SAMPLES = 8 @@ -27,9 +27,9 @@ class CarState(CarStateBase): "GEAR_SHIFTER" if CP.carFingerprint in CANFD_CAR: self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"] - elif self.CP.carFingerprint in FEATURES["use_cluster_gears"]: + elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]: self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"] - elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: + elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]: self.shifter_values = can_define.dv["TCU12"]["CUR_GR"] else: # preferred and elect gear methods use same definition self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"] @@ -123,11 +123,11 @@ class CarState(CarStateBase): # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, # as this seems to be standard over all cars, but is not the preferred method. - if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: + if self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]: gear = cp.vl["CLU15"]["CF_Clu_Gear"] - elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: + elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]: gear = cp.vl["TCU12"]["CUR_GR"] - elif self.CP.carFingerprint in FEATURES["use_elect_gears"]: + elif self.CP.carFingerprint in CAN_GEARS["use_elect_gears"]: gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"] else: gear = cp.vl["LVR12"]["CF_Lvr_Gear"] @@ -354,12 +354,12 @@ class CarState(CarStateBase): ("EMS16", 100), ] - if CP.carFingerprint in FEATURES["use_cluster_gears"]: + if CP.carFingerprint in CAN_GEARS["use_cluster_gears"]: signals.append(("CF_Clu_Gear", "CLU15")) - elif CP.carFingerprint in FEATURES["use_tcu_gears"]: + elif CP.carFingerprint in CAN_GEARS["use_tcu_gears"]: signals.append(("CUR_GR", "TCU12")) checks.append(("TCU12", 100)) - elif CP.carFingerprint in FEATURES["use_elect_gears"]: + elif CP.carFingerprint in CAN_GEARS["use_elect_gears"]: signals.append(("Elect_Gear_Shifter", "ELECT_GEAR")) checks.append(("ELECT_GEAR", 20)) else: @@ -516,7 +516,7 @@ class CarState(CarStateBase): ("ACCELERATOR_BRAKE_ALT", 100), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, get_e_can_bus(CP)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).ECAN) @staticmethod def get_cam_can_parser_canfd(CP): @@ -543,4 +543,4 @@ class CarState(CarStateBase): ("SCC_CONTROL", 50), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).CAM) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index af7239571..3717a4590 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,14 +1,44 @@ +import math + from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags -def get_e_can_bus(CP): - # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars - # have a different harness than the HDA1 and non-HDA variants in order to split - # a different bus, since the steering is done by different ECUs. - return 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 + +class CanBus: + def __init__(self, CP, hda2=None, fingerprint=None): + if CP is None: + assert None not in (hda2, fingerprint) + num = math.ceil(max([k for k, v in fingerprint.items() if len(v)], default=1) / 4) + else: + hda2 = CP.flags & HyundaiFlags.CANFD_HDA2.value + num = len(CP.safetyConfigs) + + # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars + # have a different harness than the HDA1 and non-HDA variants in order to split + # a different bus, since the steering is done by different ECUs. + self._a, self._e = 1, 0 + if hda2: + self._a, self._e = 0, 1 + + offset = 4*(num - 1) + self._a += offset + self._e += offset + self._cam = 2 + offset + + @property + def ECAN(self): + return self._e + + @property + def ACAN(self): + return self._a + + @property + def CAM(self): + return self._cam -def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): +def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer): ret = [] @@ -26,45 +56,45 @@ def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): if CP.flags & HyundaiFlags.CANFD_HDA2: if CP.openpilotLongitudinalControl: - ret.append(packer.make_can_msg("LFA", 5, values)) - ret.append(packer.make_can_msg("LKAS", 4, values)) + ret.append(packer.make_can_msg("LFA", CAN.ECAN, values)) + ret.append(packer.make_can_msg("LKAS", CAN.ACAN, values)) else: - ret.append(packer.make_can_msg("LFA", 4, values)) + ret.append(packer.make_can_msg("LFA", CAN.ECAN, values)) return ret -def create_cam_0x2a4(packer, camera_values): +def create_cam_0x2a4(packer, CAN, camera_values): camera_values.update({ "BYTE7": 0, }) - return packer.make_can_msg("CAM_0x2a4", 4, camera_values) + return packer.make_can_msg("CAM_0x2a4", CAN.ACAN, camera_values) -def create_buttons(packer, CP, cnt, btn): +def create_buttons(packer, CP, CAN, cnt, btn): values = { "COUNTER": cnt, "SET_ME_1": 1, "CRUISE_BUTTONS": btn, } - bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 6 + bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM return packer.make_can_msg("CRUISE_BUTTONS", bus, values) -def create_acc_cancel(packer, CP, cruise_info_copy): +def create_acc_cancel(packer, CAN, cruise_info_copy): values = cruise_info_copy values.update({ "ACCMode": 4, }) - return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_lfahda_cluster(packer, CP, enabled): +def create_lfahda_cluster(packer, CAN, enabled): values = { "HDA_ICON": 1 if enabled else 0, "LFA_ICON": 2 if enabled else 0, } - return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values) + return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_override, set_speed): +def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -92,15 +122,15 @@ def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_ove "DISTANCE_SETTING": 4, } - return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_spas_messages(packer, frame, left_blink, right_blink): +def create_spas_messages(packer, CAN, frame, left_blink, right_blink): ret = [] values = { } - ret.append(packer.make_can_msg("SPAS1", 5, values)) + ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values)) blink = 0 if left_blink: @@ -110,12 +140,12 @@ def create_spas_messages(packer, frame, left_blink, right_blink): values = { "BLINKER_CONTROL": blink, } - ret.append(packer.make_can_msg("SPAS2", 5, values)) + ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values)) return ret -def create_adrv_messages(packer, frame): +def create_adrv_messages(packer, CAN, frame): # messages needed to car happy after disabling # the ADAS Driving ECU to do longitudinal control @@ -123,7 +153,7 @@ def create_adrv_messages(packer, frame): values = { } - ret.append(packer.make_can_msg("ADRV_0x51", 4, values)) + ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values)) if frame % 2 == 0: values = { @@ -133,7 +163,7 @@ def create_adrv_messages(packer, frame): 'SET_ME_FC': 0xfc, 'SET_ME_9': 0x9, } - ret.append(packer.make_can_msg("ADRV_0x160", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values)) if frame % 5 == 0: values = { @@ -142,25 +172,25 @@ def create_adrv_messages(packer, frame): 'SET_ME_TMP_F': 0xf, 'SET_ME_TMP_F_2': 0xf, } - ret.append(packer.make_can_msg("ADRV_0x1ea", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values)) values = { 'SET_ME_E1': 0xe1, 'SET_ME_3A': 0x3a, } - ret.append(packer.make_can_msg("ADRV_0x200", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values)) if frame % 20 == 0: values = { 'SET_ME_15': 0x15, } - ret.append(packer.make_can_msg("ADRV_0x345", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values)) if frame % 100 == 0: values = { 'SET_ME_22': 0x22, 'SET_ME_41': 0x41, } - ret.append(packer.make_can_msg("ADRV_0x1da", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values)) return ret diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index d2cc5b4ec..59d7319de 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,6 +2,7 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV +from selfdrive.car.hyundai.hyundaicanfd import CanBus from 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, Buttons from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config @@ -18,7 +19,7 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "hyundai" ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None @@ -27,17 +28,20 @@ class CarInterface(CarInterfaceBase): # added to selfdrive/car/tests/routes.py, we can remove it from this list. ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, } + hda2 = Ecu.adas in [fw.ecu for fw in car_fw] + CAN = CanBus(None, hda2, fingerprint) + if candidate in CANFD_CAR: # detect HDA2 with ADAS Driving ECU - if Ecu.adas in [fw.ecu for fw in car_fw]: + if hda2: ret.flags |= HyundaiFlags.CANFD_HDA2.value else: # non-HDA2 - if 0x1cf not in fingerprint[4]: + if 0x1cf not in fingerprint[CAN.ECAN]: ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value # ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead - if 0x130 not in fingerprint[4]: - if 0x40 not in fingerprint[4]: + if 0x130 not in fingerprint[CAN.ECAN]: + if 0x40 not in fingerprint[CAN.ECAN]: ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value else: ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value @@ -103,18 +107,13 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.6 ret.steerRatio = 13.42 # Spec tire_stiffness_factor = 0.385 - elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): + elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV): ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 ret.steerRatio = 13.73 # Spec tire_stiffness_factor = 0.385 - if candidate not in (CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): + if candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019): ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate == CAR.IONIQ_PHEV_2019: - ret.mass = 1550. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/us/en/vehicles/2019-ioniq-plug-in-hybrid/compare-specs - ret.wheelbase = 2.7 - ret.steerRatio = 13.73 - ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.VELOSTER: ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 @@ -140,10 +139,10 @@ class CarInterface(CarInterfaceBase): ret.mass = 1985. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN): - ret.mass = 3452. * CV.LB_TO_KG + STD_CARGO_KG # average of all the cars + elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN): + ret.mass = 3543. * CV.LB_TO_KG + STD_CARGO_KG # average of all the cars ret.wheelbase = 2.7 - ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec + ret.steerRatio = 13.6 # average of all the cars tire_stiffness_factor = 0.385 if candidate == CAR.KIA_NIRO_PHEV: ret.minSteerSpeed = 32 * CV.MPH_TO_MS @@ -230,6 +229,10 @@ class CarInterface(CarInterfaceBase): ret.mass = 2200 ret.wheelbase = 3.15 ret.steerRatio = 12.069 + elif candidate == CAR.GENESIS_GV80: + ret.mass = 2258. + STD_CARGO_KG + ret.wheelbase = 2.95 + ret.steerRatio = 14.14 # *** longitudinal control *** if candidate in CANFD_CAR: @@ -252,22 +255,23 @@ class CarInterface(CarInterfaceBase): # *** feature detection *** if candidate in CANFD_CAR: - bus = 5 if ret.flags & HyundaiFlags.CANFD_HDA2 else 4 - ret.enableBsm = 0x1e5 in fingerprint[bus] + ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN] else: ret.enableBsm = 0x58b in fingerprint[0] # *** panda safety config *** if candidate in CANFD_CAR: - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput), - get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd)] + cfgs = [get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd), ] + if CAN.ECAN >= 4: + cfgs.insert(0, get_safety_config(car.CarParams.SafetyModel.noOutput)) + ret.safetyConfigs = cfgs if ret.flags & HyundaiFlags.CANFD_HDA2: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC else: if candidate in LEGACY_SAFETY_MODE_CAR: # these cars require a special panda safety mode due to missing counters and checksums in the messages @@ -302,12 +306,12 @@ class CarInterface(CarInterfaceBase): if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value): addr, bus = 0x7d0, 0 if CP.flags & HyundaiFlags.CANFD_HDA2.value: - addr, bus = 0x730, 5 + addr, bus = 0x730, CanBus(CP).ECAN disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') # for blinkers if CP.flags & HyundaiFlags.ENABLE_BLINKERS: - disable_ecu(logcan, sendcan, bus=5, addr=0x7B1, com_cont_req=b'\x28\x83\x01') + disable_ecu(logcan, sendcan, bus=CanBus(CP.ECAN), addr=0x7B1, com_cont_req=b'\x28\x83\x01') def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index d0a4e4dd1..8be3a5fe5 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -6,7 +6,7 @@ from cereal import car from panda.python import uds from common.conversions import Conversions as CV from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -100,6 +100,7 @@ class CAR: KIA_K5_2021 = "KIA K5 2021" KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020" KIA_NIRO_EV = "KIA NIRO EV 2020" + KIA_NIRO_EV_2ND_GEN = "KIA NIRO EV 2ND GEN" KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" KIA_NIRO_HEV_2ND_GEN = "KIA NIRO HYBRID 2ND GEN" @@ -124,6 +125,7 @@ class CAR: GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN" GENESIS_G80 = "GENESIS G80 2017" GENESIS_G90 = "GENESIS G90 2017" + GENESIS_GV80 = "GENESIS GV80 2023" class Footnote(Enum): @@ -145,107 +147,114 @@ class HyundaiCarInfo(CarInfo): CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.ELANTRA: [ - HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b), - HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness=Harness.hyundai_e), - HyundaiCarInfo("Hyundai i30 2017-19", harness=Harness.hyundai_e), + HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_b)), + HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness_kit=HarnessKit(Harness.hyundai_e)), + HyundaiCarInfo("Hyundai i30 2017-19", harness_kit=HarnessKit(Harness.hyundai_e)), ], - CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), - CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), + CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness_kit=HarnessKit(Harness.hyundai_k)), + CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness_kit=HarnessKit(Harness.hyundai_k)), CAR.HYUNDAI_GENESIS: [ - HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages - HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), + HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_j)), # TODO: check 2015 packages + HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_j)), ], - CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c), - CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness=Harness.hyundai_h), # TODO: confirm 2020-21 harness - CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c), - CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", harness=Harness.hyundai_h), - CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness=Harness.hyundai_c), - CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-21", "All", harness=Harness.hyundai_h), - CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", harness=Harness.hyundai_b), - CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness=Harness.hyundai_g), - CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", harness=Harness.hyundai_o), - CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness=Harness.hyundai_i), # TODO: check packages - CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness=Harness.hyundai_d), - CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", "https://youtu.be/VnHzSTygTS4", harness=Harness.hyundai_l), - CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness=Harness.hyundai_l), - CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness=Harness.hyundai_l), - CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", "https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a), - CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e), + CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness_kit=HarnessKit(Harness.hyundai_h)), # TODO: confirm 2020-21 harness + CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", harness_kit=HarnessKit(Harness.hyundai_h)), + CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_h)), + CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", harness_kit=HarnessKit(Harness.hyundai_b)), + CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness_kit=HarnessKit(Harness.hyundai_g)), + CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", harness_kit=HarnessKit(Harness.hyundai_o)), + CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness_kit=HarnessKit(Harness.hyundai_i)), # TODO: check packages + CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness_kit=HarnessKit(Harness.hyundai_d)), + CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", video_link="https://youtu.be/VnHzSTygTS4", harness_kit=HarnessKit(Harness.hyundai_l)), + CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness_kit=HarnessKit(Harness.hyundai_l)), + CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness_kit=HarnessKit(Harness.hyundai_l)), + CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness_kit=HarnessKit(Harness.hyundai_e)), CAR.TUCSON: [ - HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l), - HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness=Harness.hyundai_l), + HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_l)), + HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness_kit=HarnessKit(Harness.hyundai_l)), ], CAR.PALISADE: [ - HyundaiCarInfo("Hyundai Palisade 2020-22", "All", "https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h), - HyundaiCarInfo("Kia Telluride 2020-22", "All", harness=Harness.hyundai_h), + HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness_kit=HarnessKit(Harness.hyundai_h)), + HyundaiCarInfo("Kia Telluride 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_h)), ], - CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e), - CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness=Harness.hyundai_a), + CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_e)), + CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_a)), CAR.IONIQ_5: [ - HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", harness=Harness.hyundai_q), - HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", harness=Harness.hyundai_k), - HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", harness=Harness.hyundai_q), + HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_q)), + HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", harness_kit=HarnessKit(Harness.hyundai_k)), + HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", harness_kit=HarnessKit(Harness.hyundai_q)), ], CAR.TUCSON_4TH_GEN: [ - HyundaiCarInfo("Hyundai Tucson 2022", harness=Harness.hyundai_n), - HyundaiCarInfo("Hyundai Tucson 2023", "All", harness=Harness.hyundai_n), + HyundaiCarInfo("Hyundai Tucson 2022", harness_kit=HarnessKit(Harness.hyundai_n)), + HyundaiCarInfo("Hyundai Tucson 2023", "All", harness_kit=HarnessKit(Harness.hyundai_n)), ], - CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022", "All", harness=Harness.hyundai_n), - CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2021-22", harness=Harness.hyundai_n), + CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_n)), + CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", harness_kit=HarnessKit(Harness.hyundai_n)), # Kia - CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), - CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness=Harness.hyundai_a), - CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness=Harness.hyundai_a), - CAR.KIA_NIRO_EV: [ - HyundaiCarInfo("Kia Niro EV 2019", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), - HyundaiCarInfo("Kia Niro EV 2020", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f), - HyundaiCarInfo("Kia Niro EV 2021", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), - HyundaiCarInfo("Kia Niro EV 2022", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), + CAR.KIA_FORTE: [ + HyundaiCarInfo("Kia Forte 2019-21", harness_kit=HarnessKit(Harness.hyundai_g)), + HyundaiCarInfo("Kia Forte 2023", harness_kit=HarnessKit(Harness.hyundai_e)), ], + CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_NIRO_EV: [ + HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_h)), + HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_f)), + HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_c)), + HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_h)), + ], + CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", harness_kit=HarnessKit(Harness.hyundai_a)), CAR.KIA_NIRO_PHEV: [ - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness=Harness.hyundai_d), + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_c)), + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness_kit=HarnessKit(Harness.hyundai_d)), ], CAR.KIA_NIRO_HEV_2021: [ - HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify - HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h), + HyundaiCarInfo("Kia Niro Hybrid 2021-22", harness_kit=HarnessKit(Harness.hyundai_f)), # TODO: 2021 could be hyundai_d, verify ], - CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", harness=Harness.hyundai_a), - CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018 - CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g), + CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness_kit=HarnessKit(Harness.hyundai_b)), # TODO: may support 2016, 2018 + CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness_kit=HarnessKit(Harness.hyundai_g)), CAR.KIA_OPTIMA_H: [ HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years HyundaiCarInfo("Kia Optima Hybrid 2019"), ], - CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a), - CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", harness=Harness.hyundai_n), + CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", harness_kit=HarnessKit(Harness.hyundai_n)), CAR.KIA_SORENTO: [ - HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", "https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c), - HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_e), + HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness_kit=HarnessKit(Harness.hyundai_c)), + HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness_kit=HarnessKit(Harness.hyundai_e)), ], - CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2022-23", harness=Harness.hyundai_k), - CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", harness=Harness.hyundai_a), - CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", harness=Harness.hyundai_n), - CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c), - CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness=Harness.hyundai_k), - CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e), + CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", harness_kit=HarnessKit(Harness.hyundai_k)), + CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", harness_kit=HarnessKit(Harness.hyundai_n)), + CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness_kit=HarnessKit(Harness.hyundai_k)), + CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness_kit=HarnessKit(Harness.hyundai_e)), CAR.KIA_EV6: [ - HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", harness=Harness.hyundai_p), - HyundaiCarInfo("Kia EV6 (without HDA II) 2022", "Highway Driving Assist", harness=Harness.hyundai_l), - HyundaiCarInfo("Kia EV6 (with HDA II) 2022", "Highway Driving Assist II", harness=Harness.hyundai_p) + HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_p)), + HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", harness_kit=HarnessKit(Harness.hyundai_l)), + HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", harness_kit=HarnessKit(Harness.hyundai_p)) ], # Genesis CAR.GENESIS_GV60_EV_1ST_GEN: [ - HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", harness=Harness.hyundai_a), - HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", harness=Harness.hyundai_k), + HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", harness_kit=HarnessKit(Harness.hyundai_a)), + HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", harness_kit=HarnessKit(Harness.hyundai_k)), ], - CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f), - CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f), - CAR.GENESIS_GV70_1ST_GEN: HyundaiCarInfo("Genesis GV70 2022-23", "All", harness=Harness.hyundai_l), - CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness=Harness.hyundai_h), - CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c), + CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness_kit=HarnessKit(Harness.hyundai_f)), + CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness_kit=HarnessKit(Harness.hyundai_f)), + CAR.GENESIS_GV70_1ST_GEN: [ + HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_l)), + HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_m)), + ], + CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness_kit=HarnessKit(Harness.hyundai_h)), + CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", harness_kit=HarnessKit(Harness.hyundai_m)), } class Buttons: @@ -335,10 +344,15 @@ FINGERPRINTS = { HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(0xf100) # Long description + +HYUNDAI_VERSION_REQUEST_ALT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf110) # Alt long description + HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \ p16(0xf100) + HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) FW_QUERY_CONFIG = FwQueryConfig( @@ -355,22 +369,46 @@ FW_QUERY_CONFIG = FwQueryConfig( [HYUNDAI_VERSION_RESPONSE], whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar], ), - # CAN-FD queries (camera) + + # CAN-FD queries (from camera) + # TODO: combine shared whitelists with CAN requests Request( [HYUNDAI_VERSION_REQUEST_LONG], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar], - bus=4, + whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac], + bus=0, + auxiliary=True, ), Request( [HYUNDAI_VERSION_REQUEST_LONG], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar], - bus=5, + whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.hvac], + bus=1, + auxiliary=True, + obd_multiplexing=False, + ), + + # CAN-FD debugging queries + Request( + [HYUNDAI_VERSION_REQUEST_ALT], + [HYUNDAI_VERSION_RESPONSE], + whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac], + bus=0, + auxiliary=True, + ), + Request( + [HYUNDAI_VERSION_REQUEST_ALT], + [HYUNDAI_VERSION_RESPONSE], + whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac], + bus=1, + auxiliary=True, + obd_multiplexing=False, ), ], extra_ecus=[ (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms + (Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms) + (Ecu.hvac, 0x7b3, None), # HVAC Control Assembly (Ecu.cornerRadar, 0x7b7, None), ], ) @@ -424,6 +462,7 @@ FW_VERSIONS = { b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2200 ', b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ', b'\xf1\x00AEhe SCC F-CUP 1.00 1.02 99110-G2100 ', + b'\xf1\x00AEhe SCC FHCUP 1.00 1.00 99110-G2600 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2510 4APHC101', @@ -511,8 +550,6 @@ FW_VERSIONS = { b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ', b'\xf1\x00DN89110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ', - b'\xf1\x8799110L0000\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ', - b'\xf1\x8799110L0000\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', @@ -521,11 +558,11 @@ FW_VERSIONS = { b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300', + b'\xf1\x00DN ESC \x07 107"\x08\x07 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', - b'\xf1\x8758910-L0300\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300', b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', ], (Ecu.engine, 0x7e0, None): [ @@ -537,6 +574,7 @@ FW_VERSIONS = { b'\xf1\x82DNCVN5GMCCXXXG2B', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DNDWN5TMDCXXXJ1A', + b'\xf1\x8739110-2S041\xf1\x81HM6M1_0a0_M10', b'\xf1\x87391162M003', b'\xf1\x87391162M013', b'\xf1\x87391162M023', @@ -545,10 +583,12 @@ FW_VERSIONS = { b'HM6M2_0a0_BD0', b'\xf1\x8739110-2S278\xf1\x82DNDVD5GMCCXXXL5B', b'\xf1\x8739110-2S041\xf1\x81HM6M1_0a0_M00', + b'\xf1\x8739110-2S042\xf1\x81HM6M1_0a0_M00', b'\xf1\x81HM6M1_0a0_G20', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC102', b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101', @@ -564,6 +604,7 @@ FW_VERSIONS = { b'\xf1\x8757700-L0000\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100', b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC102', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0200\x00 4DNAC102', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422', @@ -585,8 +626,10 @@ FW_VERSIONS = { b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:', b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB4\x00\x00\x00\x00\x00\x00g!l[', + b'\xf1\x00HT6WA280BLHT6WAE10A1SDN8G25NB5\x00\x00\x00\x00\x00\x00\xe0t\xa9\xba', b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', b'\xf1\x00T02601BL T02832A1 VDN8T25XXX832NS8G\x0e\xfeE', + b'\xf1\x00T02601BL T02900A1 VDN8T25XXX900NSCF\xe4!Y', b'\xf1\x87954A02N060\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', b'\xf1\x87SAKFBA2926554GJ2VefVww\x87xwwwww\x88\x87xww\x87wTo\xfb\xffvUo\xff\x8d\x16\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', b'\xf1\x87SAKFBA3030524GJ2UVugww\x97yx\x88\x87\x88vw\x87gww\x87wto\xf9\xfffUo\xff\xa2\x0c\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', @@ -750,11 +793,10 @@ FW_VERSIONS = { CAR.SANTA_FE_2022: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ', - b'\xf1\x8799110S1500\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ', - b'\xf1\x8799110S1500\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ', b'\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ', ], (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00TM ESC \x01 102!\x04\x03 58910-S2DA0', b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0', b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', @@ -765,6 +807,8 @@ FW_VERSIONS = { b'\xf1\x00TM ESC \x04 101 \x08\x04 58910-S2GA0', ], (Ecu.engine, 0x7e0, None): [ + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_L50', + b'\xf1\x81HM6M1_0a0_H00', b'\xf1\x82TACVN5GMI3XXXH0A', b'\xf1\x82TMBZN5TMD3XXXG2E', b'\xf1\x82TACVN5GSI3XXXH0A', @@ -785,6 +829,9 @@ FW_VERSIONS = { b'\xf1\x00TMA MFC AT USA LHD 1.00 1.01 99211-S2500 210205', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00HT6WA280BLHT6WAD00A1STM2G25NH2\x00\x00\x00\x00\x00\x00\xf8\xc0\xc3\xaa', + b'\xf1\x00HT6WA280BLHT6WAD00A1STM4G25NH1\x00\x00\x00\x00\x00\x00\x9cl\x04\xbc', + b'\xf1\x00T02601BL T02900A1 VTMPT25XXX900NSA\xf3\xf4Uj', b'\xf1\x87SDMXCA9087684GN1VfvgUUeVwwgwwwwwffffU?\xfb\xff\x97\x88\x7f\xff+\xa4\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', b'\xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', @@ -883,18 +930,23 @@ FW_VERSIONS = { CAR.KIA_STINGER_2022: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CK__ SCC F-CUP 1.00 1.00 99110-J5500 ', + b'\xf1\x00CK__ SCC FHCUP 1.00 1.00 99110-J5500 ', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x81640R0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81HM6M1_0a0_H00', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5380 4C2VR503', + b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5300 4C2CL503', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CK MFC AT AUS RHD 1.00 1.00 99211-J5500 210622', + b'\xf1\x00CK MFC AT KOR LHD 1.00 1.00 99211-J5500 210622', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x87VCNLF11383972DK1vffV\x99\x99\x89\x98\x86eUU\x88wg\x89vfff\x97fff\x99\x87o\xff"\xc1\xf1\x81E30\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E30\x00\x00\x00\x00\x00\x00\x00SCK0T33GH0\xbe`\xfb\xc6', + b'\xf1\x00bcsh8p54 E31\x00\x00\x00\x00\x00\x00\x00SCK0T25KH2B\xfbI\xe2', ], }, CAR.PALISADE: { @@ -1026,11 +1078,23 @@ FW_VERSIONS = { ], }, CAR.GENESIS_G70: { - (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ', ], - (Ecu.engine, 0x7e0, None): [b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.eps, 0x7d4, None): [b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', ], - (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ], - (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ', + b'\xf1\x00IK__ SCC F-CUP 1.00 1.01 96400-G9100 ', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', + b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', + ], }, CAR.GENESIS_G70_2020: { (Ecu.eps, 0x7d4, None): [ @@ -1107,21 +1171,27 @@ FW_VERSIONS = { b'\xf1\x00BD MDPS C 1.00 1.02 56310-XX000 4BD2C102', b'\xf1\x00BD MDPS C 1.00 1.08 56310/M6300 4BDDC108', b'\xf1\x00BD MDPS C 1.00 1.08 56310M6300\x00 4BDDC108', + b'\xf1\x00BDm MDPS C A.01 1.03 56310M7800\x00 4BPMC103', ], (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00BD LKAS AT USA LHD 1.00 1.04 95740-M6000 J33', + b'\xf1\x00BDP LKAS AT USA LHD 1.00 1.05 99211-M6500 744', ], (Ecu.fwdRadar, 0x7D0, None): [ b'\xf1\x00BD__ SCC H-CUP 1.00 1.02 99110-M6000 ', + b'\xf1\x00BDPE_SCC FHCUPC 1.00 1.04 99110-M6500\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x01TBDM1NU06F200H01', b'391182B945\x00', + b'\xf1\x81616F2051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x8758900-M7AB0 \xf1\x816VQRAD00127.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006V2B0_C2\x00\x006V2C6051\x00\x00CBD0N20NL1\x00\x00\x00\x00', b'\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\x00\x00\x00\x00', b"\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\xcf\x1e'\xc3", ], @@ -1264,6 +1334,15 @@ FW_VERSIONS = { b'\xf1\x00DEE MFC AT KOR LHD 1.00 1.03 95740-Q4000 180821', ], }, + CAR.KIA_NIRO_EV_2ND_GEN: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00SG2_ RDR ----- 1.00 1.01 99110-AT000 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00SG2EMFC AT EUR LHD 1.01 1.09 99211-AT000 220801', + b'\xf1\x00SG2EMFC AT USA LHD 1.01 1.09 99211-AT000 220801', + ], + }, CAR.KIA_NIRO_PHEV: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1445,25 +1524,30 @@ FW_VERSIONS = { b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.05 99210-AA000 210930', b'\xf1\000CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819', b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.07 99210-AA000 220426', + b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.08 99210-AA000 220728', + b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.09 99210-AA000 221108', ], (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\000CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', + b'\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', b'\xf1\x8799110BY000\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00CN7 MDPS C 1.00 1.03 56310BY0500 4CNHC103', b'\xf1\x8756310/BY050\xf1\x00CN7 MDPS C 1.00 1.03 56310/BY050 4CNHC103', b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102', + b'\xf1\x00CN7 MDPS C 1.00 1.04 56310BY050\x00 4CNHC104', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa', b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000', b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa', b'\xf1\x816U3K3051\x00\x00\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\x00\x00\x00\x00', + b'\xf1\x006U3L0_C2\x00\x006U3K9051\x00\x00HCN0G16NS1\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816H6G8051\x00\x00\x00\x00\x00\x00\x00\x00', ] }, CAR.KONA_HEV: { @@ -1553,6 +1637,9 @@ FW_VERSIONS = { b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.06 99210-CV000 220328', b'\xf1\x00CV1 MFC AT EUR RHD 1.00 1.00 99210-CV100 220630', b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.00 99210-CV100 220630', + b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.04 99210-CV000 210823', + b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.05 99210-CV000 211027', + b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.06 99210-CV000 220328', ], }, CAR.IONIQ_5: { @@ -1563,16 +1650,21 @@ FW_VERSIONS = { b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.02 99211-GI010 211206', b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614', + b'\xf1\x00NE1 MFC AT KOR LHD 1.00 1.05 99211-GI010 220614', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007', b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.01 99211-GI010 211007', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.02 99211-GI010 211206', + b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.03 99211-GI010 220401', ], }, CAR.TUCSON_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9210 14G', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9240 14T', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW010 14X', ], (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ', b'\xf1\x00NX4__ 1.01 1.00 99110-N9100 ', ], }, @@ -1581,13 +1673,16 @@ FW_VERSIONS = { b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9240 14Q', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9220 14K', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9100 14A', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9250 14W', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ', + b'\xf1\x00NX4__ 1.01 1.00 99110-N9100 ', ], }, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NQ5 FR_CMR AT GEN LHD 1.00 1.00 99211-P1060 665', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1060 665', ], (Ecu.fwdRadar, 0x7d0, None): [ @@ -1597,9 +1692,11 @@ FW_VERSIONS = { CAR.SANTA_CRUZ_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW000 14M', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW010 14X', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.00 1.00 99110-K5000 ', + b'\xf1\x00NX4__ 1.01 1.00 99110-K5000 ', ], }, CAR.KIA_SPORTAGE_5TH_GEN: { @@ -1610,22 +1707,26 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NQ5__ 1.00 1.02 99110-P1000 ', b'\xf1\x00NQ5__ 1.00 1.03 99110-P1000 ', + b'\xf1\x00NQ5__ 1.01 1.03 99110-P1000 ', ], }, CAR.GENESIS_GV70_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.04 99211-AR000 210204', b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR200 220125', + b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR300 220125', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ', b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR200 ', + b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR300 ', ], }, CAR.GENESIS_GV60_EV_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU100 211215', b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU000 211215', + b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.03 99211-CU000 221118', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JW1_ RDR ----- 1.00 1.00 99110-CU000 ', @@ -1634,19 +1735,30 @@ FW_VERSIONS = { CAR.KIA_SORENTO_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00MQ4 MFC AT USA LHD 1.00 1.05 99210-R5000 210623', + b'\xf1\x00MQ4 MFC AT USA LHD 1.00 1.03 99210-R5000 200903', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00MQ4_ SCC FHCUP 1.00 1.06 99110-P2000 ', + b'\xf1\x00MQ4_ SCC F-CUP 1.00 1.06 99110-P2000 ', ], }, CAR.KIA_NIRO_HEV_2ND_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.08 99211-AT000 220531', + b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.09 99211-AT000 220801', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00SG2_ RDR ----- 1.00 1.01 99110-AT000 ', ], }, + CAR.GENESIS_GV80: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00JX1 MFC AT USA LHD 1.00 1.02 99211-T6110 220513', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JX1_ SCC FHCUP 1.00 1.01 99110-T6100 ', + ], + }, } CHECKSUM = { @@ -1654,23 +1766,23 @@ CHECKSUM = { "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], } -FEATURES = { +CAN_GEARS = { # which message has the gear "use_cluster_gears": {CAR.ELANTRA, CAR.KONA}, "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022, CAR.KIA_K5_HEV_2020}, } -CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN} +CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN, CAR.GENESIS_GV80} # 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} +CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80} # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020, CAR.KIA_NIRO_HEV_2ND_GEN} # these cars use a different gas signal -EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.GENESIS_GV60_EV_1ST_GEN} +EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.GENESIS_GV60_EV_1ST_GEN} # 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_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, @@ -1733,4 +1845,6 @@ DBC = { CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None), + CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None), + CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None), } diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 249818369..e5d743087 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -8,10 +8,10 @@ from cereal import car from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from common.realtime import DT_CTRL from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_center_deadzone +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -64,6 +64,7 @@ class CarInterfaceBase(ABC): self.frame = 0 self.steering_unpressed = 0 self.low_speed_alert = False + self.no_steer_warning = False self.silent_steer_warning = True self.v_ego_cluster_seen = False @@ -92,12 +93,12 @@ class CarInterfaceBase(ABC): """ Parameters essential to controlling the car may be incomplete or wrong without FW versions or fingerprints. """ - return cls.get_params(candidate, gen_empty_fingerprint(), list(), False) + return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False) @classmethod - def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool): + def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool): ret = CarInterfaceBase.get_std_params(candidate) - ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long) + ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs) # Set common params using fields set by the car interface # TODO: get actual value, for now starting with reasonable value for @@ -114,7 +115,7 @@ class CarInterfaceBase(ABC): @staticmethod @abstractmethod - def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool): + def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool): raise NotImplementedError @staticmethod @@ -131,15 +132,11 @@ class CarInterfaceBase(ABC): return self.get_steer_feedforward_default @staticmethod - def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, friction_compensation): + def torque_from_lateral_accel_linear(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, + lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float: # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) - friction_interp = interp( - apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), - [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], - [-torque_params.friction, torque_params.friction] - ) - friction = friction_interp if friction_compensation else 0.0 - return (lateral_accel_value / torque_params.latAccelFactor) + friction + friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) + return (lateral_accel_value / float(torque_params.latAccelFactor)) + friction def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: return self.torque_from_lateral_accel_linear @@ -282,13 +279,19 @@ class CarInterfaceBase(ABC): # Handle permanent and temporary steering faults self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1 if cs_out.steerFaultTemporary: - # if the user overrode recently, show a less harsh alert - if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL): - self.silent_steer_warning = True - events.add(EventName.steerTempUnavailableSilent) + if cs_out.steeringPressed and (not self.CS.out.steerFaultTemporary or self.no_steer_warning): + self.no_steer_warning = True else: - events.add(EventName.steerTempUnavailable) + self.no_steer_warning = False + + # if the user overrode recently, show a less harsh alert + if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL): + self.silent_steer_warning = True + events.add(EventName.steerTempUnavailableSilent) + else: + events.add(EventName.steerTempUnavailable) else: + self.no_steer_warning = False self.silent_steer_warning = False if cs_out.steerFaultPermanent: events.add(EventName.steerUnavailable) diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index d9c658a14..965d2e183 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -60,7 +60,7 @@ class IsoTpParallelQuery: return msgs def _drain_rx(self): - messaging.drain_sock(self.logcan) + messaging.drain_sock_raw(self.logcan) self.msg_buffer = defaultdict(list) def _create_isotp_msg(self, tx_addr, sub_addr, rx_addr): @@ -100,18 +100,16 @@ class IsoTpParallelQuery: while True: self.rx() - if all(request_done.values()): - break - for tx_addr, msg in msgs.items(): try: - dat, updated = msg.recv() + dat, rx_in_progress = msg.recv() except Exception: cloudlog.exception(f"Error processing UDS response: {tx_addr}") request_done[tx_addr] = True continue - if updated: + # Extend timeout for each consecutive ISO-TP frame to avoid timing out on long responses + if rx_in_progress: response_timeouts[tx_addr] = time.monotonic() + timeout if not dat: @@ -123,6 +121,7 @@ class IsoTpParallelQuery: if response_valid: if counter + 1 < len(self.request): + response_timeouts[tx_addr] = time.monotonic() + timeout msg.send(self.request[counter + 1]) request_counter[tx_addr] += 1 else: @@ -132,18 +131,21 @@ class IsoTpParallelQuery: error_code = dat[2] if len(dat) > 2 else -1 if error_code == 0x78: response_timeouts[tx_addr] = time.monotonic() + self.response_pending_timeout - if self.debug: - cloudlog.warning(f"iso-tp query response pending: {tx_addr}") + cloudlog.error(f"iso-tp query response pending: {tx_addr}") else: - response_timeouts[tx_addr] = 0 request_done[tx_addr] = True cloudlog.error(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") + # Mark request done if address timed out cur_time = time.monotonic() - if cur_time - max(response_timeouts.values()) > 0: - for tx_addr in msgs: + for tx_addr in response_timeouts: + if cur_time - response_timeouts[tx_addr] > 0: if request_counter[tx_addr] > 0 and not request_done[tx_addr]: cloudlog.error(f"iso-tp query timeout after receiving response: {tx_addr}") + request_done[tx_addr] = True + + # Break if all requests are done (finished or timed out) + if all(request_done.values()): break if cur_time - start_time > total_timeout: diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 524a02a37..cb401a8ab 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.mazda import mazdacan from selfdrive.car.mazda.values import CarControllerParams, Buttons @@ -23,8 +23,8 @@ class CarController: if CC.latActive: # calculate steer and also set limits due to driver torque new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, - CS.out.steeringTorque, CarControllerParams) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, + CS.out.steeringTorque, CarControllerParams) if CC.cruiseControl.cancel: # If brake is pressed, let us wait >70ms before trying to disable crz to avoid diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 2930b002d..443116bc1 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -11,7 +11,7 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 598b598a1..79f07a1fd 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -1,10 +1,9 @@ from dataclasses import dataclass -from enum import Enum from typing import Dict, List, Union from cereal import car from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -38,7 +37,7 @@ class CAR: @dataclass class MazdaCarInfo(CarInfo): package: str = "All" - harness: Enum = Harness.mazda + harness_kit: HarnessKit = HarnessKit(Harness.mazda) CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { @@ -258,11 +257,14 @@ FW_VERSIONS = { CAR.MAZDA6: { (Ecu.eps, 0x730, None): [ b'GBEF-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GBEF-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GFBC-3210X-A-00\000\000\000\000\000\000\000\000\000', ], (Ecu.engine, 0x7e0, None): [ + b'PA34-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX4F-188K2-D\000\000\000\000\000\000\000\000\000\000\000\000', b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYH7-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -270,13 +272,16 @@ FW_VERSIONS = { ], (Ecu.abs, 0x760, None): [ b'GBVH-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GBVH-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GDDM-437K2-A\000\000\000\000\000\000\000\000\000\000\000\000', ], (Ecu.fwdCamera, 0x706, None): [ b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-P\000\000\000\000\000\000\000\000\000\000\000\000', ], (Ecu.transmission, 0x7e1, None): [ + b'PA28-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYH3-21PS1-D\000\000\000\000\000\000\000\000\000\000\000\000', b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -291,6 +296,7 @@ FW_VERSIONS = { b'PXM4-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM4-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM6-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXGW-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 13210c86d..1c74aef1f 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -19,7 +19,7 @@ class CarInterface(CarInterfaceBase): self.prev_speed = 0. @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "mock" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)] ret.mass = 1700. diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 45c3dd720..5064e8c45 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -26,13 +26,11 @@ class CarController: can_sends = [] ### STEER ### - lkas_hud_msg = CS.lkas_hud_msg - lkas_hud_info_msg = CS.lkas_hud_info_msg steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 if CC.latActive: # windup slower - apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CarControllerParams) # Max torque from driver before EPS will give up and not apply torque if not bool(CS.out.steeringPressed): @@ -63,16 +61,16 @@ class CarController: can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) can_sends.append(nissancan.create_steering_control( - self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque)) + self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque)) - if lkas_hud_msg and lkas_hud_info_msg: + if self.CP.carFingerprint != CAR.ALTIMA: if self.frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg( - self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) if self.frame % 50 == 0: can_sends.append(nissancan.create_lkas_hud_info_msg( - self.packer, lkas_hud_info_msg, steer_hud_alert + self.packer, CS.lkas_hud_info_msg, steer_hud_alert )) new_actuators = actuators.copy() diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index d6b6d17d5..bbba92dde 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -14,8 +14,8 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - self.lkas_hud_msg = None - self.lkas_hud_info_msg = None + self.lkas_hud_msg = {} + self.lkas_hud_info_msg = {} self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 074cd1cc5..573dff9f0 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -8,7 +8,7 @@ from selfdrive.car.nissan.values import CAR class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.autoResumeSng = False diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index e9af828e2..cbe62645d 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -1,11 +1,10 @@ from dataclasses import dataclass from typing import Dict, List, Optional, Union -from enum import Enum from cereal import car from panda.python import uds from selfdrive.car import AngleRateLimit, dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit, HarnessPart from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -31,18 +30,21 @@ class CAR: ALTIMA = "NISSAN ALTIMA 2020" +NISSAN_HARNESS_PARTS = [HarnessPart.harness_box, HarnessPart.rj45_cable, HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler] + + @dataclass class NissanCarInfo(CarInfo): package: str = "ProPILOT Assist" - harness: Enum = Harness.nissan_a + harness_kit: HarnessKit = HarnessKit(Harness.nissan_a, parts=NISSAN_HARNESS_PARTS) CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), - CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22", video_link="https://youtu.be/vaMbtAh_0cY"), + CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY"), CAR.LEAF_IC: None, # same platforms CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), - CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b), + CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness_kit=HarnessKit(Harness.nissan_b, parts=NISSAN_HARNESS_PARTS)), } FINGERPRINTS = { @@ -126,18 +128,22 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x707, None): [ b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80', + b'5SH4BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', ], (Ecu.abs, 0x740, None): [ b'476605SH1D', b'476605SK2A', + b'476605SD2E', ], (Ecu.eps, 0x742, None): [ b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80', b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80', + b'5SH2C\xb7A\x05\x02N123F\x15\xa3\x00\x00\x00\x00\x00\x00\x00\x80', ], (Ecu.gateway, 0x18dad0f1, None): [ b'284U25SH3A', b'284U25SK2D', + b'284U25SF0C', ], }, CAR.XTRAIL: { diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 24d85877d..c4246b380 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,7 +1,7 @@ from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.subaru import subarucan -from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams +from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams, SubaruFlags class CarController: @@ -10,9 +10,10 @@ class CarController: self.apply_steer_last = 0 self.frame = 0 - self.es_lkas_cnt = -1 + self.es_lkas_state_cnt = -1 self.es_distance_cnt = -1 self.es_dashstatus_cnt = -1 + self.infotainmentstatus_cnt = -1 self.cruise_button_prev = 0 self.last_cancel_frame = 0 @@ -34,7 +35,7 @@ class CarController: # limits due to driver torque new_steer = int(round(apply_steer)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) if not CC.latActive: apply_steer = 0 @@ -79,11 +80,15 @@ class CarController: can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg)) self.es_dashstatus_cnt = CS.es_dashstatus_msg["COUNTER"] - if self.es_lkas_cnt != CS.es_lkas_msg["COUNTER"]: - can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.enabled, hud_control.visualAlert, - hud_control.leftLaneVisible, hud_control.rightLaneVisible, - hud_control.leftLaneDepart, hud_control.rightLaneDepart)) - self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"] + if self.es_lkas_state_cnt != CS.es_lkas_state_msg["COUNTER"]: + can_sends.append(subarucan.create_es_lkas_state(self.packer, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert, + hud_control.leftLaneVisible, hud_control.rightLaneVisible, + hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + self.es_lkas_state_cnt = CS.es_lkas_state_msg["COUNTER"] + + if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT and self.infotainmentstatus_cnt != CS.es_infotainmentstatus_msg["COUNTER"]: + can_sends.append(subarucan.create_infotainmentstatus(self.packer, CS.es_infotainmentstatus_msg, hud_control.visualAlert)) + self.infotainmentstatus_cnt = CS.es_infotainmentstatus_msg["COUNTER"] new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ba873c48d..9d7b0a65c 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS +from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags class CarState(CarStateBase): @@ -77,11 +77,13 @@ class CarState(CarStateBase): ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1 ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2 - self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) + self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) + if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: + self.es_infotainmentstatus_msg = copy.copy(cp_cam.vl["INFOTAINMENT_STATUS"]) return ret @@ -248,7 +250,7 @@ class CarState(CarStateBase): ] else: signals = [ - ("Counter", "ES_DashStatus"), + ("COUNTER", "ES_DashStatus"), ("PCB_Off", "ES_DashStatus"), ("LDW_Off", "ES_DashStatus"), ("Signal1", "ES_DashStatus"), @@ -256,7 +258,7 @@ class CarState(CarStateBase): ("LKAS_State_Msg", "ES_DashStatus"), ("Signal2", "ES_DashStatus"), ("Cruise_Soft_Disable", "ES_DashStatus"), - ("EyeSight_Status_Msg", "ES_DashStatus"), + ("Cruise_Status_Msg", "ES_DashStatus"), ("Signal3", "ES_DashStatus"), ("Cruise_Distance", "ES_DashStatus"), ("Signal4", "ES_DashStatus"), @@ -301,6 +303,15 @@ class CarState(CarStateBase): signals += CarState.get_global_es_distance_signals()[0] checks += CarState.get_global_es_distance_signals()[1] + if CP.flags & SubaruFlags.SEND_INFOTAINMENT: + signals += [ + ("LKAS_State_Infotainment", "INFOTAINMENT_STATUS"), + ("LKAS_Blue_Lines", "INFOTAINMENT_STATUS"), + ("Signal1", "INFOTAINMENT_STATUS"), + ("Signal2", "INFOTAINMENT_STATUS"), + ] + checks.append(("INFOTAINMENT_STATUS", 10)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) @staticmethod diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 733482ef8..f7698dbe7 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -3,21 +3,25 @@ from cereal import car from panda import Panda from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase -from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS +from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "subaru" ret.radarUnavailable = True ret.dashcamOnly = candidate in PREGLOBAL_CARS ret.autoResumeSng = False + # Detect infotainment message sent from the camera + if candidate not in PREGLOBAL_CARS and 0x323 in fingerprint[2]: + ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value + if candidate in PREGLOBAL_CARS: ret.enableBsm = 0x25c in fingerprint[0] - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruLegacy)] + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruPreglobal)] else: ret.enableBsm = 0x228 in fingerprint[0] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index d83b639a4..3b498d3f7 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -3,6 +3,7 @@ from cereal import car VisualAlert = car.CarControl.HUDControl.VisualAlert + def create_steering_control(packer, apply_steer): values = { "LKAS_Output": apply_steer, @@ -11,9 +12,11 @@ def create_steering_control(packer, apply_steer): } return packer.make_can_msg("ES_LKAS", 0, values) + def create_steering_status(packer): return packer.make_can_msg("ES_LKAS_State", 0, {}) + def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): values = copy.copy(es_distance_msg) values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 @@ -21,9 +24,9 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): values["Cruise_Cancel"] = 1 return packer.make_can_msg("ES_Distance", bus, values) -def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): - values = copy.copy(es_lkas_msg) +def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): + values = copy.copy(es_lkas_state_msg) # Filter the stock LKAS "Keep hands on wheel" alert if values["LKAS_Alert_Msg"] == 1: @@ -52,36 +55,52 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_ # Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW) if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0: if left_lane_depart: - values["LKAS_Alert"] = 12 # Left lane departure dash alert + values["LKAS_Alert"] = 12 # Left lane departure dash alert elif right_lane_depart: - values["LKAS_Alert"] = 11 # Right lane departure dash alert + values["LKAS_Alert"] = 11 # Right lane departure dash alert - if enabled: - values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines - values["LKAS_Dash_State"] = 2 # Green enabled indicator - else: - values["LKAS_Dash_State"] = 0 # LKAS Not enabled + values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines + values["LKAS_Dash_State"] = 2 if enabled else 0 # Green enabled indicator values["LKAS_Left_Line_Visible"] = int(left_line) values["LKAS_Right_Line_Visible"] = int(right_line) return packer.make_can_msg("ES_LKAS_State", 0, values) + def create_es_dashstatus(packer, dashstatus_msg): values = copy.copy(dashstatus_msg) # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts - if values["LKAS_State_Msg"] in [2, 3]: + if values["LKAS_State_Msg"] in (2, 3): values["LKAS_State_Msg"] = 0 return packer.make_can_msg("ES_DashStatus", 0, values) + +def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): + # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts + if infotainmentstatus_msg["LKAS_State_Infotainment"] in (3, 4): + infotainmentstatus_msg["LKAS_State_Infotainment"] = 0 + + # Show Keep hands on wheel alert for openpilot steerRequired alert + if visual_alert == VisualAlert.steerRequired: + infotainmentstatus_msg["LKAS_State_Infotainment"] = 3 + + # Show Obstacle Detected for fcw + if visual_alert == VisualAlert.fcw: + infotainmentstatus_msg["LKAS_State_Infotainment"] = 2 + + return packer.make_can_msg("INFOTAINMENT_STATUS", 0, infotainmentstatus_msg) + + # *** Subaru Pre-global *** def subaru_preglobal_checksum(packer, values, addr): dat = packer.make_can_msg(addr, 0, values)[2] return (sum(dat[:7])) % 256 + def create_preglobal_steering_control(packer, apply_steer): values = { "LKAS_Command": apply_steer, @@ -91,8 +110,8 @@ def create_preglobal_steering_control(packer, apply_steer): return packer.make_can_msg("ES_LKAS", 0, values) -def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): +def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): values = copy.copy(es_distance_msg) values["Cruise_Button"] = cruise_button diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 5c7d3f5e4..f90f015bf 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -1,11 +1,11 @@ from dataclasses import dataclass -from enum import Enum +from enum import IntFlag from typing import Dict, List, Union from cereal import car from panda.python import uds from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -30,6 +30,10 @@ class CarControllerParams: self.STEER_MAX = 2047 +class SubaruFlags(IntFlag): + SEND_INFOTAINMENT = 1 + + class CAR: # Global platform ASCENT = "SUBARU ASCENT LIMITED 2019" @@ -49,13 +53,13 @@ class CAR: @dataclass class SubaruCarInfo(CarInfo): package: str = "EyeSight Driver Assistance" - harness: Enum = Harness.subaru_a + harness_kit: HarnessKit = HarnessKit(Harness.subaru_a) CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"), - CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", harness=Harness.subaru_b), - CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", harness=Harness.subaru_b), + CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", harness_kit=HarnessKit(Harness.subaru_b)), + CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", harness_kit=HarnessKit(Harness.subaru_b)), CAR.IMPREZA: [ SubaruCarInfo("Subaru Impreza 2017-19"), SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), @@ -185,6 +189,7 @@ FW_VERSIONS = { b'\x00\x00d\xdc\x00\x00\x00\x00', b'\x00\x00dd\x00\x00\x00\x00', b'\x00\x00c\xf4\x1f@ \x07', + b'\x00\x00e\x1c\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\xaa\x61\x66\x73\x07', @@ -205,6 +210,8 @@ FW_VERSIONS = { b'\xc5!dr\x07', b'\xaa!aw\x07', b'\xaa!av\x07', + b'\xaa\x01bt\x07', + b'\xc5!ap\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xe3\xe5\x46\x31\x00', @@ -220,6 +227,7 @@ FW_VERSIONS = { b'\xe4\xf5\002\000\000', b'\xe3\xd0\x081\x00', b'\xe3\xf5\x06\x00\x00', + b'\xe3\xd5\x161\x00', ], }, CAR.IMPREZA_2020: { @@ -239,7 +247,7 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x787, None): [ b'\000\000eb\037@ \"', - b'\000\000e\x8f\037@ )', + b'\x00\x00e\x8f\x1f@ )', b'\x00\x00eq\x1f@ "', b'\x00\x00eq\x00\x00\x00\x00', b'\x00\x00e\x8f\x00\x00\x00\x00', @@ -254,6 +262,7 @@ FW_VERSIONS = { b'\xca!fp\x07', b'\xf3"f@\x07', b'\xe6!fp\x07', + b'\xf3"fp\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xe6\xf5\004\000\000', @@ -273,9 +282,12 @@ FW_VERSIONS = { b'\xa3 \031\024\000', b'\xa3 \x14\x01', b'\xf1\x00\xbb\r\x05', + b'\xa3 \x18&\x00', + b'\xa3 \x19&\x00', ], (Ecu.eps, 0x746, None): [ b'\x8d\xc0\x04\x00', + b'\x8d\xc0\x00\x00', ], (Ecu.fwdCamera, 0x787, None): [ b'\x00\x00e!\x1f@ \x11', @@ -284,6 +296,8 @@ FW_VERSIONS = { b'\xf1\x00\xac\x02\x00', b'\x00\x00e!\x00\x00\x00\x00', b'\x00\x00e\x97\x00\x00\x00\x00', + b'\x00\x00e^\x1f@ !', + b'\x00\x00e^\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\xb6"`A\x07', @@ -292,6 +306,7 @@ FW_VERSIONS = { b'\xcb\"`p\a', b'\xf1\x00\xa2\x10\n', b'\xcf"`p\x07', + b'\xb6\xa2`A\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\032\xf6B0\000', @@ -299,6 +314,8 @@ FW_VERSIONS = { b'\032\xf6b`\000', b'\x1a\xf6B`\x00', b'\x1a\xf6b0\x00', + b'\x1a\xe6B1\x00', + b'\x1a\xe6F1\x00', ], }, CAR.FORESTER_PREGLOBAL: { @@ -466,6 +483,7 @@ FW_VERSIONS = { b'\xa1 "\t\x01', b'\xa1 \x08\x02', b'\xa1 \x06\x02', + b'\xa1 \x07\x02', b'\xa1 \x08\x00', b'\xa1 "\t\x00', ], @@ -477,6 +495,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x787, None): [ b'\x00\x00eJ\x00\x1f@ \x19\x00', b'\000\000e\x80\000\037@ \031\000', + b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00', b'\x00\x00e\x9a\x00\x1f@ 1\x00', b'\x00\x00eJ\x00\x00\x00\x00\x00\x00', ], @@ -486,6 +505,7 @@ FW_VERSIONS = { b'\xde"`0\a', b'\xf1\x82\xbc,\xa0q\a', b'\xf1\x82\xe3,\xa0@\x07', + b'\xe2"`0\x07', b'\xe2"`p\x07', b'\xf1\x82\xe2,\xa0@\x07', b'\xbc"`q\x07', @@ -497,6 +517,7 @@ FW_VERSIONS = { b'\xa5\xfe\xf6@\x00', b'\xa7\x8e\xf40\x00', b'\xf1\x82\xa7\xf6D@\x00', + b'\xa7\xf6D@\x00', b'\xa7\xfe\xf4@\x00', ], }, diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 70d49896c..afd3fb3be 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "tesla" # There is no safe way to do steer blending with user torque, diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index ac8213e4c..7198218d6 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -6,9 +6,9 @@ from parameterized import parameterized from cereal import car from selfdrive.car import gen_empty_fingerprint -from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.car_helpers import interfaces -from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS +from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS, all_known_cars + class TestCarInterfaces(unittest.TestCase): @@ -25,7 +25,7 @@ class TestCarInterfaces(unittest.TestCase): car_fw = [] - car_params = CarInterface.get_params(car_name, fingerprints, car_fw, experimental_long=False) + car_params = CarInterface.get_params(car_name, fingerprints, car_fw, experimental_long=False, docs=False) car_interface = CarInterface(car_params, CarController, CarState) assert car_params assert car_interface @@ -51,7 +51,7 @@ class TestCarInterfaces(unittest.TestCase): elif tune.which() == 'torque': self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0) - self.assertTrue(not math.isnan(tune.torque.friction)) + self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0) elif tune.which() == 'indi': self.assertTrue(len(tune.indi.outerLoopGainV)) diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index cc1681bce..afd4624a8 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -7,6 +7,10 @@ NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan] NISSAN LEAF 2018: [.nan, 1.5, .nan] NISSAN ROGUE 2019: [.nan, 1.5, .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] TESLA AP2 MODEL S: [.nan, 2.5, .nan] @@ -23,12 +27,13 @@ FORD MAVERICK 1ST GEN: [.nan, 1.5, .nan] COMMA BODY: [.nan, 1000, .nan] # Totally new cars -RAM 1500 5TH GEN: [2.0, 2.0, 0.0] -RAM HD 5TH GEN: [1.4, 1.4, 0.0] +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.3, 2.3, 0.11] CADILLAC ESCALADE 2017: [1.899999976158142, 1.842270016670227, 0.1120000034570694] 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] CHEVROLET EQUINOX 2019: [2.0, 2.0, 0.05] VOLKSWAGEN PASSAT NMS: [2.5, 2.5, 0.1] VOLKSWAGEN SHARAN 2ND GEN: [2.5, 2.5, 0.1] @@ -40,9 +45,12 @@ KIA SORENTO PLUG-IN HYBRID 4TH GEN: [2.5, 2.5, 0.1] GENESIS GV60 ELECTRIC 1ST GEN: [2.5, 2.5, 0.1] KIA SORENTO 4TH GEN: [2.5, 2.5, 0.1] KIA NIRO HYBRID 2ND GEN: [2.42, 2.5, 0.12] +KIA NIRO EV 2ND GEN: [2.05, 2.5, 0.14] +GENESIS GV80 2023: [2.5, 2.5, 0.1] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] # Manually checked HONDA CIVIC 2022: [2.5, 1.2, 0.15] +HONDA HR-V 2023: [2.5, 1.2, 0.2] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index 6f8cfe0ce..cb423e1d8 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -30,7 +30,7 @@ HYUNDAI IONIQ 5 2022: [3.172929, 2.713050, 0.096019] HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276] HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] HYUNDAI IONIQ PLUG-IN HYBRID 2019: [2.970807902012267, 1.6312321830002083, 0.1088964990357482] -HYUNDAI KONA ELECTRIC 2019: [3.078814714619148, 3.2961956260770484, 0.12359762054065548] +HYUNDAI KONA ELECTRIC 2019: [3.078814714619148, 2.307336938253934, 0.12359762054065548] HYUNDAI PALISADE 2020: [2.544642494803999, 1.8721703683337008, 0.1301424599248651] HYUNDAI SANTA FE 2019: [3.0787027729757632, 2.6173437483495565, 0.1207019341823945] HYUNDAI SANTA FE HYBRID 2022: [3.501877602644835, 2.729064118456137, 0.10384068104538963] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index 61243424f..5ea84e559 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -12,6 +12,7 @@ TOYOTA C-HR HYBRID 2018: TOYOTA C-HR 2018 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 NX HYBRID 2020: LEXUS NX 2020 LEXUS RC 2020: LEXUS NX 2020 @@ -50,12 +51,14 @@ HONDA CIVIC SEDAN 1.6 DIESEL 2019: HONDA CIVIC (BOSCH) 2019 HONDA E 2020: HONDA CIVIC (BOSCH) 2019 HONDA ODYSSEY CHN 2019: HONDA ODYSSEY 2018 +BUICK LACROSSE 2017: CHEVROLET VOLT PREMIER 2017 BUICK REGAL ESSENCE 2018: CHEVROLET VOLT PREMIER 2017 CADILLAC ESCALADE ESV 2016: CHEVROLET VOLT PREMIER 2017 CADILLAC ATS Premium Performance 2018: CHEVROLET VOLT PREMIER 2017 CHEVROLET MALIBU PREMIER 2017: CHEVROLET VOLT PREMIER 2017 HOLDEN ASTRA RS-V BK 2017: CHEVROLET VOLT PREMIER 2017 +SKODA FABIA 4TH GEN: VOLKSWAGEN GOLF 7TH GEN SKODA OCTAVIA 3RD GEN: SKODA SUPERB 3RD GEN SKODA SCALA 1ST GEN: SKODA SUPERB 3RD GEN SKODA KODIAQ 1ST GEN: SKODA SUPERB 3RD GEN diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 2e0d7009c..66a7b57f0 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car from common.numpy_fast import clip, interp -from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg +from selfdrive.car import apply_meas_steer_torque_limits, create_gas_interceptor_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ create_fcw_command, create_lta_steer_command @@ -60,7 +60,7 @@ class CarController: # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) + apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: @@ -76,6 +76,11 @@ class CarController: apply_steer_req = 0 self.steer_rate_counter = 0 + # Never actuate with LKA on cars that only support LTA + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + apply_steer = 0 + apply_steer_req = 0 + # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different if not CC.enabled and CS.pcm_acc_status: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 050f8747a..68adc2ee5 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -115,7 +115,8 @@ class CarState(CarStateBase): cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp if self.CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR): - self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] + if not (self.CP.flags & ToyotaFlags.SMART_DSU.value): + self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] ret.stockFcw = bool(cp_acc.vl["ACC_HUD"]["FCW"]) # some TSS2 cars have low speed lockout permanently set, so ignore on those cars @@ -235,12 +236,17 @@ class CarState(CarStateBase): checks.append(("BSM", 1)) if CP.carFingerprint in RADAR_ACC_CAR: + if not CP.flags & ToyotaFlags.SMART_DSU.value: + signals += [ + ("ACC_TYPE", "ACC_CONTROL"), + ] + checks += [ + ("ACC_CONTROL", 33), + ] signals += [ - ("ACC_TYPE", "ACC_CONTROL"), ("FCW", "ACC_HUD"), ] checks += [ - ("ACC_CONTROL", 33), ("ACC_HUD", 1), ] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b3fd048d..c222363d1 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,7 +2,8 @@ from cereal import car from common.conversions import Conversions as CV from panda import Panda -from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR +from selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ + MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -15,21 +16,27 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] - if candidate in (CAR.RAV4, CAR.PRIUS_V, CAR.COROLLA, CAR.LEXUS_ESH, CAR.LEXUS_CTH): + # BRAKE_MODULE is on a different address for these cars + if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated": ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE + if candidate in ANGLE_CONTROL_CAR: + ret.dashcamOnly = True + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA + else: + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop stop_and_go = False - steering_angle_deadzone_deg = 0.0 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) if candidate == CAR.PRIUS: stop_and_go = True @@ -40,9 +47,8 @@ class CarInterface(CarInterfaceBase): # Only give steer angle deadzone to for bad angle sensor prius for fw in car_fw: if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': - steering_angle_deadzone_deg = 0.2 ret.steerActuatorDelay = 0.25 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2) elif candidate == CAR.PRIUS_V: stop_and_go = True @@ -102,7 +108,8 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022): + elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, + CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023): stop_and_go = True ret.wheelbase = 2.68986 ret.steerRatio = 14.3 @@ -131,8 +138,9 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): - stop_and_go = True + elif candidate in (CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2): + if candidate not in (CAR.LEXUS_ES,): # TODO: LEXUS_ES may have sng + stop_and_go = True ret.wheelbase = 2.8702 ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet @@ -194,14 +202,18 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor=tire_stiffness_factor) ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR - # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it - smartDsu = 0x2FF in fingerprint[0] - # In TSS2 cars the camera does long control + + # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it + if 0x2FF in fingerprint[0]: + ret.flags |= ToyotaFlags.SMART_DSU.value + + # In TSS2 cars, the camera does long control found_ecus = [fw.ecu for fw in car_fw] - ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) and not smartDsu + ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) and not (ret.flags & ToyotaFlags.SMART_DSU) ret.enableGasInterceptor = 0x201 in fingerprint[0] + # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") - ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) + ret.openpilotLongitudinalControl = bool(ret.flags & ToyotaFlags.SMART_DSU) or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR if not ret.openpilotLongitudinalControl: diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 7e360cc4e..103136135 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -17,7 +17,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt): "SETME_X1": 1, "SETME_X3": 3, "PERCENTAGE": 100, - "SETME_X64": 0x64, + "SETME_X64": 0, "ANGLE": 0, "STEER_ANGLE_CMD": steer, "STEER_REQUEST": steer_req, @@ -99,6 +99,13 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep # lane sway functionality # not all cars have LKAS_HUD — update with camera values if available - values.update(stock_lkas_hud) + if len(stock_lkas_hud): + values.update({s: stock_lkas_hud[s] for s in [ + "LANE_SWAY_FLD", + "LANE_SWAY_BUZZER", + "LANE_SWAY_WARNING", + "LANE_SWAY_SENSITIVITY", + "LANE_SWAY_TOGGLE", + ]}) return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index b6a556438..00552485a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -6,7 +6,7 @@ from typing import Dict, List, Union from cereal import car from common.conversions import Conversions as CV from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -33,6 +33,7 @@ class CarControllerParams: class ToyotaFlags(IntFlag): HYBRID = 1 + SMART_DSU = 2 class CAR: @@ -67,13 +68,16 @@ class CAR: RAV4H = "TOYOTA RAV4 HYBRID 2017" RAV4_TSS2 = "TOYOTA RAV4 2019" RAV4_TSS2_2022 = "TOYOTA RAV4 2022" + RAV4_TSS2_2023 = "TOYOTA RAV4 2023" RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019" 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" # Lexus LEXUS_CTH = "LEXUS CT HYBRID 2018" + LEXUS_ES = "LEXUS ES 2018" LEXUS_ESH = "LEXUS ES HYBRID 2018" LEXUS_ES_TSS2 = "LEXUS ES 2019" LEXUS_ESH_TSS2 = "LEXUS ES HYBRID 2019" @@ -98,7 +102,7 @@ class Footnote(Enum): @dataclass class ToyotaCarInfo(CarInfo): package: str = "All" - harness: Enum = Harness.toyota + harness_kit: HarnessKit = HarnessKit(Harness.toyota) CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { @@ -115,12 +119,12 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { 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-22", footnotes=[Footnote.CAMRY]), + CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-23", footnotes=[Footnote.CAMRY]), CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-23"), CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-20"), CAR.CHR_TSS2: ToyotaCarInfo("Toyota C-HR 2021"), - CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-19"), - CAR.CHRH_TSS2: ToyotaCarInfo("Toyota C-HR Hybrid 2022"), + CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"), + CAR.CHRH_TSS2: 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"), @@ -138,7 +142,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { 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", "https://www.youtube.com/watch?v=8zopPJI8XQ0"), + 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"), ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), ], @@ -152,19 +156,22 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { ToyotaCarInfo("Toyota RAV4 2017-18") ], CAR.RAV4H: [ - ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", "https://youtu.be/LhT5VzJVfNI?t=26"), + ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26"), ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26") ], CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"), + CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"), CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), 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.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), # Lexus CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"), - CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "Lexus Safety System+"), + CAR.LEXUS_ES: ToyotaCarInfo("Lexus ES 2017-18"), + CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18"), CAR.LEXUS_ES_TSS2: ToyotaCarInfo("Lexus ES 2019-22"), CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-23", video_link="https://youtu.be/BZ29osRVJeg?t=12"), CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"), @@ -172,7 +179,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19"), CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"), CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"), - CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"), + CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2018-20"), CAR.LEXUS_RX: [ ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"), ToyotaCarInfo("Lexus RX 2017-19"), @@ -188,23 +195,24 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { # (addr, cars, bus, 1/freq*100, vl) STATIC_DSU_MSGS = [ (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), - (0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), - (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), - (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), + (0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), + (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), + (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V, CAR.LEXUS_ES), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), (0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), (0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), (0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), (0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), (0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), - (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), + (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), + (0x366, (CAR.LEXUS_ES,), 0, 20, b'\x00\x95\x07\xfe\x08\x05\x00'), (0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), - (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), - (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), + (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), ] TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01' @@ -323,6 +331,7 @@ FW_VERSIONS = { CAR.AVALON_TSS2: { (Ecu.abs, 0x7b0, None): [ b'\x01F152607240\x00\x00\x00\x00\x00\x00', + b'\x01F152607250\x00\x00\x00\x00\x00\x00', b'\x01F152607280\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -330,6 +339,7 @@ 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', @@ -527,14 +537,19 @@ FW_VERSIONS = { CAR.CAMRY_TSS2: { (Ecu.eps, 0x7a1, None): [ b'8965B33630\x00\x00\x00\x00\x00\x00', + b'8965B33640\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', ], (Ecu.engine, 0x700, None): [ b'\x018966306Q5000\x00\x00\x00\x00', + b'\x018966306Q9000\x00\x00\x00\x00', + b'\x018966306R3000\x00\x00\x00\x00', + b'\x018966306R8000\x00\x00\x00\x00', b'\x018966306T3100\x00\x00\x00\x00', b'\x018966306T3200\x00\x00\x00\x00', b'\x018966306T4000\x00\x00\x00\x00', @@ -542,12 +557,16 @@ FW_VERSIONS = { ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F0602300\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + 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: { @@ -712,25 +731,32 @@ FW_VERSIONS = { b'8646FF404000 ', b'8646FF406000 ', b'8646FF407000 ', + b'8646FF407100 ', ], }, CAR.CHRH_TSS2: { - (Ecu.eps, 0x7a1, None): [ - b'8965B10092\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', - ], - (Ecu.fwdRadar, 0x750, 15): [ - b'\x018821FF410500\x00\x00\x00\x00', - ], - (Ecu.fwdCamera, 0x750, 109): [ - b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00', - ], - }, + (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: { (Ecu.engine, 0x7e0, None): [ b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -898,6 +924,7 @@ FW_VERSIONS = { b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966312Q3100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00', b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', @@ -908,6 +935,7 @@ FW_VERSIONS = { b'8965B12451\x00\x00\x00\x00\x00\x00', b'8965B16011\x00\x00\x00\x00\x00\x00', b'8965B16101\x00\x00\x00\x00\x00\x00', + b'8965B16170\x00\x00\x00\x00\x00\x00', b'8965B76012\x00\x00\x00\x00\x00\x00', b'8965B76050\x00\x00\x00\x00\x00\x00', b'\x018965B12350\x00\x00\x00\x00\x00\x00', @@ -935,6 +963,7 @@ FW_VERSIONS = { b'F152612D00\x00\x00\x00\x00\x00\x00', b'F152616011\x00\x00\x00\x00\x00\x00', b'F152616060\x00\x00\x00\x00\x00\x00', + b'F152616030\x00\x00\x00\x00\x00\x00', b'F152642540\x00\x00\x00\x00\x00\x00', b'F152676293\x00\x00\x00\x00\x00\x00', b'F152676303\x00\x00\x00\x00\x00\x00', @@ -1047,6 +1076,7 @@ FW_VERSIONS = { b'\x01896630E62200\x00\x00\x00\x00', b'\x01896630E64100\x00\x00\x00\x00', b'\x01896630E64200\x00\x00\x00\x00', + b'\x01896630E64400\x00\x00\x00\x00', b'\x01896630EB1000\x00\x00\x00\x00', b'\x01896630EB1100\x00\x00\x00\x00', b'\x01896630EB1200\x00\x00\x00\x00', @@ -1089,6 +1119,7 @@ FW_VERSIONS = { 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', @@ -1453,6 +1484,23 @@ FW_VERSIONS = { 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'\x01896634AJ2000\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: { (Ecu.engine, 0x700, None): [ b'\x01896634A15000\x00\x00\x00\x00', @@ -1540,6 +1588,23 @@ FW_VERSIONS = { b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', ], }, + CAR.RAV4H_TSS2_2023: { + (Ecu.abs, 0x7b0, None): [ + b'\x01F15264283200\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896634AE1001\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.SIENNA: { (Ecu.engine, 0x700, None): [ b'\x01896630832100\x00\x00\x00\x00', @@ -1636,6 +1701,7 @@ FW_VERSIONS = { b'\x028966333S8000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966333W1000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', b'\x01896633T38000\x00\x00\x00\x00', b'\x01896633T58000\x00\x00\x00\x00', @@ -1671,6 +1737,26 @@ FW_VERSIONS = { b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, + CAR.LEXUS_ES: { + (Ecu.engine, 0x7e0, None): [ + b'\x02333R0000\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152606202\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881513309500\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33502\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4701200\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F3302200\x00\x00\x00\x00', + ], + }, CAR.LEXUS_ESH: { (Ecu.engine, 0x7e0, None): [ b'\x02333M4200\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1726,6 +1812,8 @@ FW_VERSIONS = { (Ecu.engine, 0x700, None): [ b'\x018966378B2100\x00\x00\x00\x00', b'\x018966378B3000\x00\x00\x00\x00', + b'\x018966378B4100\x00\x00\x00\x00', + b'\x018966378G2000\x00\x00\x00\x00', b'\x018966378G3000\x00\x00\x00\x00', b'\x018966378B2000\x00\x00\x00\x00', ], @@ -1748,6 +1836,7 @@ FW_VERSIONS = { CAR.LEXUS_NXH_TSS2: { (Ecu.engine, 0x7e0, None): [ b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02378A0000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152678210\x00\x00\x00\x00\x00\x00', @@ -1757,6 +1846,7 @@ FW_VERSIONS = { ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', @@ -1928,12 +2018,15 @@ FW_VERSIONS = { b'\x01896630ED0000\x00\x00\x00\x00', b'\x01896630ED0100\x00\x00\x00\x00', b'\x01896630ED6000\x00\x00\x00\x00', + b'\x018966348T8000\x00\x00\x00\x00', b'\x018966348W5100\x00\x00\x00\x00', b'\x018966348W9000\x00\x00\x00\x00', b'\x01896634D12000\x00\x00\x00\x00', b'\x01896634D12100\x00\x00\x00\x00', b'\x01896634D43000\x00\x00\x00\x00', b'\x01896634D44000\x00\x00\x00\x00', + b'\x018966348X0000\x00\x00\x00\x00', + b'\x01896630ED5000\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'\x01F15260E031\x00\x00\x00\x00\x00\x00', @@ -1959,11 +2052,12 @@ FW_VERSIONS = { }, CAR.LEXUS_RXH_TSS2: { (Ecu.engine, 0x7e0, None): [ + b'\x02348X4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348X5000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', - b'\x02348X4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152648831\x00\x00\x00\x00\x00\x00', @@ -2093,8 +2187,10 @@ DBC = { 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), CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_ES: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_ESH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), @@ -2103,6 +2199,7 @@ DBC = { CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_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_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), 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'), @@ -2117,8 +2214,8 @@ DBC = { EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) # Toyota/Lexus Safety Sense 2.0 and 2.5 -TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, - CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, +TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, + CAR.RAV4H_TSS2_2023, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} @@ -2127,10 +2224,13 @@ NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} 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.CHR_TSS2, CAR.CHRH_TSS2} +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} + +# these cars use the Lane Tracing Assist (LTA) message for lateral control +ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023} EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.CHRH_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS, - CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, + CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2} # no resume button press required diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5d00b5a52..c17b63245 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker from common.numpy_fast import clip from common.conversions import Conversions as CV from common.realtime import DT_CTRL -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.volkswagen import mqbcan, pqcan from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams @@ -44,7 +44,7 @@ class CarController: if CC.latActive: new_steer = int(round(actuators.steer * self.CCP.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP) if apply_steer == 0: hcaEnabled = False self.hcaEnabledFrameCount = 0 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 64d124688..f3cd2808a 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -228,13 +228,13 @@ class CarState(CarStateBase): ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"]) ret.cruiseState.enabled = pt_cp.vl["Motor_2"]["GRA_Status"] in (1, 2) if self.CP.pcmCruise: - ret.accFaulted = ext_cp.vl["ACC_GRA_Anziege"]["ACA_StaACC"] in (6, 7) + ret.accFaulted = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"] in (6, 7) else: ret.accFaulted = pt_cp.vl["Motor_2"]["GRA_Status"] == 3 # Update ACC setpoint. When the setpoint reads as 255, the driver has not # yet established an ACC setpoint, so treat it as zero. - ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anziege"]["ACA_V_Wunsch"] * CV.KPH_TO_MS + ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_V_Wunsch"] * CV.KPH_TO_MS if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint ret.cruiseState.speed = 0 @@ -516,12 +516,12 @@ class PqExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ ("ACS_Typ_ACC", "ACC_System"), # Basic vs FtS (no SnG support on PQ) - ("ACA_StaACC", "ACC_GRA_Anziege"), # ACC drivetrain coordinator status - ("ACA_V_Wunsch", "ACC_GRA_Anziege"), # ACC set speed + ("ACA_StaACC", "ACC_GRA_Anzeige"), # ACC drivetrain coordinator status + ("ACA_V_Wunsch", "ACC_GRA_Anzeige"), # ACC set speed ] fwd_radar_checks = [ ("ACC_System", 50), # From J428 ACC radar control module - ("ACC_GRA_Anziege", 25), # From J428 ACC radar control module + ("ACC_GRA_Anzeige", 25), # From J428 ACC radar control module ] bsm_radar_signals = [ ("SWA_Infostufe_SWA_li", "SWA_1"), # Blind spot object info, left diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 521c68184..874b85e68 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -21,18 +21,16 @@ class CarInterface(CarInterfaceBase): self.cp_ext = self.cp_cam @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "volkswagen" ret.radarUnavailable = True - use_off_car_defaults = len(fingerprint[0]) == 0 # Pick sensible carParams during offline doc generation/CI jobs - if candidate in PQ_CARS: # Set global PQ35/PQ46/NMS parameters ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagenPq)] ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1 - if 0x440 in fingerprint[0] or use_off_car_defaults: # Getriebe_1 + if 0x440 in fingerprint[0] or docs: # Getriebe_1 ret.transmissionType = TransmissionType.automatic else: ret.transmissionType = TransmissionType.manual @@ -55,7 +53,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)] ret.enableBsm = 0x30F in fingerprint[0] # SWA_01 - if 0xAD in fingerprint[0] or use_off_car_defaults: # Getriebe_11 + if 0xAD in fingerprint[0] or docs: # Getriebe_11 ret.transmissionType = TransmissionType.automatic elif 0x187 in fingerprint[0]: # EV_Gearshift ret.transmissionType = TransmissionType.direct @@ -80,7 +78,7 @@ class CarInterface(CarInterfaceBase): # Global longitudinal tuning defaults, can be overridden per-vehicle - ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or use_off_car_defaults + ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or docs if experimental_long: # Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required. ret.openpilotLongitudinalControl = True @@ -187,6 +185,10 @@ class CarInterface(CarInterfaceBase): ret.mass = 1227 + STD_CARGO_KG ret.wheelbase = 2.64 + elif candidate == CAR.SKODA_FABIA_MK4: + ret.mass = 1266 + STD_CARGO_KG + ret.wheelbase = 2.56 + elif candidate == CAR.SKODA_KAMIQ_MK1: ret.mass = 1265 + STD_CARGO_KG ret.wheelbase = 2.66 diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 30a51f6fe..b461fd02a 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -1,14 +1,11 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled): values = { - "SET_ME_0X3": 0x3, - "Assist_Torque": abs(apply_steer), - "Assist_Requested": lkas_enabled, - "Assist_VZ": 1 if apply_steer < 0 else 0, - "HCA_Available": 1, - "HCA_Standby": not lkas_enabled, - "HCA_Active": lkas_enabled, - "SET_ME_0XFE": 0xFE, - "SET_ME_0X07": 0x07, + "HCA_01_Status_HCA": 5 if lkas_enabled else 3, + "HCA_01_LM_Offset": abs(apply_steer), + "HCA_01_LM_OffSign": 1 if apply_steer < 0 else 0, + "HCA_01_Vib_Freq": 18, + "HCA_01_Sendestatus": 1 if lkas_enabled else 0, + "EA_ACC_Wunschgeschwindigkeit": 327.36, } return packer.make_can_msg("HCA_01", bus, values) @@ -56,18 +53,18 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active): return acc_control_value(main_switch_on, acc_faulted, long_active) -def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold): +def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_control, stopping, starting, esp_hold): commands = [] acc_06_values = { "ACC_Typ": acc_type, "ACC_Status_ACC": acc_control, - "ACC_StartStopp_Info": enabled, - "ACC_Sollbeschleunigung_02": accel if enabled else 3.01, + "ACC_StartStopp_Info": acc_enabled, + "ACC_Sollbeschleunigung_02": accel if acc_enabled else 3.01, "ACC_zul_Regelabw_unten": 0.2, # TODO: dynamic adjustment of comfort-band "ACC_zul_Regelabw_oben": 0.2, # TODO: dynamic adjustment of comfort-band - "ACC_neg_Sollbeschl_Grad_02": 4.0 if enabled else 0, # TODO: dynamic adjustment of jerk limits - "ACC_pos_Sollbeschl_Grad_02": 4.0 if enabled else 0, # TODO: dynamic adjustment of jerk limits + "ACC_neg_Sollbeschl_Grad_02": 4.0 if acc_enabled else 0, # TODO: dynamic adjustment of jerk limits + "ACC_pos_Sollbeschl_Grad_02": 4.0 if acc_enabled else 0, # TODO: dynamic adjustment of jerk limits "ACC_Anfahren": starting, "ACC_Anhalten": stopping, } @@ -84,9 +81,9 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, acc_07_values = { "ACC_Anhalteweg": 0.75 if stopping else 20.46, # Distance to stop (stopping coordinator handles terminal roll-out) - "ACC_Freilauf_Info": 2 if enabled else 0, + "ACC_Freilauf_Info": 2 if acc_enabled else 0, "ACC_Folgebeschl": 3.02, # Not using secondary controller accel unless and until we understand its impact - "ACC_Sollbeschleunigung_02": accel if enabled else 3.01, + "ACC_Sollbeschleunigung_02": accel if acc_enabled else 3.01, "ACC_Anforderung_HMS": acc_hold_type, "ACC_Anfahren": starting, "ACC_Anhalten": stopping, diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 130f10795..bac3ca121 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -59,17 +59,18 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active): return hud_status -def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold): +def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_control, stopping, starting, esp_hold): commands = [] values = { "ACS_Sta_ADR": acc_control, - "ACS_StSt_Info": acc_control != 1, + "ACS_StSt_Info": acc_enabled, "ACS_Typ_ACC": acc_type, "ACS_Anhaltewunsch": acc_type == 1 and stopping, - "ACS_Sollbeschl": accel if acc_control == 1 else 3.01, - "ACS_zul_Regelabw": 0.2 if acc_control == 1 else 1.27, - "ACS_max_AendGrad": 3.0 if acc_control == 1 else 5.08, + "ACS_FreigSollB": acc_enabled, + "ACS_Sollbeschl": accel if acc_enabled else 3.01, + "ACS_zul_Regelabw": 0.2 if acc_enabled else 1.27, + "ACS_max_AendGrad": 3.0 if acc_enabled else 5.08, } commands.append(packer.make_can_msg("ACC_System", bus, values)) @@ -83,9 +84,9 @@ def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance "ACA_Zeitluecke": 2, "ACA_V_Wunsch": set_speed, "ACA_gemZeitl": lead_distance, - # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke - # display/display-prio handling probably needed to stop confusing the instrument cluster - # kmh_mph handling probably needed to resolve rounding errors in displayed setpoint + "ACA_PrioDisp": 3, + # TODO: restore dynamic pop-to-foreground/highlight behavior with ACA_PrioDisp and ACA_AnzDisplay + # TODO: ACA_kmh_mph handling probably needed to resolve rounding errors in displayed setpoint } - return packer.make_can_msg("ACC_GRA_Anziege", bus, values) + return packer.make_can_msg("ACC_GRA_Anzeige", bus, values) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index cd30bef73..db28a6243 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -7,7 +7,7 @@ from cereal import car from panda.python import uds from opendbc.can.can_define import CANDefine from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit, HarnessPart from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -36,7 +36,7 @@ class CarControllerParams: if CP.carFingerprint in PQ_CARS: self.LDW_STEP = 5 # LDW_1 message frequency 20Hz - self.ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz + self.ACC_HUD_STEP = 4 # ACC_GRA_Anzeige frequency 25Hz self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm self.STEER_DELTA_UP = 6 # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00)) self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) @@ -129,6 +129,7 @@ class CAR: AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca SEAT_LEON_MK3 = "SEAT LEON 3RD GEN" # Chassis 5F, Mk3 SEAT Leon and variants + SKODA_FABIA_MK4 = "SKODA FABIA 4TH GEN" # Chassis PJ, Mk4 Skoda Fabia SKODA_KAMIQ_MK1 = "SKODA KAMIQ 1ST GEN" # Chassis NW, Mk1 Skoda Kamiq SKODA_KAROQ_MK1 = "SKODA KAROQ 1ST GEN" # Chassis NU, Mk1 Skoda Karoq SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN" # Chassis NS, Mk1 Skoda Kodiaq @@ -165,7 +166,7 @@ class Footnote(Enum): @dataclass class VWCarInfo(CarInfo): package: str = "Adaptive Cruise Control (ACC) & Lane Assist" - harness: Enum = Harness.j533 + harness_kit: HarnessKit = HarnessKit(Harness.j533, parts=[HarnessPart.harness_box, HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler]) def init_make(self, CP: car.CarParams): self.footnotes.insert(0, Footnote.VW_EXP_LONG) @@ -213,8 +214,8 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { ], CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22"), CAR.POLO_MK6: [ - VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_MQB_A0]), - VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_MQB_A0]), + VWCarInfo("Volkswagen Polo 2018-23", footnotes=[Footnote.VW_MQB_A0]), + VWCarInfo("Volkswagen Polo GTI 2018-23", footnotes=[Footnote.VW_MQB_A0]), ], CAR.SHARAN_MK2: [ VWCarInfo("Volkswagen Sharan 2018-22"), @@ -222,7 +223,10 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { ], CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022"), CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0]), - CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2018-23"), + CAR.TIGUAN_MK2: [ + VWCarInfo("Volkswagen Tiguan 2018-23"), + VWCarInfo("Volkswagen Tiguan eHybrid 2021-23"), + ], CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"), CAR.TRANSPORTER_T61: [ VWCarInfo("Volkswagen Caravelle 2020"), @@ -239,9 +243,10 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2019-23"), CAR.SEAT_ATECA_MK1: VWCarInfo("SEAT Ateca 2018"), CAR.SEAT_LEON_MK3: VWCarInfo("SEAT Leon 2014-20"), + CAR.SKODA_FABIA_MK4: VWCarInfo("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0]), CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]), CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21"), - CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2018-19"), + CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2017-23"), CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020", footnotes=[Footnote.VW_MQB_A0]), CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"), CAR.SKODA_OCTAVIA_MK3: [ @@ -287,32 +292,39 @@ FW_QUERY_CONFIG = FwQueryConfig( FW_VERSIONS = { CAR.ARTEON_MK1: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x873G0906259M \xf1\x890003', b'\xf1\x873G0906259F \xf1\x890004', b'\xf1\x873G0906259N \xf1\x890004', b'\xf1\x873G0906259P \xf1\x890001', b'\xf1\x875NA907115H \xf1\x890002', + b'\xf1\x873G0906259G \xf1\x890004', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870DL300014C \xf1\x893704', b'\xf1\x8709G927158L \xf1\x893611', b'\xf1\x870GC300011L \xf1\x891401', b'\xf1\x870GC300014M \xf1\x892802', b'\xf1\x870GC300040P \xf1\x891401', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x875QF959655AP\xf1\x890755\xf1\x82\x1311110011111311111100110200--1611125F49', b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121157161111572900', b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121177161113772900', b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00', + b'\xf1\x873Q0959655CK\xf1\x890711\xf1\x82\x0e1712141712141105121122052900', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002NB4202N7N', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571B41815A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571B00817A1', - b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567B0020800', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020800', b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002MB4092M7N', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572T \xf1\x890383', b'\xf1\x875Q0907572J \xf1\x890654', + b'\xf1\x875Q0907572R \xf1\x890771', ], }, CAR.ATLAS_MK1: { @@ -382,6 +394,7 @@ FW_VERSIONS = { b'\xf1\x8704E906016A \xf1\x897697', b'\xf1\x8704E906016AD\xf1\x895758', b'\xf1\x8704E906016CE\xf1\x899096', + b'\xf1\x8704E906016CH\xf1\x899226', b'\xf1\x8704E906023AG\xf1\x891726', b'\xf1\x8704E906023BN\xf1\x894518', b'\xf1\x8704E906024K \xf1\x896811', @@ -392,6 +405,8 @@ FW_VERSIONS = { b'\xf1\x8704L906021DT\xf1\x895520', b'\xf1\x8704L906021DT\xf1\x898127', b'\xf1\x8704L906021N \xf1\x895518', + b'\xf1\x8704L906021N \xf1\x898138', + b'\xf1\x8704L906026BN\xf1\x891197', b'\xf1\x8704L906026BP\xf1\x897608', b'\xf1\x8704L906026NF\xf1\x899528', b'\xf1\x8704L906056CL\xf1\x893823', @@ -428,6 +443,7 @@ FW_VERSIONS = { b'\xf1\x8709G927749AP\xf1\x892943', b'\xf1\x8709S927158A \xf1\x893585', b'\xf1\x870CW300040H \xf1\x890606', + b'\xf1\x870CW300041D \xf1\x891004', b'\xf1\x870CW300041H \xf1\x891010', b'\xf1\x870CW300042F \xf1\x891604', b'\xf1\x870CW300043B \xf1\x891601', @@ -436,7 +452,9 @@ FW_VERSIONS = { b'\xf1\x870CW300044T \xf1\x895245', b'\xf1\x870CW300045 \xf1\x894531', b'\xf1\x870CW300047D \xf1\x895261', + b'\xf1\x870CW300047E \xf1\x895261', b'\xf1\x870CW300048J \xf1\x890611', + b'\xf1\x870CW300049H \xf1\x890905', b'\xf1\x870D9300012 \xf1\x894904', b'\xf1\x870D9300012 \xf1\x894913', b'\xf1\x870D9300012 \xf1\x894937', @@ -467,6 +485,7 @@ FW_VERSIONS = { b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120043114317121C111C9113', b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120043114417121411149113', b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120053114317121C111C9113', + b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\x13141500111233003142114A2131219333313100', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333463100', b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2251229333463100', @@ -512,6 +531,7 @@ FW_VERSIONS = { b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0511A00403A0', b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1', b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00404A1', + b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00504A1', b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00604A1', b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A07A02A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521A00507A1', @@ -529,6 +549,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x875Q0907567G \xf1\x890390\xf1\x82\x0101', b'\xf1\x875Q0907567J \xf1\x890396\xf1\x82\x0101', + b'\xf1\x875Q0907567L \xf1\x890098\xf1\x82\x0101', b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\x0101', b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\x0101', b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\x0101', @@ -598,6 +619,7 @@ FW_VERSIONS = { b'\xf1\x8704L906026FP\xf1\x892012', b'\xf1\x8704L906026GA\xf1\x892013', b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x873G0906259B \xf1\x890002', b'\xf1\x873G0906264 \xf1\x890004', ], (Ecu.transmission, 0x7e1, None): [ @@ -608,7 +630,10 @@ FW_VERSIONS = { b'\xf1\x870D9300041A \xf1\x894801', b'\xf1\x870DD300045T \xf1\x891601', b'\xf1\x870DL300011H \xf1\x895201', + b'\xf1\x870CW300042H \xf1\x891601', b'\xf1\x870GC300042H \xf1\x891404', + b'\xf1\x870D9300018C \xf1\x895297', + b'\xf1\x870GC300043 \xf1\x892301', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', @@ -616,7 +641,10 @@ FW_VERSIONS = { b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311', b'\xf1\x873Q0959655BA\xf1\x890195\xf1\x82\r56140056130012516612125111', b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211', + b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\x0e5915005914001305701311052900', + b'\xf1\x873Q0959655BG\xf1\x890712\xf1\x82\x0e5915005914001305701311052900', b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\0165915005914001344701311442900', + b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e5915005914001354701311542900', b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e5915005914001305701311052900', b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111', ], @@ -629,6 +657,7 @@ FW_VERSIONS = { b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1', b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516B00501A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1', + b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563B0000600', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020600', ], (Ecu.fwdRadar, 0x757, None): [ @@ -663,17 +692,24 @@ FW_VERSIONS = { CAR.POLO_MK6: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704C906025H \xf1\x895177', + b'\xf1\x8705C906032J \xf1\x891702', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300042D \xf1\x891612', b'\xf1\x870CW300050D \xf1\x891908', + b'\xf1\x870CW300051G \xf1\x891909', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AG\xf1\x890248\xf1\x82\x1218130411110411--04040404231811152H14', b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1248130411110416--04040404784811152H14', + b'\xf1\x872Q0959655AS\xf1\x890411\xf1\x82\x1384830511110516041405820599841215391471', ], (Ecu.eps, 0x712, None): [ b'\xf1\x872Q1909144M \xf1\x896041', + b'\xf1\x872Q2909144AB\xf1\x896050', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572R \xf1\x890372', ], }, @@ -735,28 +771,35 @@ FW_VERSIONS = { b'\xf1\x8704L906027G \xf1\x899893', b'\xf1\x875N0906259 \xf1\x890002', b'\xf1\x875NA906259H \xf1\x890002', + b'\xf1\x875NA907115E \xf1\x890003', b'\xf1\x875NA907115E \xf1\x890005', b'\xf1\x8783A907115B \xf1\x890005', b'\xf1\x8783A907115F \xf1\x890002', b'\xf1\x8783A907115G \xf1\x890001', + b'\xf1\x8783A907115K \xf1\x890001', + b'\xf1\x8704E906024AP\xf1\x891461', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158DT\xf1\x893698', b'\xf1\x8709G927158FM\xf1\x893757', b'\xf1\x8709G927158GC\xf1\x893821', b'\xf1\x8709G927158GD\xf1\x893820', + b'\xf1\x8709G927158GM\xf1\x893936', b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300011N \xf1\x892001', b'\xf1\x870DL300011N \xf1\x892012', + b'\xf1\x870DL300012M \xf1\x892107', b'\xf1\x870DL300012P \xf1\x892103', b'\xf1\x870DL300013A \xf1\x893005', b'\xf1\x870DL300013G \xf1\x892119', b'\xf1\x870DL300013G \xf1\x892120', b'\xf1\x870DL300014C \xf1\x893703', + b'\xf1\x870DD300046K \xf1\x892302', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\02331310031333334313132573732379333313100', b'\xf1\x875Q0959655BJ\xf1\x890336\xf1\x82\x1312110031333300314232583732379333423100', + b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x1331310031333334313132013730379333423100', b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100', @@ -764,9 +807,11 @@ FW_VERSIONS = { b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140573752379333423100', b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100', b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x1331310031333300314240024050409333613100', + b'\xf1\x875Q0959655CD\xf1\x890421\xf1\x82\x13123112313333003145406F6154619333613100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527A6050705', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A60604A1', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6000600', b'\xf1\x875QF909144A \xf1\x895581\xf1\x82\x0571A60834A1', @@ -775,6 +820,8 @@ 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\x002SA6092SOM', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6017A00', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572AA\xf1\x890396', @@ -788,34 +835,45 @@ FW_VERSIONS = { CAR.TOURAN_MK2: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906026HM\xf1\x893017', + b'\xf1\x8705E906018CQ\xf1\x890808', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300041E \xf1\x891005', + b'\xf1\x870CW300051M \xf1\x891926', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\023363500213533353141324C4732479333313100', + b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1336350021353336314740025250529333613100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820531B0062105', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A8090400', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x872Q0907572AA\xf1\x890396', ], }, CAR.TRANSPORTER_T61: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906056AG\xf1\x899970', + b'\xf1\x8704L906056AL\xf1\x899970', b'\xf1\x8704L906057AP\xf1\x891186', b'\xf1\x8704L906057N \xf1\x890413', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870BT300012G \xf1\x893102', b'\xf1\x870BT300012E \xf1\x893105', + b'\xf1\x870BT300046R \xf1\x893102', ], (Ecu.srs, 0x715, None): [ - b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\02316170411110411--04041704161611152S1411', + b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\x1316170411110411--04041704161611152S1411', + b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\x1316170411110411--04041704171711152S1411', b'\xf1\x872Q0959655AF\xf1\x890506\xf1\x82\x1316171111110411--04041711121211152S1413', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\x0532380518A2', + b'\xf1\x877LA909144G \xf1\x897160\xf1\x82\x05333A5519A2', b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\005323A5519A2', ], (Ecu.fwdRadar, 0x757, None): [ @@ -827,9 +885,11 @@ FW_VERSIONS = { b'\xf1\x8705E906018AT\xf1\x899640', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300050J \xf1\x891911', b'\xf1\x870CW300051M \xf1\x891925', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1311110012333300314240681152119333463100', b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x13111100123333003142404M1152119333613100', ], (Ecu.eps, 0x712, None): [ @@ -856,6 +916,7 @@ FW_VERSIONS = { b'\xf1\x878V0906264B \xf1\x890003', b'\xf1\x878V0907115B \xf1\x890007', b'\xf1\x878V0907404A \xf1\x890005', + b'\xf1\x875G0906259D \xf1\x890002', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300044T \xf1\x895245', @@ -863,6 +924,7 @@ FW_VERSIONS = { b'\xf1\x870D9300012 \xf1\x894912', b'\xf1\x870D9300012 \xf1\x894931', b'\xf1\x870D9300012K \xf1\x894513', + b'\xf1\x870D9300013B \xf1\x894902', b'\xf1\x870D9300013B \xf1\x894931', b'\xf1\x870D9300041N \xf1\x894512', b'\xf1\x870D9300043T \xf1\x899699', @@ -900,11 +962,13 @@ FW_VERSIONS = { b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00803A0', b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0503G0G803A0', b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516G00804A1', + b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516G00804A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1', ], (Ecu.fwdRadar, 0x757, None): [ - b'\xf1\x875Q0907567N \xf1\x890400\xf1\x82\00101', - b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\00101', + b'\xf1\x875Q0907567N \xf1\x890400\xf1\x82\x0101', + b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\x0101', + b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101', b'\xf1\x875Q0907572G \xf1\x890571', b'\xf1\x875Q0907572H \xf1\x890620', b'\xf1\x875Q0907572P \xf1\x890682', @@ -933,14 +997,17 @@ FW_VERSIONS = { b'\xf1\x8705L906022M \xf1\x890901', b'\xf1\x8783A906259 \xf1\x890001', b'\xf1\x8783A906259 \xf1\x890005', + b'\xf1\x8783A906259F \xf1\x890001', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158CN\xf1\x893608', + b'\xf1\x8709G927158GP\xf1\x893937', b'\xf1\x870GC300045D \xf1\x892802', b'\xf1\x870GC300046F \xf1\x892701', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655BF\xf1\x890403\xf1\x82\x1321211111211200311121232152219321422111', + b'\xf1\x875Q0959655BQ\xf1\x890421\xf1\x82\x132121111121120031112124218C219321532111', b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111224118A119321532111', b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111', ], @@ -948,8 +1015,10 @@ FW_VERSIONS = { b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000800', b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\x0571G60533A1', + b'\xf1\x875TA907145D \xf1\x891051\xf1\x82\x001PG60A1P7N', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572R \xf1\x890372', b'\xf1\x872Q0907572T \xf1\x890383', ], @@ -1006,6 +1075,23 @@ FW_VERSIONS = { b'\xf1\x875Q0907572P \xf1\x890682', ], }, + CAR.SKODA_FABIA_MK4: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8705E906018CF\xf1\x891905', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300051M \xf1\x891936', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875QF959655AT\xf1\x890755\xf1\x82\x1311110011110011111100110200--1111120749', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872Q1909144S \xf1\x896042', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', + ], + }, CAR.SKODA_KAMIQ_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8705C906032M \xf1\x891333', @@ -1049,27 +1135,44 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906027DD\xf1\x893123', b'\xf1\x8704L906026DE\xf1\x895418', + b'\xf1\x8704L906026EJ\xf1\x893661', + b'\xf1\x8704L906026HT\xf1\x893617', + b'\xf1\x8783A907115E \xf1\x890001', + b'\xf1\x8705E906018DJ\xf1\x890915', + b'\xf1\x8705E906018DJ\xf1\x891903', b'\xf1\x875NA907115E \xf1\x890003', + b'\xf1\x875NA907115E \xf1\x890005', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870D9300043 \xf1\x895202', + b'\xf1\x870DL300011N \xf1\x892014', b'\xf1\x870DL300012M \xf1\x892107', b'\xf1\x870DL300012N \xf1\x892110', b'\xf1\x870DL300013G \xf1\x892119', + b'\xf1\x870GC300014N \xf1\x892801', + b'\xf1\x870GC300046Q \xf1\x892802', ], (Ecu.srs, 0x715, None): [ - b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\0161213001211001205212111052100', - b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\0161213001211001205212112052100', + b'\xf1\x873Q0959655AP\xf1\x890306\xf1\x82\r11110011110011421111314211', + b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\x0e1213001211001205212111052100', + b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1213001211001244212111442100', + b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e1213001211001205212112052100', b'\xf1\x873Q0959655CQ\xf1\x890720\xf1\x82\x0e1213111211001205212112052111', + b'\xf1\x873Q0959655DJ\xf1\x890731\xf1\x82\x0e1513001511001205232113052J00', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6060405', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6070405', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G500', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G600', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572Q \xf1\x890342', b'\xf1\x872Q0907572R \xf1\x890372', + b'\xf1\x872Q0907572T \xf1\x890383', + b'\xf1\x872Q0907572AA\xf1\x890396', + b'\xf1\x872Q0907572AB\xf1\x890397', ], }, CAR.SKODA_OCTAVIA_MK3: { @@ -1139,21 +1242,29 @@ FW_VERSIONS = { b'\xf1\x8704L906026FP\xf1\x891196', b'\xf1\x8704L906026KB\xf1\x894071', b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x8704L906026MT\xf1\x893076', + b'\xf1\x873G0906259 \xf1\x890004', b'\xf1\x873G0906259B \xf1\x890002', + b'\xf1\x873G0906259L \xf1\x890003', b'\xf1\x873G0906264A \xf1\x890002', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300042H \xf1\x891601', b'\xf1\x870D9300011T \xf1\x894801', b'\xf1\x870D9300012 \xf1\x894940', + b'\xf1\x870D9300013A \xf1\x894905', b'\xf1\x870D9300041H \xf1\x894905', + b'\xf1\x870GC300014M \xf1\x892801', b'\xf1\x870GC300043 \xf1\x892301', + b'\xf1\x870D9300043F \xf1\x895202', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\x12111200111121001121110012211292221111', b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111', b'\xf1\x875Q0959655AK\xf1\x890130\xf1\x82\022111200111121001121110012211292221111', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100', + b'\xf1\x875Q0959655CA\xf1\x890403\xf1\x82\x1331310031313100313151013141319331423100', + b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1333310031313100313152025350539331463100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514UZ070203', @@ -1161,11 +1272,14 @@ FW_VERSIONS = { b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700', b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563UZ060600', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070600', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070700', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x873Q0907572B \xf1\x890192', b'\xf1\x873Q0907572B \xf1\x890194', b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x875Q0907572R \xf1\x890771', + b'\xf1\x875Q0907572S \xf1\x890780', ], }, } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2c359ec32..959740d97 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -26,9 +26,7 @@ from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.locationd.calibrationd import Calibration from system.hardware import HARDWARE -from selfdrive.manager.process_config import managed_processes SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -37,9 +35,7 @@ LANE_DEPARTURE_THRESHOLD = 0.1 REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ NOSENSOR = "NOSENSOR" in os.environ -IGNORE_PROCESSES = {"uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "statsd", - "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"} | \ - {k for k, v in managed_processes.items() if not v.enabled} +IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} ThermalStatus = log.DeviceState.ThermalStatus State = log.ControlsState.OpenpilotState @@ -112,9 +108,6 @@ class Controls: if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - if self.CP.dashcamOnly and self.params.get_bool("DashcamOverride"): - self.CP.dashcamOnly = False - # read params self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") @@ -280,17 +273,21 @@ class Controls: # Alert if fan isn't spinning for 5 seconds if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: - if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: - if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: + if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50: + # allow enough time for the fan controller in the panda to recover from stalls + if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0: self.events.add(EventName.fanMalfunction) else: self.last_functional_fan_frame = self.sm.frame # Handle calibration status cal_status = self.sm['liveCalibration'].calStatus - if cal_status != Calibration.CALIBRATED: - if cal_status == Calibration.UNCALIBRATED: + if cal_status != log.LiveCalibrationData.Status.calibrated: + if cal_status == log.LiveCalibrationData.Status.uncalibrated: self.events.add(EventName.calibrationIncomplete) + elif cal_status == log.LiveCalibrationData.Status.recalibrating: + set_offroad_alert("Offroad_Recalibration", True) + self.events.add(EventName.calibrationRecalibrating) else: self.events.add(EventName.calibrationInvalid) @@ -338,9 +335,9 @@ class Controls: self.events.add(EventName.cameraMalfunction) elif not self.sm.all_freq_ok(self.camera_packets): self.events.add(EventName.cameraFrameRate) - if self.rk.lagging: + if not REPLAY and self.rk.lagging: self.events.add(EventName.controlsdLagging) - if len(self.sm['radarState'].radarErrors) or not self.sm.all_checks(['radarState']): + if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])): self.events.add(EventName.radarFault) if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) @@ -413,7 +410,7 @@ class Controls: # TODO: fix simulator if not SIMULATION: if not NOSENSOR: - if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000): + if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000): # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes self.events.add(EventName.noGps) @@ -702,15 +699,15 @@ class Controls: recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ - and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED + and not CC.latActive and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction if len(desire_prediction) and ldw_allowed: right_lane_visible = model_v2.laneLineProbs[2] > 0.5 left_lane_visible = model_v2.laneLineProbs[1] > 0.5 - l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1] - r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1] + l_lane_change_prob = desire_prediction[Desire.laneChangeLeft] + r_lane_change_prob = desire_prediction[Desire.laneChangeRight] lane_lines = model_v2.laneLines l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET)) diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 2f85ea917..42d2512e5 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -48,5 +48,9 @@ "Offroad_NoFirmware": { "text": "openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.", "severity": 0 + }, + "Offroad_Recalibration": { + "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.", + "severity": 0 } } diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index a332d0676..7cbcbc3d4 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,6 +1,6 @@ import math -from cereal import car +from cereal import car, log from common.conversions import Conversions as CV from common.numpy_fast import clip, interp from common.realtime import DT_MDL @@ -17,14 +17,14 @@ V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors MIN_SPEED = 1.0 -LAT_MPC_N = 16 -LON_MPC_N = 32 CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 # EU guidelines MAX_LATERAL_JERK = 5.0 +MAX_VEL_ERR = 5.0 + ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type CRUISE_LONG_PRESS = 50 @@ -192,3 +192,21 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): current_curvature_desired + max_curvature_rate * DT_MDL) return safe_desired_curvature, safe_desired_curvature_rate + + +def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float: + friction_interp = interp( + apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), + [-friction_threshold, friction_threshold], + [-torque_params.friction, torque_params.friction] + ) + friction = float(friction_interp) if friction_compensation else 0.0 + return friction + + +def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: + # ToDo: Try relative error, and absolute speed + if len(modelV2.temporalPose.trans): + vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) + return float(vel_err) + return 0.0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index a85e48649..b935e2461 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -242,8 +242,9 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + first_word = 'Recalibration' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibration' return Alert( - "Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc, + f"{first_word} in Progress: {sm['liveCalibration'].calPerc:.0f}%", f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", AlertStatus.normal, AlertSize.mid, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) @@ -292,7 +293,7 @@ def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging rpy = sm['liveCalibration'].rpyCalib yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan) pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan) - angles = f"Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°" + angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)" return NormalPermanentAlert("Calibration Invalid", angles) @@ -317,9 +318,9 @@ def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: - text = "Cruise Mode Disabled" + text = "Enable Adaptive Cruise to Engage" if CP.carName == "honda": - text = "Main Switch Off" + text = "Enable Main Switch to Engage" return NoEntryAlert(text) @@ -358,6 +359,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { # Car is recognized, but marked as dashcam only EventName.startupNoControl: { ET.PERMANENT: StartupAlert("Dashcam mode"), + ET.NO_ENTRY: NoEntryAlert("Dashcam mode"), }, # Car is not recognized @@ -658,6 +660,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), }, + EventName.steerTimeLimit: { + ET.SOFT_DISABLE: soft_disable_alert("Vehicle Steering Time Limit"), + ET.NO_ENTRY: NoEntryAlert("Vehicle Steering Time Limit"), + }, + EventName.outOfSpace: { ET.PERMANENT: out_of_space_alert, ET.NO_ENTRY: NoEntryAlert("Out of Storage"), @@ -714,9 +721,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.calibrationIncomplete: { ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Calibration in Progress"), + ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), }, + + EventName.calibrationRecalibrating: { + ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"), + ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"), + }, EventName.doorOpen: { ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), @@ -811,10 +824,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), }, - EventName.accFaultedTemp: { - ET.NO_ENTRY: NoEntryAlert("Cruise Temporarily Faulted"), - }, - EventName.controlsMismatch: { ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"), ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"), @@ -944,4 +953,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"), }, + EventName.vehicleSensorsInvalid: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"), + ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"), + ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), + }, + } diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2f5609437..6550b1922 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -61,10 +61,12 @@ class LatControlTorque(LatControl): low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - error = setpoint - measurement gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY - pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error, + torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.torque_params, setpoint, lateral_accel_deadzone, friction_compensation=False) + torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params, measurement, + lateral_accel_deadzone, friction_compensation=False) + pid_log.error = torque_from_setpoint - torque_from_measurement ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript index 868b5a873..745ed99d1 100644 --- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript +++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript @@ -47,6 +47,7 @@ acados_dir = '#third_party/acados' acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' source_list = ['lat_mpc.py', + '#/selfdrive/modeld/constants.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', f'{acados_dir}/x86_64/lib/libacados.so', f'{acados_dir}/larch64/lib/libacados.so', diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 536f436fc..ca7b991e6 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -3,8 +3,8 @@ import os import numpy as np from casadi import SX, vertcat, sin, cos - from common.realtime import sec_since_boot +# WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import T_IDXS if __name__ == '__main__': # generating code @@ -17,12 +17,12 @@ EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json") X_DIM = 4 P_DIM = 2 -N = 16 COST_E_DIM = 3 COST_DIM = COST_E_DIM + 2 SPEED_OFFSET = 10.0 MODEL_NAME = 'lat' ACADOS_SOLVER_TYPE = 'SQP_RTI' +N = 32 def gen_lat_model(): model = AcadosModel() @@ -168,14 +168,14 @@ class LateralMpc(): self.solver.constraints_set(0, "lbx", x0_cp) self.solver.constraints_set(0, "ubx", x0_cp) self.yref[:,0] = y_pts - v_ego = p_cp[0] + v_ego = p_cp[0, 0] # rotation_radius = p_cp[1] self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET) self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.set(i, "p", p_cp) - self.solver.set(N, "p", p_cp) + self.solver.set(i, "p", p_cp[i]) + self.solver.set(N, "p", p_cp[N]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) t = sec_since_boot() diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 932ad4953..fc29aeb08 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -4,7 +4,7 @@ from common.numpy_fast import interp from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N -from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED +from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error from selfdrive.controls.lib.desire_helper import DesireHelper import cereal.messaging as messaging from cereal import log @@ -16,12 +16,12 @@ CAMERA_OFFSET = 0.04 PATH_COST = 1.0 LATERAL_MOTION_COST = 0.11 LATERAL_ACCEL_COST = 0.0 -LATERAL_JERK_COST = 0.05 +LATERAL_JERK_COST = 0.04 # Extreme steering rate is unpleasant, even # when it does not cause bad jerk. # TODO this cost should be lowered when low # speed lateral control is stable on all cars -STEERING_RATE_COST = 800.0 +STEERING_RATE_COST = 700.0 class LateralPlanner: @@ -35,10 +35,15 @@ class LateralPlanner: self.solution_invalid_cnt = 0 self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) + self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) - self.y_pts = np.zeros(TRAJECTORY_SIZE) + self.y_pts = np.zeros((TRAJECTORY_SIZE,)) + self.v_plan = np.zeros((TRAJECTORY_SIZE,)) + self.v_ego = 0.0 + self.l_lane_change_prob = 0.0 + self.r_lane_change_prob = 0.0 self.lat_mpc = LateralMpc() self.reset_mpc(np.zeros(4)) @@ -49,8 +54,8 @@ class LateralPlanner: def update(self, sm): # clip speed , lateral planning is not possible at 0 speed - self.v_ego = max(MIN_SPEED, sm['carState'].vEgo) measured_curvature = sm['controlsState'].curvature + v_ego_car = sm['carState'].vEgo # Parse model predictions md = sm['modelV2'] @@ -59,6 +64,10 @@ class LateralPlanner: self.t_idxs = np.array(md.position.t) self.plan_yaw = np.array(md.orientation.z) self.plan_yaw_rate = np.array(md.orientationRate.z) + self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z]) + car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car) + self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf) + self.v_ego = self.v_plan[0] # Lane change logic desire_state = md.meta.desireState @@ -68,21 +77,20 @@ class LateralPlanner: lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) - d_path_xyz = self.path_xyz self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST) - y_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) - heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) - yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) + y_pts = self.path_xyz[:LAT_MPC_N+1, 1] + heading_pts = self.plan_yaw[:LAT_MPC_N+1] + yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1] self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 assert len(yaw_rate_pts) == LAT_MPC_N + 1 - lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2)) - p = np.array([self.v_ego, lateral_factor]) + lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf) + p = np.column_stack([self.v_plan, lateral_factor]) self.lat_mpc.run(self.x0, p, y_pts, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index e5b236060..7f5daf157 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -54,6 +54,7 @@ acados_dir = '#third_party/acados' acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' source_list = ['long_mpc.py', + '#/selfdrive/modeld/constants.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', f'{acados_dir}/x86_64/lib/libacados.so', f'{acados_dir}/larch64/lib/libacados.so', diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index c01795123..660002691 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -5,6 +5,7 @@ import numpy as np from common.realtime import sec_since_boot from common.numpy_fast import clip from system.swaglog import cloudlog +# WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import index_function from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0febfbafd..308949968 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -11,7 +11,7 @@ from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error from system.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s @@ -106,8 +106,7 @@ class LongitudinalPlanner: # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error - if len(sm['modelV2'].temporalPose.trans): - self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego + self.v_model_error = get_speed_error(sm['modelV2'], v_ego) if force_slow_decel: v_cruise = 0.0 @@ -121,8 +120,10 @@ class LongitudinalPlanner: x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j) - self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) - self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) + self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) + self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) + self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] + self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N] self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 93d0c80da..14934254a 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,12 +1,30 @@ #!/usr/bin/env python3 +import numpy as np from cereal import car from common.params import Params from common.realtime import Priority, config_realtime_process from system.swaglog import cloudlog +from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging +def cumtrapz(x, t): + return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))]) + +def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner): + plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, T_IDXS) + model_odo = cumtrapz(lateral_planner.v_plan, T_IDXS) + + ui_send = messaging.new_message('uiPlan') + ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) + uiPlan = ui_send.uiPlan + uiPlan.frameId = sm['modelV2'].frameId + uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist() + uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist() + uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() + uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist() + pm.send('uiPlan', ui_send) def plannerd_thread(sm=None, pm=None): config_realtime_process(5, Priority.CTRL_LOW) @@ -24,7 +42,7 @@ def plannerd_thread(sm=None, pm=None): poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) if pm is None: - pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan']) + pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan']) while True: sm.update() @@ -34,7 +52,7 @@ def plannerd_thread(sm=None, pm=None): lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) - + publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner) def main(sm=None, pm=None): plannerd_thread(sm, pm) diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index 07ce5ebdd..3a0ff33cb 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -32,6 +32,9 @@ SUPPORTED_FW_VERSIONS = { b"DN8_ SCC FHCUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + b"DN8_ SCC F-CUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2021 SONATA HYBRID b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 8952405b8..6b5ec3693 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -49,7 +49,7 @@ if __name__ == "__main__": sw_pn = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER).decode("utf-8") sw_ver = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER).decode("utf-8") component = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.SYSTEM_NAME_OR_ENGINE_TYPE).decode("utf-8") - odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8") + odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8").rstrip('\x00') current_coding = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING) # type: ignore coding_text = current_coding.hex() @@ -70,14 +70,14 @@ if __name__ == "__main__": coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0 coding_length = len(current_coding) - # EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS) - if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"): - coding_variant = "ZF" + # EPS_MQB_ZFLS + if odx_file in ("EV_SteerAssisMQB", "EV_SteerAssisMNB"): + coding_variant = "ZFLS" coding_byte = 0 coding_bit = 4 - # APA racks (MQB_PP_APA) have a different coding layout - elif odx_file == "EV_SteerAssisVWBSMQBA\x00\x00\x00\x00": + # MQB_PP_APA, MQB_VWBS_GEN2 + elif odx_file in ("EV_SteerAssisVWBSMQBA", "EV_SteerAssisVWBSMQBGen2"): coding_variant = "APA" coding_byte = 3 coding_bit = 0 @@ -111,8 +111,8 @@ if __name__ == "__main__": if args.action in ["enable", "disable"]: print("\nAttempting configuration update") - assert(coding_variant in ("ZF", "APA")) - # ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the + assert(coding_variant in ("ZFLS", "APA")) + # ZFLS EPS config coding length can be anywhere from 1 to 4 bytes, but the # bit we care about is always in the same place in the first byte if args.action == "enable": new_byte = current_coding_array[coding_byte] | (1 << coding_bit) diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 86a228a6f..11b9f127b 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -1,6 +1,3 @@ -ubloxd -ubloxd_test params_learner paramsd locationd -test/test_glonass_runner diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 61a0ed7f4..740f827a4 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,20 +1,6 @@ Import('env', 'common', 'cereal', 'messaging', 'libkf', 'transformations') -loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread'] - -if GetOption('kaitai'): - generated = Dir('generated').srcnode().abspath - cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" - env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd) - env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) - glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd) - - # kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910 - patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES') - env.Depends(patch, glonass) - -glonass_obj = env.Object('generated/glonass.cpp') -env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs) +loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread'] ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] @@ -25,7 +11,4 @@ lenv.Depends(locationd, libkf) if File("liblocationd.cc").exists(): liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations) - lenv.Depends(liblocationd, libkf) - -if GetOption('test'): - env.Program("test/test_glonass_runner", ['test/test_glonass_runner.cc', 'test/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs]) \ No newline at end of file + lenv.Depends(liblocationd, libkf) \ No newline at end of file diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 1c68eb67b..dc4ffbb1f 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -25,7 +25,7 @@ MAX_VEL_ANGLE_STD = np.radians(0.25) MAX_YAW_RATE_FILTER = np.radians(2) # per second # This is at model frequency, blocks needed for efficiency -SMOOTH_CYCLES = 400 +SMOOTH_CYCLES = 10 BLOCK_SIZE = 100 INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_WANTED = 50 # We want a little bit more than we need for stability @@ -39,12 +39,6 @@ YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235]) DEBUG = os.getenv("DEBUG") is not None -class Calibration: - UNCALIBRATED = 0 - CALIBRATED = 1 - INVALID = 2 - - def is_calibration_valid(rpy: np.ndarray) -> bool: return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) # type: ignore @@ -69,6 +63,7 @@ class Calibrator: rpy_init = RPY_INIT wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT valid_blocks = 0 + self.cal_status = log.LiveCalibrationData.Status.uncalibrated if param_put and calibration_params: try: @@ -134,16 +129,20 @@ class Calibrator: self.calib_spread = np.zeros(3) if self.valid_blocks < INPUTS_NEEDED: - self.cal_status = Calibration.UNCALIBRATED + if self.cal_status == log.LiveCalibrationData.Status.recalibrating: + self.cal_status = log.LiveCalibrationData.Status.recalibrating + else: + self.cal_status = log.LiveCalibrationData.Status.uncalibrated elif is_calibration_valid(self.rpy): - self.cal_status = Calibration.CALIBRATED + self.cal_status = log.LiveCalibrationData.Status.calibrated else: - self.cal_status = Calibration.INVALID + self.cal_status = log.LiveCalibrationData.Status.invalid # If spread is too high, assume mounting was changed and reset to last block. # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. - if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED: - self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy) + if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated: + self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy) + self.cal_status = log.LiveCalibrationData.Status.recalibrating write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: @@ -162,7 +161,7 @@ class Calibrator: rot: List[float], wide_from_device_euler: List[float], trans_std: List[float]) -> Optional[np.ndarray]: - self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) + self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) angle_std_threshold = MAX_VEL_ANGLE_STD @@ -210,7 +209,7 @@ class Calibrator: if self.not_car: liveCalibration.validBlocks = INPUTS_NEEDED - liveCalibration.calStatus = Calibration.CALIBRATED + liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated liveCalibration.calPerc = 100. liveCalibration.rpyCalib = [0, 0, 0] liveCalibration.rpyCalibSpread = self.calib_spread.tolist() diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 2eb1e9452..71b81cc30 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -1,14 +1,12 @@ #!/usr/bin/env python3 -import json import math import os import time import shutil from collections import defaultdict from concurrent.futures import Future, ProcessPoolExecutor -from datetime import datetime from enum import IntEnum -from typing import List, Optional +from typing import List, Optional, Dict, Any import numpy as np @@ -17,9 +15,9 @@ from 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 Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem +from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem from laika.gps_time import GPSTime -from laika.helpers import ConstellationId +from laika.helpers import ConstellationId, get_sv_id from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind @@ -28,15 +26,53 @@ from selfdrive.locationd.models.gnss_kf import States as GStates from system.swaglog import cloudlog MAX_TIME_GAP = 10 -EPHEMERIS_CACHE = 'LaikadEphemerisV2' +EPHEMERIS_CACHE = 'LaikadEphemerisV3' DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/" 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=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False, - valid_ephem_types=(EphemerisType.NAV,), + 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 @@ -52,9 +88,9 @@ class Laikad: self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None self.orbit_fetch_future: Optional[Future] = None - self.last_fetch_navs_t = None - self.got_first_gnss_msg = False - self.last_cached_t = 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() @@ -64,59 +100,109 @@ class Laikad: self.last_fix_t = None self.gps_week = None self.use_qcom = use_qcom + self.first_log_time = None + self.ttff = -1 def load_cache(self): if not self.save_ephemeris: return - cache = Params().get(EPHEMERIS_CACHE) - if not cache: + cache_bytes = Params().get(EPHEMERIS_CACHE) + if not cache_bytes: return + nav_dict = {} try: - cache = json.loads(cache, object_hook=deserialize_hook) - if cache['version'] == CACHE_VERSION: - self.astro_dog.add_navs(cache['navs']) - self.last_fetch_navs_t = cache['last_fetch_navs_t'] - else: - cache['navs'] = {} - except json.decoder.JSONDecodeError: + ephem_cache = ephemeris_structs.EphemerisCache.from_bytes(cache_bytes) + 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") - timestamp = self.last_fetch_navs_t.as_datetime() if self.last_fetch_navs_t is not None else 'Nan' cloudlog.debug( - f"Loaded navs ({sum([len(v) for v in cache['navs']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['navs'].keys())} " + - f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.navs_fetched_times._ranges]}") + 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, t: GPSTime): - if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN): - put_nonblocking(EPHEMERIS_CACHE, json.dumps( - {'version': CACHE_VERSION, 'last_fetch_navs_t': self.last_fetch_navs_t, 'navs': self.astro_dog.navs}, - cls=CacheSerializer)) + 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()], []) + ephem_cache = ephemeris_structs.EphemerisCache(**{'glonassEphemerides': [e.data for e in nav_list if e.prn[0]=='R'], + 'gpsEphemerides': [e.data for e in nav_list if e.prn[0]=='G']}) + + put_nonblocking(EPHEMERIS_CACHE, ephem_cache.to_bytes()) cloudlog.debug("Cache saved") - self.last_cached_t = t + self.last_cached_t = self.last_report_time + + def create_ephem_statuses(self): + ephemeris_statuses = [] + prns_to_check = list(self.astro_dog.get_all_ephem_prns()) + prns_to_check.sort() + for prn in prns_to_check: + eph = self.astro_dog.get_eph(prn, self.last_report_time) + if eph is not None: + status = log.GnssMeasurements.EphemerisStatus.new_message() + status.constellationId = ConstellationId.from_rinex_char(prn[0]).value + status.svId = get_sv_id(prn) + status.type = get_log_eph_type(eph).value + status.source = get_log_eph_source(eph).value + 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 = 7 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 6 - position_solution, pr_residuals = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) + 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] - #TODO median abs residual is decent estimate of std, can be improved with measurements stds and/or DOP - position_std = np.median(np.abs(pr_residuals)) * np.ones(3) - velocity_solution, prr_residuals = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements) + + position_std_residual = np.median(np.abs(pr_residuals)) + position_std = np.median(np.abs(pos_std))/10 + position_std = max(position_std_residual, position_std) * np.ones(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 = np.median(np.abs(prr_residuals)) * np.ones(3) + + velocity_std_residual = np.median(np.abs(prr_residuals)) + velocity_std = np.median(np.abs(vel_std))/10 + velocity_std = max(velocity_std, velocity_std_residual) * np.ones(3) + return position_estimate, position_std, velocity_estimate, velocity_std + def gps_time_from_qcom_report(self, gnss_msg): + report = gnss_msg.drMeasurementReport + if report.source == log.QcomGnss.MeasurementSource.gps: + report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) + elif report.source == log.QcomGnss.MeasurementSource.sbas: + report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) + elif report.source == log.QcomGnss.MeasurementSource.glonass: + report_time = GPSTime.from_glonass(report.glonassYear, + report.glonassDay, + report.glonassMilliseconds / 1000.0) + else: + raise NotImplementedError(f'Unknownconstellation {report.source}') + return report_time + def is_good_report(self, gnss_msg): if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) - # TODO support GLONASS - return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS] + # TODO: Understand and use remaining unknown constellations + try: + good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS] + except NotImplementedError: + good_constellation = False + # Garbage timestamps with week > 32767 are sometimes sent by module. + # This is an issue with gpsTime and GLONASS time. + report_time = self.gps_time_from_qcom_report(gnss_msg) + 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: @@ -124,16 +210,26 @@ class Laikad: def read_report(self, gnss_msg): if self.use_qcom: + # QCOM reports are per constellation, should always send 3 reports report = gnss_msg.drMeasurementReport - week = report.gpsWeek - tow = report.gpsMilliseconds / 1000.0 - new_meas = read_raw_qcom(report) + report_time = self.gps_time_from_qcom_report(gnss_msg) + + if report_time - self.last_report_time > 0: + self.qcom_reports = [report] + else: + self.qcom_reports.append(report) + self.last_report_time = report_time + + new_meas = [] + if len(self.qcom_reports) == 3: + for report in self.qcom_reports: + new_meas.extend(read_raw_qcom(report)) + else: report = gnss_msg.measurementReport - week = report.gpsWeek - tow = report.rcvTow + self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow) new_meas = read_raw_ublox(report) - return week, tow, new_meas + return self.last_report_time, new_meas def is_ephemeris(self, gnss_msg): if self.use_qcom: @@ -146,29 +242,49 @@ class Laikad: # TODO this is not robust to gps week rollover if self.gps_week is None: return - ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) + try: + ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) + 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': - ephem = convert_ublox_gps_ephem(gnss_msg.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': - ephem = convert_ublox_glonass_ephem(gnss_msg.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(t=ephem.epoch) + 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: - corrected_measurements = correct_measurements(processed_measurements, self.last_fix_pos, self.astro_dog) - instant_fix = self.get_lsq_fix(t, corrected_measurements) - #instant_fix = self.get_lsq_fix(t, processed_measurements) + est_pos = self.last_fix_pos + correct_delay = True else: - corrected_measurements = [] - instant_fix = self.get_lsq_fix(t, processed_measurements) + 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) + 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: @@ -176,60 +292,51 @@ class Laikad: self.last_fix_t = t self.last_fix_pos = position_estimate self.lat_fix_pos_std = position_std - if (t*1e9) % 10 == 0: - cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}") - return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements + 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} + 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) - return None elif self.is_good_report(gnss_msg): - - week, tow, new_meas = self.read_report(gnss_msg) - if len(new_meas) == 0: - return None - - self.gps_week = week - t = gnss_mono_time * 1e-9 - if week > 0: - self.got_first_gnss_msg = True - latest_msg_t = GPSTime(week, tow) + report_t, new_meas = self.read_report(gnss_msg) + self.gps_week = report_t.week + if report_t.week > 0: if self.auto_fetch_navs: - self.fetch_navs(latest_msg_t, block) + self.fetch_navs(report_t, block) - output = self.process_report(new_meas, t) - if output is None: - return None - position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output + corrected_measurements = self.process_report(new_meas, t) + msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements] - self.update_localizer(position_estimate, t, corrected_measurements) - meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] - msg = messaging.new_message("gnssMeasurements") + 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.gnssMeasurements = { - "gpsWeek": week, - "gpsTimeOfWeek": tow, - "kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(), + 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), - "kalmanVelocityECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].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), - "positionECEF": measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t)), - "velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)), - - "measTime": gnss_mono_time, - "correctedMeasurements": meas_msgs - } - return msg - - #elif gnss_msg.which() == 'ionoData': - # TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them. + 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 @@ -241,7 +348,7 @@ class Laikad: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") - if len(est_pos) > 0: + 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: @@ -266,7 +373,7 @@ class Laikad: 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 (self.last_fetch_navs_t is None or abs(t - self.last_fetch_navs_t) > SECS_IN_MIN): + 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 @@ -284,7 +391,7 @@ class Laikad: 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(t=t) + self.cache_ephemeris() def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir): @@ -314,31 +421,8 @@ def create_measurement_msg(meas: GNSSMeasurement): c.satPos = meas.sat_pos_final.tolist() c.satVel = meas.sat_vel.tolist() c.satVel = meas.sat_vel.tolist() - ephem = meas.sat_ephemeris - assert ephem is not None - week, time_of_week = -1, -1 - if ephem.eph_type == EphemerisType.NAV: - source_type = EphemerisSourceType.nav - elif ephem.eph_type == EphemerisType.QCOM_POLY: - source_type = EphemerisSourceType.qcom - else: - assert ephem.file_epoch is not None - week = ephem.file_epoch.week - time_of_week = ephem.file_epoch.tow - file_src = ephem.file_source - if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z' - source_type = EphemerisSourceType.nasaUltraRapid - elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3' - source_type = EphemerisSourceType.glonassIacUltraRapid - else: - raise Exception(f"Didn't expect file source {file_src}") - - c.ephemerisSource.type = source_type.value - c.ephemerisSource.gpsWeek = week - c.ephemerisSource.gpsTimeOfWeek = int(time_of_week) return c - def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]): ekf_data = defaultdict(list) for m in measurements: @@ -354,93 +438,38 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe gnss_kf.predict_and_observe(t, kind, data) -class CacheSerializer(json.JSONEncoder): - - def default(self, o): - if isinstance(o, Ephemeris): - return o.to_json() - if isinstance(o, GPSTime): - return o.__dict__ - if isinstance(o, np.ndarray): - return o.tolist() - return json.JSONEncoder.default(self, o) - - -def deserialize_hook(dct): - if 'ephemeris' in dct: - return Ephemeris.from_json(dct) - if 'week' in dct: - return GPSTime(dct['week'], dct['tow']) - return dct - - -class EphemerisSourceType(IntEnum): - nav = 0 - nasaUltraRapid = 1 - glonassIacUltraRapid = 2 - qcom = 3 - - -def process_msg(laikad, gnss_msg, mono_time, block=False): - # TODO: Understand and use remaining unknown constellations - if gnss_msg.which() == "drMeasurementReport": - if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']: - return None - - if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max: - # gpsWeek 65535 is received rarely from quectel, this cannot be - # passed to GnssMeasurements's gpsWeek (Int16) - return None - - return laikad.process_gnss_msg(gnss_msg, mono_time, block=block) - - def clear_tmp_cache(): if os.path.exists(DOWNLOADS_CACHE_FOLDER): shutil.rmtree(DOWNLOADS_CACHE_FOLDER) os.mkdir(DOWNLOADS_CACHE_FOLDER) -def main(sm=None, pm=None, qc=None): +def main(sm=None, pm=None): #clear_tmp_cache() use_qcom = not Params().get_bool("UbloxAvailable", block=True) - if use_qcom or (qc is not None and qc): - raw_gnss_socket = "qcomGnss" + if use_qcom: + raw_name = "qcomGnss" else: - raw_gnss_socket = "ubloxGnss" - - if sm is None: - sm = messaging.SubMaster([raw_gnss_socket, 'clocks']) + raw_name = "ubloxGnss" + raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False) if pm is None: pm = messaging.PubMaster(['gnssMeasurements']) # disable until set as main gps source, to better analyze startup time - use_internet = False #"LAIKAD_NO_INTERNET" not in os.environ + use_internet = False # "LAIKAD_NO_INTERNET" not in os.environ replay = "REPLAY" in os.environ - if replay or "CI" in os.environ: + if replay: use_internet = True laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom) while True: - sm.update() + 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 sm.updated[raw_gnss_socket]: - gnss_msg = sm[raw_gnss_socket] - - msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay) - if msg is None: - # TODO: beautify this, locationd needs a valid message - msg = messaging.new_message("gnssMeasurements") - pm.send('gnssMeasurements', msg) - - if not laikad.got_first_gnss_msg and sm.updated['clocks']: - clocks_msg = sm['clocks'] - t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9)) - if laikad.auto_fetch_navs: - laikad.fetch_navs(t, block=replay) if __name__ == "__main__": main() diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 307626506..9ee9e33fb 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -27,7 +27,6 @@ const double MAX_FILTER_REWIND_TIME = 0.8; // s // They should be replaced with synced time from a real clock const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s -const float GPS_MUL_FACTOR = 10.0; const float GPS_POS_STD_THRESHOLD = 50.0; const float GPS_VEL_STD_THRESHOLD = 5.0; const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; @@ -181,6 +180,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); fix.setDeviceStable(!this->device_fell); fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER); + fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff); this->device_fell = false; //fix.setGpsWeek(this->time.week); @@ -304,13 +304,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); - // quectel gps verticalAccuracy is clipped to 500 - bool gps_accuracy_insane_quectel = false; - if (!ublox_available) { - gps_accuracy_insane_quectel = log.getVerticalAccuracy() == 500; - } - - if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane || gps_accuracy_insane_quectel) { + if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { //this->gps_valid = false; this->determine_gps_mode(current_time); return; @@ -326,8 +320,14 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; - MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getAccuracy(),2) + std::pow(GPS_MUL_FACTOR * log.getVerticalAccuracy(),2)).asDiagonal(); - MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getSpeedAccuracy(), 2)).asDiagonal(); + float ecef_pos_std; + if (ublox_available) { + ecef_pos_std = std::sqrt(std::pow(log.getAccuracy(), 2) + std::pow(log.getVerticalAccuracy(), 2)); + } else { + ecef_pos_std = std::sqrt(3 * std::pow(log.getVerticalAccuracy(), 2)); + } + MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); this->unix_timestamp_millis = log.getUnixTimestampMillis(); double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); @@ -377,14 +377,14 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: // indexed at 0 cause all std values are the same MAE auto ecef_pos_std = log.getPositionECEF().getStd()[0]; - MatrixXdr ecef_pos_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_pos_std, 2)).asDiagonal(); + MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); auto ecef_vel_v = log.getVelocityECEF().getValue(); VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); // indexed at 0 cause all std values are the same MAE auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; - MatrixXdr ecef_vel_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_vel_std, 2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); @@ -507,7 +507,7 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra this->calib = live_calib; this->device_from_calib = euler2rot(this->calib); this->calib_from_device = this->device_from_calib.transpose(); - this->calibrated = log.getCalStatus() == 1; + this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; this->observation_values_invalid["liveCalibration"] *= DECAY; } } @@ -530,6 +530,9 @@ void Localizer::time_check(double current_time) { if (std::isnan(this->last_reset_time)) { this->last_reset_time = current_time; } + if (std::isnan(this->first_valid_log_time)) { + this->first_valid_log_time = current_time; + } double filter_time = this->kf->get_filter_time(); bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); if (big_time_gap) { @@ -661,8 +664,10 @@ int Localizer::locationd_thread() { const char* gps_location_socket; if (ublox_available) { gps_location_socket = "gpsLocationExternal"; + this->gps_std_factor = 10.0; } else { gps_location_socket = "gpsLocation"; + this->gps_std_factor = 2.0; } const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; @@ -699,6 +704,11 @@ int Localizer::locationd_thread() { bool gpsOK = this->is_gps_ok(); bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); + // Log time to first fix + if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { + this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); + } + MessageBuilder msg_builder; kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); pm.send("liveLocationKalman", bytes.begin(), bytes.size()); diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index a292a3c93..e2b2096af 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -14,7 +14,7 @@ #include "common/timing.h" #include "common/util.h" -#include "selfdrive/sensord/sensors/constants.h" +#include "system/sensord/sensors/constants.h" #define VISION_DECIMATION 2 #define SENSOR_DECIMATION 10 #include "selfdrive/locationd/models/live_kf.h" @@ -78,10 +78,13 @@ private: double reset_tracker = 0.0; bool device_fell = false; bool gps_mode = false; + double first_valid_log_time = NAN; + double ttff = NAN; double last_gps_msg = 0; bool ublox_available = true; bool observation_timings_invalid = false; std::map observation_values_invalid; bool standstill = true; int32_t orientation_reset_count = 0; + float gps_std_factor; }; diff --git a/selfdrive/manager/helpers.py b/selfdrive/manager/helpers.py index 983c7cc0b..f8607fffc 100644 --- a/selfdrive/manager/helpers.py +++ b/selfdrive/manager/helpers.py @@ -36,3 +36,8 @@ def unblock_stdout() -> None: # whose low byte is the signal number and whose high byte is the exit status exit_status = os.wait()[1] >> 8 os._exit(exit_status) + + +def write_onroad_params(started, params): + params.put_bool("IsOnroad", started) + params.put_bool("IsOffroad", not started) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 865966d6c..a1efd564f 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -14,13 +14,14 @@ from common.params import Params, ParamKeyType from common.text_window import TextWindow from selfdrive.boardd.set_time import set_time from system.hardware import HARDWARE, PC -from selfdrive.manager.helpers import unblock_stdout +from selfdrive.manager.helpers import unblock_stdout, write_onroad_params from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID from system.swaglog import cloudlog, add_file_handler from system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ - terms_version, training_version, is_tested_branch, is_release_branch + get_normalized_origin, terms_version, training_version, \ + is_tested_branch, is_release_branch @@ -29,14 +30,14 @@ def manager_init() -> None: set_time(cloudlog) # save boot log - subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) + subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "system/loggerd")) params = Params() params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) default_params: List[Tuple[str, Union[str, bytes]]] = [ ("CompletedTrainingVersion", "0"), - ("DisengageOnAccelerator", "1"), + ("DisengageOnAccelerator", "0"), ("GsmMetered", "1"), ("HasAcceptedTerms", "0"), ("LanguageSetting", "main_en"), @@ -92,7 +93,12 @@ def manager_init() -> None: # init logging sentry.init(sentry.SentryProject.SELFDRIVE) - cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=is_dirty(), + cloudlog.bind_global(dongle_id=dongle_id, + version=get_version(), + origin=get_normalized_origin(), + branch=get_short_branch(), + commit=get_commit(), + dirty=is_dirty(), device=HARDWARE.get_device_type()) @@ -130,12 +136,27 @@ def manager_thread() -> None: sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState']) pm = messaging.PubMaster(['managerState']) + write_onroad_params(False, params) ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) + started_prev = False + while True: sm.update() started = sm['deviceState'].started + + if started and not started_prev: + params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) + elif not started and started_prev: + params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + + # update onroad params, which drives boardd's safety setter thread + if started != started_prev: + write_onroad_params(started, params) + + started_prev = started + ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) @@ -153,7 +174,7 @@ def manager_thread() -> None: for param in ("DoUninstall", "DoShutdown", "DoReboot"): if params.get_bool(param): shutdown = True - params.put("LastManagerExitReason", param) + params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}") cloudlog.warning(f"Shutting down manager - {param} set") if shutdown: diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 8fc4d94e5..57c42aaaa 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -17,11 +17,17 @@ def logging(started, params, CP: car.CarParams) -> bool: run = (not CP.notCar) or not params.get_bool("DisableLogging") return started and run +def ublox_available() -> bool: + return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') + def ublox(started, params, CP: car.CarParams) -> bool: - use_ublox = os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') + use_ublox = ublox_available() params.put_bool("UbloxAvailable", use_ublox) return started and use_ublox +def qcomgps(started, params, CP: car.CarParams) -> bool: + return started and not ublox_available() + procs = [ # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption NativeProcess("camerad", "system/camerad", ["./camerad"], unkillable=True, callback=driverview), @@ -34,12 +40,12 @@ procs = [ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview), - NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), - NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), + NativeProcess("encoderd", "system/loggerd", ["./encoderd"]), + NativeProcess("loggerd", "system/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False), NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False), - NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), + NativeProcess("sensord", "system/sensord", ["./sensord"], enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), @@ -47,21 +53,21 @@ procs = [ PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"), PythonProcess("torqued", "selfdrive.locationd.torqued"), PythonProcess("controlsd", "selfdrive.controls.controlsd"), - PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True), + PythonProcess("deleter", "system.loggerd.deleter", offroad=True), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview), PythonProcess("laikad", "selfdrive.locationd.laikad"), - PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=TICI), + PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI, onroad=False, callback=qcomgps), PythonProcess("navd", "selfdrive.navd.navd"), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), - NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=TICI, onroad=False, callback=ublox), - PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI, onroad=False, callback=ublox), + NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], enabled=TICI, onroad=False, callback=ublox), + PythonProcess("pigeond", "system.sensord.pigeond", enabled=TICI, onroad=False, callback=ublox), PythonProcess("plannerd", "selfdrive.controls.plannerd"), PythonProcess("radard", "selfdrive.controls.radard"), PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True), PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True), PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True), - PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), + PythonProcess("uploader", "system.loggerd.uploader", offroad=True), PythonProcess("statsd", "selfdrive.statsd", offroad=True), # debug procs diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 7bbc1b347..c7e0dc5d8 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -70,7 +70,7 @@ lenv.Program('_dmonitoringmodeld', [ if use_thneed and arch == "larch64" or GetOption('pc_thneed'): fn = File("models/supercombo").abspath - tinygrad_opts = ["NATIVE_EXPLOG=1", "VALIDHACKS=1", "OPTWG=1", "IMAGE=2", "GPU=1", "CLCACHE=0"] + tinygrad_opts = ["NATIVE_EXPLOG=1", "VALIDHACKS=1", "OPTLOCAL=1", "IMAGE=2", "GPU=1", "ENABLE_METHOD_CACHE=1"] if not GetOption('pc_thneed'): # use FLOAT16 on device for speed + don't cache the CL kernels for space tinygrad_opts += ["FLOAT16=1", "PYOPENCL_NO_CACHE=1"] diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index ac101bfee..5538d6ff9 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -200,7 +200,7 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMet } template -void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array &t, +void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array &t, const std::array &x, const std::array &y, const std::array &z) { xyzt.setT(to_kj_array_ptr(t)); xyzt.setX(to_kj_array_ptr(x)); @@ -209,7 +209,7 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array -void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array &t, +void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array &t, const std::array &x, const std::array &y, const std::array &z, const std::array &x_std, const std::array &y_std, const std::array &z_std) { fill_xyzt(xyzt, t, x, y, z); diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 2f2dbc476..915883ad0 100644 Binary files a/selfdrive/modeld/models/supercombo.onnx and b/selfdrive/modeld/models/supercombo.onnx differ diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 35eee5b03..836ed9cc4 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -3,10 +3,10 @@ import gc import cereal.messaging as messaging from cereal import car +from cereal import log from common.params import Params, put_bool_nonblocking from common.realtime import set_realtime_priority from selfdrive.controls.lib.events import Events -from selfdrive.locationd.calibrationd import Calibration from selfdrive.monitoring.driver_monitor import DriverStatus @@ -22,7 +22,7 @@ def dmonitoringd_thread(sm=None, pm=None): driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected")) - sm['liveCalibration'].calStatus = Calibration.INVALID + sm['liveCalibration'].calStatus = log.LiveCalibrationData.Status.invalid sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].buttonEvents = [] sm['carState'].standstill = True diff --git a/selfdrive/navd/.gitignore b/selfdrive/navd/.gitignore new file mode 100644 index 000000000..a070fe32b --- /dev/null +++ b/selfdrive/navd/.gitignore @@ -0,0 +1,5 @@ +moc_* +*.moc + +map_renderer +libmap_renderer.so diff --git a/selfdrive/navd/README.md b/selfdrive/navd/README.md new file mode 100644 index 000000000..6c7f7eabe --- /dev/null +++ b/selfdrive/navd/README.md @@ -0,0 +1,24 @@ +# navigation + +This directory contains two daemons, `navd` and `map_renderer`, which support navigation in the openpilot stack. + +### navd + +`navd` takes in a route through the `NavDestination` param and sends out two packets: `navRoute` and `navInstruction`. These packets contain the coordinates of the planned route and turn-by-turn instructions. + +### map renderer + +The map renderer listens for the `navRoute` and publishes a rendered map view over VisionIPC for the navigation model, which lives in `selfdrive/modeld/`. The rendered maps look like this: + +![](https://i.imgur.com/oZLfmwq.png) + +## development + +Currently, [mapbox](https://www.mapbox.com/) is used for navigation. + +* get an API token: https://docs.mapbox.com/help/glossary/access-token/ +* set an API token using the `MAPBOX_TOKEN` environment variable +* routes/destinations are set through the `NavDestination` param + * use `set_destination.py` for debugging +* edit the map: https://www.mapbox.com/contribute +* mapbox API playground: https://docs.mapbox.com/playground/ diff --git a/selfdrive/navd/SConscript b/selfdrive/navd/SConscript new file mode 100644 index 000000000..23b36adc0 --- /dev/null +++ b/selfdrive/navd/SConscript @@ -0,0 +1,20 @@ +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') + +map_env = qt_env.Clone() +libs = ['qt_widgets', 'qt_util', 'qmapboxgl', common, messaging, cereal, visionipc, transformations, + 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', 'json11'] + map_env["LIBS"] +if arch == 'larch64': + libs.append('EGL') + +if arch in ['larch64', 'x86_64']: + if arch == 'x86_64': + rpath = Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath + map_env["RPATH"] += [rpath, ] + + style_path = File("style.json").abspath + map_env['CXXFLAGS'].append(f'-DSTYLE_PATH=\\"{style_path}\\"') + + map_env["RPATH"].append(Dir('.').abspath) + map_env["LIBPATH"].append(Dir('.').abspath) + maplib = map_env.SharedLibrary("maprender", ["map_renderer.cc"], LIBS=libs) + map_env.Program("map_renderer", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs) diff --git a/selfdrive/navd/main.cc b/selfdrive/navd/main.cc new file mode 100644 index 000000000..8cef07edc --- /dev/null +++ b/selfdrive/navd/main.cc @@ -0,0 +1,21 @@ +#include +#include +#include + +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" +#include "selfdrive/navd/map_renderer.h" +#include "system/hardware/hw.h" + +int main(int argc, char *argv[]) { + qInstallMessageHandler(swagLogMessageHandler); + + QApplication app(argc, argv); + std::signal(SIGINT, sigTermHandler); + std::signal(SIGTERM, sigTermHandler); + + MapRenderer * m = new MapRenderer(get_mapbox_settings()); + assert(m); + + return app.exec(); +} diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc new file mode 100644 index 000000000..203470bb4 --- /dev/null +++ b/selfdrive/navd/map_renderer.cc @@ -0,0 +1,303 @@ +#include "selfdrive/navd/map_renderer.h" + +#include +#include +#include +#include + +#include "common/util.h" +#include "common/timing.h" +#include "common/swaglog.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" + +const float DEFAULT_ZOOM = 13.5; // Don't go below 13 or features will start to disappear +const int HEIGHT = 256, WIDTH = 256; +const int NUM_VIPC_BUFFERS = 4; + +const int EARTH_CIRCUMFERENCE_METERS = 40075000; +const int PIXELS_PER_TILE = 256; + +const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE"); +const int LLK_DECIMATION = TEST_MODE ? 1 : 10; + +float get_zoom_level_for_scale(float lat, float meters_per_pixel) { + float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE; + float num_tiles = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / meters_per_tile; + return log2(num_tiles) - 1; +} + + +MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_settings(settings) { + QSurfaceFormat fmt; + fmt.setRenderableType(QSurfaceFormat::OpenGLES); + + ctx = std::make_unique(); + ctx->setFormat(fmt); + ctx->create(); + assert(ctx->isValid()); + + surface = std::make_unique(); + surface->setFormat(ctx->format()); + surface->create(); + + ctx->makeCurrent(surface.get()); + assert(QOpenGLContext::currentContext() == ctx.get()); + + gl_functions.reset(ctx->functions()); + gl_functions->initializeOpenGLFunctions(); + + QOpenGLFramebufferObjectFormat fbo_format; + fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format)); + + std::string style = util::read_file(STYLE_PATH); + m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1)); + m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), DEFAULT_ZOOM); + m_map->setStyleJson(style.c_str()); + m_map->createRenderer(); + + m_map->resize(fbo->size()); + m_map->setFramebufferObject(fbo->handle(), fbo->size()); + gl_functions->glViewport(0, 0, WIDTH, HEIGHT); + + QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) { + // https://github.com/mapbox/mapbox-gl-native/blob/cf734a2fec960025350d8de0d01ad38aeae155a0/platform/qt/include/qmapboxgl.hpp#L116 + //LOGD("new state %d", change); + }); + + QObject::connect(m_map.data(), &QMapboxGL::mapLoadingFailed, [=](QMapboxGL::MapLoadingFailure err_code, const QString &reason) { + LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str()); + }); + + if (online) { + vipc_server.reset(new VisionIpcServer("navd")); + vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH/2, HEIGHT/2); + vipc_server->start_listener(); + + pm.reset(new PubMaster({"navThumbnail", "mapRenderState"})); + sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}, {"liveLocationKalman"})); + + timer = new QTimer(this); + timer->setSingleShot(true); + QObject::connect(timer, SIGNAL(timeout()), this, SLOT(msgUpdate())); + timer->start(0); + } +} + +void MapRenderer::msgUpdate() { + sm->update(1000); + + if (sm->updated("liveLocationKalman")) { + auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); + auto pos = location.getPositionGeodetic(); + auto orientation = location.getCalibratedOrientationNED(); + + bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); + if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) { + updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2])); + + // TODO: use the static rendering mode + if (!loaded() && frame_id > 0) { + for (int i = 0; i < 5 && !loaded(); i++) { + LOGW("map render retry #%d, %d", i+1, m_map.isNull()); + QApplication::processEvents(QEventLoop::AllEvents, 100); + update(); + } + + if (!loaded()) { + LOGE("failed to render map after retry"); + } + } + } + } + + if (sm->updated("navRoute")) { + QList route; + auto coords = (*sm)["navRoute"].getNavRoute().getCoordinates(); + for (auto const &c : coords) { + route.push_back(QGeoCoordinate(c.getLatitude(), c.getLongitude())); + } + updateRoute(route); + } + + // schedule next update + timer->start(0); +} + +void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) { + if (m_map.isNull()) { + return; + } + + // Choose a scale that ensures above 13 zoom level up to and above 75deg of lat + float meters_per_pixel = 2; + float zoom = get_zoom_level_for_scale(position.first, meters_per_pixel); + + m_map->setCoordinate(position); + m_map->setBearing(bearing); + m_map->setZoom(zoom); + update(); +} + +bool MapRenderer::loaded() { + return m_map->isFullyLoaded(); +} + +void MapRenderer::update() { + double start_t = millis_since_boot(); + gl_functions->glClear(GL_COLOR_BUFFER_BIT); + m_map->render(); + gl_functions->glFlush(); + double end_t = millis_since_boot(); + + if ((vipc_server != nullptr) && loaded()) { + publish((end_t - start_t) / 1000.0); + } +} + +void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array &buf) { + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initNavThumbnail(); + thumbnaild.setFrameId(frame_id); + thumbnaild.setTimestampEof(ts); + thumbnaild.setThumbnail(buf); + pm->send("navThumbnail", msg); +} + +void MapRenderer::publish(const double render_time) { + QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); + uint64_t ts = nanos_since_boot(); + VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); + VisionIpcBufExtra extra = { + .frame_id = frame_id, + .timestamp_sof = (*sm)["liveLocationKalman"].getLogMonoTime(), + .timestamp_eof = ts, + }; + + assert(cap.sizeInBytes() >= buf->len); + uint8_t* dst = (uint8_t*)buf->addr; + uint8_t* src = cap.bits(); + + // RGB to greyscale + memset(dst, 128, buf->len); + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; + } + + vipc_server->send(buf, &extra); + + // Send thumbnail + if (TEST_MODE) { + // Full image in thumbnails in test mode + kj::Array buffer_kj = kj::heapArray((const capnp::byte*)cap.bits(), cap.sizeInBytes()); + sendThumbnail(ts, buffer_kj); + } else if (frame_id % 100 == 0) { + // Write jpeg into buffer + QByteArray buffer_bytes; + QBuffer buffer(&buffer_bytes); + buffer.open(QIODevice::WriteOnly); + cap.save(&buffer, "JPG", 50); + + kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size()); + sendThumbnail(ts, buffer_kj); + } + + // Send state msg + MessageBuilder msg; + auto state = msg.initEvent().initMapRenderState(); + state.setLocationMonoTime((*sm)["liveLocationKalman"].getLogMonoTime()); + state.setRenderTime(render_time); + state.setFrameId(frame_id); + pm->send("mapRenderState", msg); + + frame_id++; +} + +uint8_t* MapRenderer::getImage() { + QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); + + uint8_t* src = cap.bits(); + uint8_t* dst = new uint8_t[WIDTH * HEIGHT]; + + // RGB to greyscale + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; + } + + return dst; +} + +void MapRenderer::updateRoute(QList coordinates) { + if (m_map.isNull()) return; + initLayers(); + + auto route_points = coordinate_list_to_collection(coordinates); + QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); + QVariantMap navSource; + navSource["type"] = "geojson"; + navSource["data"] = QVariant::fromValue(feature); + m_map->updateSource("navSource", navSource); + m_map->setLayoutProperty("navLayer", "visibility", "visible"); +} + +void MapRenderer::initLayers() { + if (!m_map->layerExists("navLayer")) { + QVariantMap nav; + nav["id"] = "navLayer"; + nav["type"] = "line"; + nav["source"] = "navSource"; + m_map->addLayer(nav, "road-intersection"); + m_map->setPaintProperty("navLayer", "line-color", QColor("grey")); + m_map->setPaintProperty("navLayer", "line-width", 5); + m_map->setLayoutProperty("navLayer", "line-cap", "round"); + } +} + +MapRenderer::~MapRenderer() { +} + +extern "C" { + MapRenderer* map_renderer_init(char *maps_host = nullptr, char *token = nullptr) { + char *argv[] = { + (char*)"navd", + nullptr + }; + int argc = 0; + QApplication *app = new QApplication(argc, argv); + assert(app); + + QMapboxGLSettings settings; + settings.setApiBaseUrl(maps_host == nullptr ? MAPS_HOST : maps_host); + settings.setAccessToken(token == nullptr ? get_mapbox_token() : token); + + return new MapRenderer(settings, false); + } + + void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) { + inst->updatePosition({lat, lon}, bearing); + QApplication::processEvents(); + } + + void map_renderer_update_route(MapRenderer *inst, char* polyline) { + inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline))); + } + + void map_renderer_update(MapRenderer *inst) { + inst->update(); + } + + void map_renderer_process(MapRenderer *inst) { + QApplication::processEvents(); + } + + bool map_renderer_loaded(MapRenderer *inst) { + return inst->loaded(); + } + + uint8_t * map_renderer_get_image(MapRenderer *inst) { + return inst->getImage(); + } + + void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) { + delete[] buf; + } +} diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h new file mode 100644 index 000000000..0019030b6 --- /dev/null +++ b/selfdrive/navd/map_renderer.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cereal/visionipc/visionipc_server.h" +#include "cereal/messaging/messaging.h" + + +class MapRenderer : public QObject { + Q_OBJECT + +public: + MapRenderer(const QMapboxGLSettings &, bool online=true); + uint8_t* getImage(); + void update(); + bool loaded(); + ~MapRenderer(); + +private: + std::unique_ptr ctx; + std::unique_ptr surface; + std::unique_ptr gl_functions; + std::unique_ptr fbo; + + std::unique_ptr vipc_server; + std::unique_ptr pm; + std::unique_ptr sm; + void publish(const double render_time); + void sendThumbnail(const uint64_t ts, const kj::Array &buf); + + QMapboxGLSettings m_settings; + QScopedPointer m_map; + + void initLayers(); + + uint32_t frame_id = 0; + + QTimer* timer; + +public slots: + void updatePosition(QMapbox::Coordinate position, float bearing); + void updateRoute(QList coordinates); + void msgUpdate(); +}; diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py new file mode 100755 index 000000000..aa5682169 --- /dev/null +++ b/selfdrive/navd/map_renderer.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 +# You might need to uninstall the PyQt5 pip package to avoid conflicts + +import os +import time +import numpy as np +import polyline +from cffi import FFI + +from common.ffi_wrapper import suffix +from common.basedir import BASEDIR + +HEIGHT = WIDTH = SIZE = 256 +METERS_PER_PIXEL = 2 + + +def get_ffi(): + lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmaprender" + suffix()) + + ffi = FFI() + ffi.cdef(""" +void* map_renderer_init(char *maps_host, char *token); +void map_renderer_update_position(void *inst, float lat, float lon, float bearing); +void map_renderer_update_route(void *inst, char *polyline); +void map_renderer_update(void *inst); +void map_renderer_process(void *inst); +bool map_renderer_loaded(void *inst); +uint8_t* map_renderer_get_image(void *inst); +void map_renderer_free_image(void *inst, uint8_t *buf); +""") + return ffi, ffi.dlopen(lib) + + +def wait_ready(lib, renderer): + while not lib.map_renderer_loaded(renderer): + lib.map_renderer_update(renderer) + + # The main qt app is not execed, so we need to periodically process events for e.g. network requests + lib.map_renderer_process(renderer) + + time.sleep(0.01) + + +def get_image(lib, renderer): + buf = lib.map_renderer_get_image(renderer) + r = list(buf[0:WIDTH * HEIGHT]) + lib.map_renderer_free_image(renderer, buf) + + # Convert to numpy + r = np.asarray(r) + return r.reshape((WIDTH, HEIGHT)) + + +def navRoute_to_polyline(nr): + coords = [(m.latitude, m.longitude) for m in nr.navRoute.coordinates] + return coords_to_polyline(coords) + + +def coords_to_polyline(coords): + # TODO: where does this factor of 10 come from? + return polyline.encode([(lat * 10., lon * 10.) for lat, lon in coords]) + + +def polyline_to_coords(p): + coords = polyline.decode(p) + return [(lat / 10., lon / 10.) for lat, lon in coords] + + + +if __name__ == "__main__": + import matplotlib.pyplot as plt + + ffi, lib = get_ffi() + renderer = lib.map_renderer_init(ffi.NULL, ffi.NULL) + wait_ready(lib, renderer) + + geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL" + lib.map_renderer_update_route(renderer, geometry.encode()) + + POSITIONS = [ + (32.71569271952601, -117.16384270868463, 0), (32.71569271952601, -117.16384270868463, 45), # San Diego + (52.378641991483136, 4.902623379456488, 0), (52.378641991483136, 4.902623379456488, 45), # Amsterdam + ] + plt.figure() + + for i, pos in enumerate(POSITIONS): + t = time.time() + lib.map_renderer_update_position(renderer, *pos) + wait_ready(lib, renderer) + + print(f"{pos} took {time.time() - t:.2f} s") + + plt.subplot(2, 2, i + 1) + plt.imshow(get_image(lib, renderer), cmap='gray') + + plt.show() diff --git a/selfdrive/navd/set_destination.py b/selfdrive/navd/set_destination.py new file mode 100755 index 000000000..e6158dbde --- /dev/null +++ b/selfdrive/navd/set_destination.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +import json +import sys + +from common.params import Params + +if __name__ == "__main__": + params = Params() + + # set from google maps url + if len(sys.argv) > 1: + coords = sys.argv[1].split("/@")[-1].split("/")[0].split(",") + dest = { + "latitude": float(coords[0]), + "longitude": float(coords[1]) + } + params.put("NavDestination", json.dumps(dest)) + params.remove("NavDestinationWaypoints") + else: + print("Setting to Taco Bell") + dest = { + "latitude": 32.71160109904473, + "longitude": -117.12556569985693, + } + params.put("NavDestination", json.dumps(dest)) + + waypoints = [ + (-117.16020713111648, 32.71997612490662), + ] + params.put("NavDestinationWaypoints", json.dumps(waypoints)) + + print(dest) + print(waypoints) diff --git a/selfdrive/navd/style.json b/selfdrive/navd/style.json new file mode 100644 index 000000000..06bb750d1 --- /dev/null +++ b/selfdrive/navd/style.json @@ -0,0 +1 @@ +{"version": 8, "name": "Navigation Model", "metadata": {"mapbox:type": "default", "mapbox:origin": "monochrome-dark-v1", "mapbox:sdk-support": {"android": "10.0.0", "ios": "10.0.0", "js": "2.3.0"}, "mapbox:autocomposite": true, "mapbox:groups": {"Transit, transit-labels": {"name": "Transit, transit-labels", "collapsed": true}, "Administrative boundaries, admin": {"name": "Administrative boundaries, admin", "collapsed": true}, "Transit, bridges": {"name": "Transit, bridges", "collapsed": true}, "Transit, surface": {"name": "Transit, surface", "collapsed": true}, "Road network, bridges": {"name": "Road network, bridges", "collapsed": false}, "Land, water, & sky, water": {"name": "Land, water, & sky, water", "collapsed": true}, "Road network, tunnels": {"name": "Road network, tunnels", "collapsed": false}, "Road network, road-labels": {"name": "Road network, road-labels", "collapsed": true}, "Buildings, built": {"name": "Buildings, built", "collapsed": true}, "Natural features, natural-labels": {"name": "Natural features, natural-labels", "collapsed": true}, "Road network, surface": {"name": "Road network, surface", "collapsed": false}, "Land, water, & sky, built": {"name": "Land, water, & sky, built", "collapsed": true}, "Place labels, place-labels": {"name": "Place labels, place-labels", "collapsed": true}, "Point of interest labels, poi-labels": {"name": "Point of interest labels, poi-labels", "collapsed": true}, "Road network, tunnels-case": {"name": "Road network, tunnels-case", "collapsed": true}, "Transit, built": {"name": "Transit, built", "collapsed": true}, "Road network, surface-icons": {"name": "Road network, surface-icons", "collapsed": false}, "Land, water, & sky, land": {"name": "Land, water, & sky, land", "collapsed": true}}}, "center": [-117.19189443261149, 32.756553679559985], "zoom": 12.932776547838778, "bearing": 0, "pitch": 0.5017568344510897, "sources": {"composite": {"url": "mapbox://mapbox.mapbox-streets-v8", "type": "vector", "maxzoom": 13}}, "sprite": "mapbox://sprites/commaai/ckvmksrpd4n0a14pfdo5heqzr/bkx9h9tjdf3xedbnjvfo5xnbv", "glyphs": "mapbox://fonts/mapbox/{fontstack}/{range}.pbf", "layers": [{"id": "land", "type": "background", "layout": {"visibility": "none"}, "paint": {"background-color": "rgb(252, 252, 252)"}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, land"}}, {"minzoom": 5, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, land"}, "filter": ["==", ["get", "class"], "national_park"], "type": "fill", "source": "composite", "id": "national-park", "paint": {"fill-color": "rgb(240, 240, 240)", "fill-opacity": ["interpolate", ["linear"], ["zoom"], 5, 0, 6, 0.5, 10, 0.5]}, "source-layer": "landuse_overlay"}, {"minzoom": 5, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, land"}, "filter": ["match", ["get", "class"], ["park", "airport", "glacier", "pitch", "sand", "facility"], true, false], "type": "fill", "source": "composite", "id": "landuse", "paint": {"fill-color": "rgb(240, 240, 240)", "fill-opacity": ["interpolate", ["linear"], ["zoom"], 5, 0, 6, ["match", ["get", "class"], "glacier", 0.5, 1]]}, "source-layer": "landuse"}, {"id": "waterway-shadow", "type": "line", "source": "composite", "source-layer": "waterway", "minzoom": 8, "layout": {"line-cap": ["step", ["zoom"], "butt", 11, "round"], "line-join": "round", "visibility": "none"}, "paint": {"line-color": "rgb(204, 204, 204)", "line-width": ["interpolate", ["exponential", 1.3], ["zoom"], 9, ["match", ["get", "class"], ["canal", "river"], 0.1, 0], 20, ["match", ["get", "class"], ["canal", "river"], 8, 3]], "line-translate": ["interpolate", ["exponential", 1.2], ["zoom"], 7, ["literal", [0, 0]], 16, ["literal", [-1, -1]]], "line-translate-anchor": "viewport", "line-opacity": ["interpolate", ["linear"], ["zoom"], 8, 0, 8.5, 1]}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, water"}}, {"id": "water-shadow", "type": "fill", "source": "composite", "source-layer": "water", "layout": {"visibility": "none"}, "paint": {"fill-color": "rgb(204, 204, 204)", "fill-translate": ["interpolate", ["exponential", 1.2], ["zoom"], 7, ["literal", [0, 0]], 16, ["literal", [-1, -1]]], "fill-translate-anchor": "viewport"}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, water"}}, {"id": "waterway", "type": "line", "source": "composite", "source-layer": "waterway", "minzoom": 8, "layout": {"line-cap": ["step", ["zoom"], "butt", 11, "round"], "line-join": "round", "visibility": "none"}, "paint": {"line-color": "rgb(224, 224, 224)", "line-width": ["interpolate", ["exponential", 1.3], ["zoom"], 9, ["match", ["get", "class"], ["canal", "river"], 0.1, 0], 20, ["match", ["get", "class"], ["canal", "river"], 8, 3]], "line-opacity": ["interpolate", ["linear"], ["zoom"], 8, 0, 8.5, 1]}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, water"}}, {"id": "water", "type": "fill", "source": "composite", "source-layer": "water", "layout": {"visibility": "none"}, "paint": {"fill-color": "rgb(224, 224, 224)"}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, water"}}, {"minzoom": 13, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, built"}, "filter": ["all", ["==", ["geometry-type"], "Polygon"], ["==", ["get", "class"], "land"]], "type": "fill", "source": "composite", "id": "land-structure-polygon", "paint": {"fill-color": "rgb(252, 252, 252)"}, "source-layer": "structure"}, {"minzoom": 13, "layout": {"line-cap": "round", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "land-and-water", "mapbox:group": "Land, water, & sky, built"}, "filter": ["all", ["==", ["geometry-type"], "LineString"], ["==", ["get", "class"], "land"]], "type": "line", "source": "composite", "id": "land-structure-line", "paint": {"line-width": ["interpolate", ["exponential", 1.99], ["zoom"], 14, 0.75, 20, 40], "line-color": "rgb(252, 252, 252)"}, "source-layer": "structure"}, {"minzoom": 11, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "transit", "mapbox:group": "Transit, built"}, "filter": ["all", ["==", ["geometry-type"], "Polygon"], ["match", ["get", "type"], ["runway", "taxiway", "helipad"], true, false]], "type": "fill", "source": "composite", "id": "aeroway-polygon", "paint": {"fill-color": "rgb(255, 255, 255)", "fill-opacity": ["interpolate", ["linear"], ["zoom"], 11, 0, 11.5, 1]}, "source-layer": "aeroway"}, {"minzoom": 9, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "transit", "mapbox:group": "Transit, built"}, "filter": ["==", ["geometry-type"], "LineString"], "type": "line", "source": "composite", "id": "aeroway-line", "paint": {"line-color": "rgb(255, 255, 255)", "line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 9, ["match", ["get", "type"], "runway", 1, 0.5], 18, ["match", ["get", "type"], "runway", 80, 20]]}, "source-layer": "aeroway"}, {"minzoom": 13, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "buildings", "mapbox:group": "Buildings, built"}, "filter": ["all", ["!=", ["get", "type"], "building:part"], ["==", ["get", "underground"], "false"]], "type": "line", "source": "composite", "id": "building-outline", "paint": {"line-color": "rgb(227, 227, 227)", "line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 15, 0.75, 20, 3], "line-opacity": ["interpolate", ["linear"], ["zoom"], 15, 0, 16, 1]}, "source-layer": "building"}, {"minzoom": 13, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "buildings", "mapbox:group": "Buildings, built"}, "filter": ["all", ["!=", ["get", "type"], "building:part"], ["==", ["get", "underground"], "false"]], "type": "fill", "source": "composite", "id": "building", "paint": {"fill-color": ["interpolate", ["linear"], ["zoom"], 15, "rgb(242, 242, 242)", 16, "rgb(242, 242, 242)"], "fill-opacity": ["interpolate", ["linear"], ["zoom"], 15, 0, 16, 1], "fill-outline-color": "rgb(227, 227, 227)"}, "source-layer": "building"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels-case"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["step", ["zoom"], ["match", ["get", "class"], ["street", "street_limited", "primary_link", "track"], true, false], 1, ["match", ["get", "class"], ["street", "street_limited", "track", "primary_link", "secondary_link", "tertiary_link", "service"], true, false]], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-street-minor-low", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 2, "track", 1, 0.5], 18, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 18, 12]], "line-color": "rgb(235, 235, 235)", "line-opacity": ["step", ["zoom"], 1, 14, 0]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels-case"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["step", ["zoom"], ["match", ["get", "class"], ["street", "street_limited", "primary_link", "track"], true, false], 1, ["match", ["get", "class"], ["street", "street_limited", "track", "primary_link", "secondary_link", "tertiary_link", "service"], true, false]], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-street-minor-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(255, 255, 255)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 2, "track", 1, 0.5], 18, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 18, 12]], "line-opacity": ["step", ["zoom"], 0, 14, 1], "line-dasharray": [3, 3]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels-case"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["match", ["get", "class"], ["primary", "secondary", "tertiary"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-primary-secondary-tertiary-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, ["match", ["get", "class"], "primary", 1, 0.75], 18, 2], "line-color": "rgb(255, 255, 255)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, ["match", ["get", "class"], "primary", 0.75, 0.1], 18, ["match", ["get", "class"], "primary", 32, 26]], "line-dasharray": [3, 3]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels-case"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-major-link-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(255, 255, 255)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-dasharray": [3, 3]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels-case"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-motorway-trunk-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, 1, 18, 2], "line-color": "rgb(255, 255, 255)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-dasharray": [3, 3]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels-case"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["==", ["get", "class"], "construction"], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-construction", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, 2, 18, 18], "line-color": "rgb(235, 235, 235)", "line-dasharray": ["step", ["zoom"], ["literal", [0.4, 0.8]], 15, ["literal", [0.3, 0.6]], 16, ["literal", [0.2, 0.3]], 17, ["literal", [0.2, 0.25]], 18, ["literal", [0.15, 0.15]]]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-major-link", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-color": "rgb(235, 235, 235)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["step", ["zoom"], ["match", ["get", "class"], ["street", "street_limited", "primary_link", "track"], true, false], 1, ["match", ["get", "class"], ["street", "street_limited", "track", "primary_link", "secondary_link", "tertiary_link", "service"], true, false]], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-street-minor", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 2, "track", 1, 0.5], 18, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 18, 12]], "line-color": "rgb(235, 235, 235)", "line-opacity": ["step", ["zoom"], 0, 14, 1]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["match", ["get", "class"], ["primary", "secondary", "tertiary"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-primary-secondary-tertiary", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, ["match", ["get", "class"], "primary", 0.75, 0.1], 18, ["match", ["get", "class"], "primary", 32, 26]], "line-color": "rgb(235, 235, 235)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, tunnels"}, "filter": ["all", ["==", ["get", "structure"], "tunnel"], ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "tunnel-motorway-trunk", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-color": "rgb(235, 235, 235)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"icon-image": "turning-circle-outline", "icon-size": ["interpolate", ["exponential", 1.5], ["zoom"], 14, 0.122, 18, 0.969, 20, 1], "icon-allow-overlap": true, "icon-ignore-placement": true, "icon-padding": 0, "icon-rotation-alignment": "map", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["==", ["geometry-type"], "Point"], ["match", ["get", "class"], ["turning_circle", "turning_loop"], true, false]], "type": "symbol", "source": "composite", "id": "turning-feature-outline", "paint": {}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["step", ["zoom"], ["==", ["get", "class"], "track"], 1, ["match", ["get", "class"], ["track", "secondary_link", "tertiary_link", "service"], true, false]], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-minor-low", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, ["match", ["get", "class"], "track", 1, 0.5], 18, 12], "line-color": "rgb(255, 255, 255)", "line-opacity": ["step", ["zoom"], 1, 14, 0]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["step", ["zoom"], ["==", ["get", "class"], "track"], 1, ["match", ["get", "class"], ["track", "secondary_link", "tertiary_link", "service"], true, false]], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-minor-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, ["match", ["get", "class"], "track", 1, 0.5], 18, 12], "line-opacity": ["step", ["zoom"], 0, 14, 1]}, "source-layer": "road"}, {"minzoom": 11, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["street", "street_limited", "primary_link"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-street-low", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-color": "rgb(255, 255, 255)", "line-opacity": ["step", ["zoom"], 1, 14, 0]}, "source-layer": "road"}, {"minzoom": 11, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["street", "street_limited", "primary_link"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-street-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-opacity": ["step", ["zoom"], 0, 14, 1]}, "source-layer": "road"}, {"minzoom": 8, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["secondary", "tertiary"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-secondary-tertiary-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, 0.75, 18, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.1, 18, 26], "line-opacity": ["step", ["zoom"], 0, 10, 1]}, "source-layer": "road"}, {"minzoom": 7, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["==", ["get", "class"], "primary"], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-primary-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, 1, 18, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-opacity": ["step", ["zoom"], 0, 10, 1]}, "source-layer": "road"}, {"minzoom": 10, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-major-link-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-opacity": ["step", ["zoom"], 0, 11, 1]}, "source-layer": "road"}, {"minzoom": 5, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-motorway-trunk-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, 1, 18, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-opacity": ["step", ["zoom"], ["match", ["get", "class"], "motorway", 1, 0], 6, 1]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["==", ["get", "class"], "construction"], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-construction", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, 2, 18, 18], "line-color": "rgb(255, 255, 255)", "line-dasharray": ["step", ["zoom"], ["literal", [0.4, 0.8]], 15, ["literal", [0.3, 0.6]], 16, ["literal", [0.2, 0.3]], 17, ["literal", [0.2, 0.25]], 18, ["literal", [0.15, 0.15]]]}, "source-layer": "road"}, {"minzoom": 10, "layout": {"line-cap": ["step", ["zoom"], "butt", 13, "round"], "line-join": ["step", ["zoom"], "miter", 13, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-major-link", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["step", ["zoom"], ["==", ["get", "class"], "track"], 1, ["match", ["get", "class"], ["track", "secondary_link", "tertiary_link", "service"], true, false]], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-minor", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, ["match", ["get", "class"], "track", 1, 0.5], 18, 12], "line-color": "rgb(255, 255, 255)", "line-opacity": ["step", ["zoom"], 0, 14, 1]}, "source-layer": "road"}, {"minzoom": 11, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["street", "street_limited", "primary_link"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-street", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-color": "rgb(255, 255, 255)", "line-opacity": ["step", ["zoom"], 0, 14, 1]}, "source-layer": "road"}, {"minzoom": 8, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["match", ["get", "class"], ["secondary", "tertiary"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-secondary-tertiary", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.1, 18, 26], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"minzoom": 6, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}, "filter": ["all", ["==", ["get", "class"], "primary"], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "road-primary", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"id": "road-motorway-trunk", "type": "line", "source": "composite", "source-layer": "road", "filter": ["all", ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false], ["==", ["geometry-type"], "LineString"]], "layout": {"line-cap": ["step", ["zoom"], "butt", 13, "round"], "line-join": ["step", ["zoom"], "miter", 13, "round"]}, "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-color": "rgb(255, 255, 255)"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface"}}, {"minzoom": 13, "layout": {"line-join": "round", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "transit", "mapbox:group": "Transit, surface"}, "filter": ["all", ["match", ["get", "class"], ["major_rail", "minor_rail"], true, false], ["match", ["get", "structure"], ["none", "ford"], true, false]], "type": "line", "source": "composite", "id": "road-rail", "paint": {"line-color": ["interpolate", ["linear"], ["zoom"], 13, "rgb(242, 242, 242)", 17, "rgb(227, 227, 227)"], "line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, 0.5, 20, 1]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"icon-image": "turning-circle", "icon-size": ["interpolate", ["exponential", 1.5], ["zoom"], 14, 0.095, 18, 1], "icon-allow-overlap": true, "icon-ignore-placement": true, "icon-padding": 0, "icon-rotation-alignment": "map", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, surface-icons"}, "filter": ["all", ["==", ["geometry-type"], "Point"], ["match", ["get", "class"], ["turning_circle", "turning_loop"], true, false]], "type": "symbol", "source": "composite", "id": "turning-feature", "paint": {}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["step", ["zoom"], ["match", ["get", "class"], ["street", "street_limited", "primary_link", "track"], true, false], 1, ["match", ["get", "class"], ["street", "street_limited", "track", "primary_link", "secondary_link", "tertiary_link", "service"], true, false]], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-street-minor-low", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 2, "track", 1, 0.5], 18, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 18, 12]], "line-color": "rgb(255, 255, 255)", "line-opacity": ["step", ["zoom"], 1, 14, 0]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["step", ["zoom"], ["match", ["get", "class"], ["street", "street_limited", "primary_link", "track"], true, false], 1, ["match", ["get", "class"], ["street", "street_limited", "track", "primary_link", "secondary_link", "tertiary_link", "service"], true, false]], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-street-minor-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 2, "track", 1, 0.5], 18, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 18, 12]], "line-opacity": ["step", ["zoom"], 0, 14, 1]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["match", ["get", "class"], ["primary", "secondary", "tertiary"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-primary-secondary-tertiary-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, ["match", ["get", "class"], "primary", 1, 0.75], 18, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, ["match", ["get", "class"], "primary", 0.75, 0.1], 18, ["match", ["get", "class"], "primary", 32, 26]], "line-opacity": ["step", ["zoom"], 0, 10, 1]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["<=", ["get", "layer"], 1], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-major-link-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["<=", ["get", "layer"], 1], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-motorway-trunk-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, 1, 18, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32]}, "source-layer": "road"}, {"minzoom": 13, "layout": {}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["==", ["get", "class"], "construction"], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-construction", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, 2, 18, 18], "line-color": "rgb(255, 255, 255)", "line-dasharray": ["step", ["zoom"], ["literal", [0.4, 0.8]], 15, ["literal", [0.3, 0.6]], 16, ["literal", [0.2, 0.3]], 17, ["literal", [0.2, 0.25]], 18, ["literal", [0.15, 0.15]]]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": "round", "line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["<=", ["get", "layer"], 1], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-major-link", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["step", ["zoom"], ["match", ["get", "class"], ["street", "street_limited", "primary_link", "track"], true, false], 1, ["match", ["get", "class"], ["street", "street_limited", "track", "primary_link", "secondary_link", "tertiary_link", "service"], true, false]], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-street-minor", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 2, "track", 1, 0.5], 18, ["match", ["get", "class"], ["street", "street_limited", "primary_link"], 18, 12]], "line-color": "rgb(255, 255, 255)", "line-opacity": ["step", ["zoom"], 0, 14, 1]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["match", ["get", "class"], ["primary", "secondary", "tertiary"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-primary-secondary-tertiary", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, ["match", ["get", "class"], "primary", 0.75, 0.1], 18, ["match", ["get", "class"], "primary", 32, 26]], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": "round", "line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["<=", ["get", "layer"], 1], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-motorway-trunk", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], [">=", ["get", "layer"], 2], ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-major-link-2-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.75, 20, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], [">=", ["get", "layer"], 2], ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-motorway-trunk-2-case", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 10, 1, 18, 2], "line-color": "rgb(242, 242, 242)", "line-gap-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32]}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": "round", "line-join": "round"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], [">=", ["get", "layer"], 2], ["match", ["get", "class"], ["motorway_link", "trunk_link"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-major-link-2", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 12, 0.5, 14, 2, 18, 18], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-cap": ["step", ["zoom"], "butt", 14, "round"], "line-join": ["step", ["zoom"], "miter", 14, "round"]}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], [">=", ["get", "layer"], 2], ["match", ["get", "class"], ["motorway", "trunk"], true, false], ["==", ["geometry-type"], "LineString"]], "type": "line", "source": "composite", "id": "bridge-motorway-trunk-2", "paint": {"line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 5, 0.75, 18, 32], "line-color": "rgb(255, 255, 255)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"line-join": "round", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "transit", "mapbox:group": "Transit, bridges"}, "filter": ["all", ["==", ["get", "structure"], "bridge"], ["match", ["get", "class"], ["major_rail", "minor_rail"], true, false]], "type": "line", "source": "composite", "id": "bridge-rail", "paint": {"line-color": "rgb(227, 227, 227)", "line-width": ["interpolate", ["exponential", 1.5], ["zoom"], 14, 0.5, 20, 1]}, "source-layer": "road"}, {"minzoom": 7, "layout": {"line-join": "bevel", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "admin-boundaries", "mapbox:group": "Administrative boundaries, admin"}, "filter": ["all", ["==", ["get", "admin_level"], 1], ["==", ["get", "maritime"], "false"], ["match", ["get", "worldview"], ["all", "US"], true, false]], "type": "line", "source": "composite", "id": "admin-1-boundary-bg", "paint": {"line-color": ["interpolate", ["linear"], ["zoom"], 8, "rgb(227, 227, 227)", 16, "rgb(227, 227, 227)"], "line-width": ["interpolate", ["linear"], ["zoom"], 7, 3.75, 12, 5.5], "line-opacity": ["interpolate", ["linear"], ["zoom"], 7, 0, 8, 0.75], "line-dasharray": [1, 0], "line-blur": ["interpolate", ["linear"], ["zoom"], 3, 0, 8, 3]}, "source-layer": "admin"}, {"minzoom": 1, "layout": {"visibility": "none"}, "metadata": {"mapbox:featureComponent": "admin-boundaries", "mapbox:group": "Administrative boundaries, admin"}, "filter": ["all", ["==", ["get", "admin_level"], 0], ["==", ["get", "maritime"], "false"], ["match", ["get", "worldview"], ["all", "US"], true, false]], "type": "line", "source": "composite", "id": "admin-0-boundary-bg", "paint": {"line-width": ["interpolate", ["linear"], ["zoom"], 3, 3.5, 10, 8], "line-color": "rgb(227, 227, 227)", "line-opacity": ["interpolate", ["linear"], ["zoom"], 3, 0, 4, 0.5], "line-blur": ["interpolate", ["linear"], ["zoom"], 3, 0, 10, 2]}, "source-layer": "admin"}, {"minzoom": 2, "layout": {"line-join": "round", "line-cap": "round", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "admin-boundaries", "mapbox:group": "Administrative boundaries, admin"}, "filter": ["all", ["==", ["get", "admin_level"], 1], ["==", ["get", "maritime"], "false"], ["match", ["get", "worldview"], ["all", "US"], true, false]], "type": "line", "source": "composite", "id": "admin-1-boundary", "paint": {"line-dasharray": ["step", ["zoom"], ["literal", [2, 0]], 7, ["literal", [2, 2, 6, 2]]], "line-width": ["interpolate", ["linear"], ["zoom"], 7, 0.75, 12, 1.5], "line-opacity": ["interpolate", ["linear"], ["zoom"], 2, 0, 3, 1], "line-color": ["interpolate", ["linear"], ["zoom"], 3, "rgb(224, 224, 224)", 7, "rgb(184, 184, 184)"]}, "source-layer": "admin"}, {"minzoom": 1, "layout": {"line-join": "round", "line-cap": "round", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "admin-boundaries", "mapbox:group": "Administrative boundaries, admin"}, "filter": ["all", ["==", ["get", "admin_level"], 0], ["==", ["get", "disputed"], "false"], ["==", ["get", "maritime"], "false"], ["match", ["get", "worldview"], ["all", "US"], true, false]], "type": "line", "source": "composite", "id": "admin-0-boundary", "paint": {"line-color": "rgb(184, 184, 184)", "line-width": ["interpolate", ["linear"], ["zoom"], 3, 0.5, 10, 2], "line-dasharray": [10, 0]}, "source-layer": "admin"}, {"minzoom": 1, "layout": {"line-join": "round", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "admin-boundaries", "mapbox:group": "Administrative boundaries, admin"}, "filter": ["all", ["==", ["get", "disputed"], "true"], ["==", ["get", "admin_level"], 0], ["==", ["get", "maritime"], "false"], ["match", ["get", "worldview"], ["all", "US"], true, false]], "type": "line", "source": "composite", "id": "admin-0-boundary-disputed", "paint": {"line-color": "rgb(184, 184, 184)", "line-width": ["interpolate", ["linear"], ["zoom"], 3, 0.5, 10, 2], "line-dasharray": ["step", ["zoom"], ["literal", [3.25, 3.25]], 6, ["literal", [2.5, 2.5]], 7, ["literal", [2, 2.25]], 8, ["literal", [1.75, 2]]]}, "source-layer": "admin"}, {"minzoom": 10, "layout": {"text-size": ["interpolate", ["linear"], ["zoom"], 10, ["match", ["get", "class"], ["motorway", "trunk", "primary", "secondary", "tertiary"], 10, ["motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "street", "street_limited"], 9, 6.5], 18, ["match", ["get", "class"], ["motorway", "trunk", "primary", "secondary", "tertiary"], 16, ["motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "street", "street_limited"], 14, 13]], "text-max-angle": 30, "text-font": ["DIN Pro Regular", "Arial Unicode MS Regular"], "symbol-placement": "line", "text-padding": 1, "visibility": "none", "text-rotation-alignment": "map", "text-pitch-alignment": "viewport", "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "text-letter-spacing": 0.01}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, road-labels"}, "filter": ["step", ["zoom"], ["match", ["get", "class"], ["motorway", "trunk", "primary", "secondary", "tertiary"], true, false], 1, ["match", ["get", "class"], ["motorway", "trunk", "primary", "secondary", "tertiary", "street", "street_limited"], true, false], 2, ["match", ["get", "class"], ["path", "pedestrian", "golf", "ferry", "aerialway"], false, true]], "type": "symbol", "source": "composite", "id": "road-label", "paint": {"text-color": "rgb(128, 128, 128)", "text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 1, "text-halo-blur": 1}, "source-layer": "road"}, {"minzoom": 13, "layout": {"text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "icon-image": "intersection", "icon-text-fit": "both", "icon-text-fit-padding": [1, 2, 1, 2], "text-size": ["interpolate", ["exponential", 1.2], ["zoom"], 15, 9, 18, 12], "text-font": ["DIN Pro Bold", "Arial Unicode MS Bold"], "visibility": "none"}, "metadata": {"mapbox:featureComponent": "road-network", "mapbox:group": "Road network, road-labels"}, "filter": ["all", ["==", ["get", "class"], "intersection"], ["has", "name"]], "type": "symbol", "source": "composite", "id": "road-intersection", "paint": {"text-color": "rgb(153, 153, 153)"}, "source-layer": "road"}, {"minzoom": 13, "layout": {"text-font": ["DIN Pro Italic", "Arial Unicode MS Regular"], "text-max-angle": 30, "symbol-spacing": ["interpolate", ["linear", 1], ["zoom"], 15, 250, 17, 400], "text-size": ["interpolate", ["linear"], ["zoom"], 13, 12, 18, 16], "symbol-placement": "line", "text-pitch-alignment": "viewport", "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "visibility": "none"}, "metadata": {"mapbox:featureComponent": "natural-features", "mapbox:group": "Natural features, natural-labels"}, "filter": ["all", ["match", ["get", "class"], ["canal", "river", "stream"], ["match", ["get", "worldview"], ["all", "US"], true, false], ["disputed_canal", "disputed_river", "disputed_stream"], ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["==", ["geometry-type"], "LineString"]], "type": "symbol", "source": "composite", "id": "waterway-label", "paint": {"text-color": "rgb(150, 150, 150)"}, "source-layer": "natural_label"}, {"minzoom": 4, "layout": {"text-size": ["step", ["zoom"], ["step", ["get", "sizerank"], 18, 5, 12], 17, ["step", ["get", "sizerank"], 18, 13, 12]], "text-max-angle": 30, "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "text-font": ["DIN Pro Medium", "Arial Unicode MS Regular"], "symbol-placement": "line-center", "text-pitch-alignment": "viewport", "visibility": "none"}, "metadata": {"mapbox:featureComponent": "natural-features", "mapbox:group": "Natural features, natural-labels"}, "filter": ["all", ["match", ["get", "class"], ["glacier", "landform"], ["match", ["get", "worldview"], ["all", "US"], true, false], ["disputed_glacier", "disputed_landform"], ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["==", ["geometry-type"], "LineString"], ["<=", ["get", "filterrank"], 1]], "type": "symbol", "source": "composite", "id": "natural-line-label", "paint": {"text-halo-width": 0.5, "text-halo-color": "rgb(255, 255, 255)", "text-halo-blur": 0.5, "text-color": "rgb(128, 128, 128)"}, "source-layer": "natural_label"}, {"minzoom": 4, "layout": {"text-size": ["step", ["zoom"], ["step", ["get", "sizerank"], 18, 5, 12], 17, ["step", ["get", "sizerank"], 18, 13, 12]], "icon-image": "", "text-font": ["DIN Pro Medium", "Arial Unicode MS Regular"], "text-offset": ["literal", [0, 0]], "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "visibility": "none"}, "metadata": {"mapbox:featureComponent": "natural-features", "mapbox:group": "Natural features, natural-labels"}, "filter": ["all", ["match", ["get", "class"], ["dock", "glacier", "landform", "water_feature", "wetland"], ["match", ["get", "worldview"], ["all", "US"], true, false], ["disputed_dock", "disputed_glacier", "disputed_landform", "disputed_water_feature", "disputed_wetland"], ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["==", ["geometry-type"], "Point"], ["<=", ["get", "filterrank"], 1]], "type": "symbol", "source": "composite", "id": "natural-point-label", "paint": {"icon-opacity": ["step", ["zoom"], ["step", ["get", "sizerank"], 0, 5, 1], 17, ["step", ["get", "sizerank"], 0, 13, 1]], "text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 0.5, "text-halo-blur": 0.5, "text-color": "rgb(128, 128, 128)"}, "source-layer": "natural_label"}, {"id": "water-line-label", "type": "symbol", "metadata": {"mapbox:featureComponent": "natural-features", "mapbox:group": "Natural features, natural-labels"}, "source": "composite", "source-layer": "natural_label", "filter": ["all", ["match", ["get", "class"], ["bay", "ocean", "reservoir", "sea", "water"], ["match", ["get", "worldview"], ["all", "US"], true, false], ["disputed_bay", "disputed_ocean", "disputed_reservoir", "disputed_sea", "disputed_water"], ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["==", ["geometry-type"], "LineString"]], "layout": {"text-size": ["interpolate", ["linear"], ["zoom"], 7, ["step", ["get", "sizerank"], 20, 6, 18, 12, 12], 10, ["step", ["get", "sizerank"], 15, 9, 12], 18, ["step", ["get", "sizerank"], 15, 9, 14]], "text-max-angle": 30, "text-letter-spacing": ["match", ["get", "class"], "ocean", 0.25, ["sea", "bay"], 0.15, 0], "text-font": ["DIN Pro Italic", "Arial Unicode MS Regular"], "symbol-placement": "line-center", "text-pitch-alignment": "viewport", "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "visibility": "none"}, "paint": {"text-color": "rgb(150, 150, 150)"}}, {"id": "water-point-label", "type": "symbol", "source": "composite", "source-layer": "natural_label", "filter": ["all", ["match", ["get", "class"], ["bay", "ocean", "reservoir", "sea", "water"], ["match", ["get", "worldview"], ["all", "US"], true, false], ["disputed_bay", "disputed_ocean", "disputed_reservoir", "disputed_sea", "disputed_water"], ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["==", ["geometry-type"], "Point"]], "layout": {"text-line-height": 1.3, "text-size": ["interpolate", ["linear"], ["zoom"], 7, ["step", ["get", "sizerank"], 20, 6, 15, 12, 12], 10, ["step", ["get", "sizerank"], 15, 9, 12]], "text-font": ["DIN Pro Italic", "Arial Unicode MS Regular"], "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "text-letter-spacing": ["match", ["get", "class"], "ocean", 0.25, ["bay", "sea"], 0.15, 0.01], "text-max-width": ["match", ["get", "class"], "ocean", 4, "sea", 5, ["bay", "water"], 7, 10], "visibility": "none"}, "paint": {"text-color": "rgb(150, 150, 150)"}, "metadata": {"mapbox:featureComponent": "natural-features", "mapbox:group": "Natural features, natural-labels"}}, {"minzoom": 6, "layout": {"text-size": ["step", ["zoom"], ["step", ["get", "sizerank"], 18, 5, 12], 17, ["step", ["get", "sizerank"], 18, 13, 12]], "icon-image": "", "text-font": ["DIN Pro Medium", "Arial Unicode MS Regular"], "text-offset": [0, 0], "text-anchor": ["step", ["zoom"], ["step", ["get", "sizerank"], "center", 5, "top"], 17, ["step", ["get", "sizerank"], "center", 13, "top"]], "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "visibility": "none"}, "metadata": {"mapbox:featureComponent": "point-of-interest-labels", "mapbox:group": "Point of interest labels, poi-labels"}, "filter": ["<=", ["get", "filterrank"], ["+", ["step", ["zoom"], 1, 2, 3, 4, 5], 1]], "type": "symbol", "source": "composite", "id": "poi-label", "paint": {"text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 0.5, "text-halo-blur": 0.5, "text-color": ["step", ["zoom"], ["step", ["get", "sizerank"], "rgb(184, 184, 184)", 5, "rgb(161, 161, 161)"], 17, ["step", ["get", "sizerank"], "rgb(184, 184, 184)", 13, "rgb(161, 161, 161)"]]}, "source-layer": "poi_label"}, {"minzoom": 8, "layout": {"text-line-height": 1.1, "text-size": ["step", ["get", "sizerank"], 18, 9, 12], "icon-image": ["get", "maki"], "text-font": ["DIN Pro Medium", "Arial Unicode MS Regular"], "visibility": "none", "text-offset": [0, 0.75], "text-rotation-alignment": "viewport", "text-anchor": "top", "text-field": ["step", ["get", "sizerank"], ["coalesce", ["get", "name_en"], ["get", "name"]], 15, ["get", "ref"]], "text-letter-spacing": 0.01, "text-max-width": 9}, "metadata": {"mapbox:featureComponent": "transit", "mapbox:group": "Transit, transit-labels"}, "filter": ["match", ["get", "class"], ["military", "civil"], ["match", ["get", "worldview"], ["all", "US"], true, false], ["disputed_military", "disputed_civil"], ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], "type": "symbol", "source": "composite", "id": "airport-label", "paint": {"text-color": "rgb(128, 128, 128)", "text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 1}, "source-layer": "airport_label"}, {"minzoom": 10, "layout": {"text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "text-transform": "uppercase", "text-font": ["DIN Pro Regular", "Arial Unicode MS Regular"], "text-letter-spacing": ["match", ["get", "type"], "suburb", 0.15, 0.1], "text-max-width": 7, "text-padding": 3, "text-size": ["interpolate", ["cubic-bezier", 0.5, 0, 1, 1], ["zoom"], 11, ["match", ["get", "type"], "suburb", 11, 10.5], 15, ["match", ["get", "type"], "suburb", 15, 14]], "visibility": "none"}, "metadata": {"mapbox:featureComponent": "place-labels", "mapbox:group": "Place labels, place-labels"}, "maxzoom": 15, "filter": ["all", ["match", ["get", "class"], "settlement_subdivision", ["match", ["get", "worldview"], ["all", "US"], true, false], "disputed_settlement_subdivision", ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["<=", ["get", "filterrank"], 4]], "type": "symbol", "source": "composite", "id": "settlement-subdivision-label", "paint": {"text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 1, "text-color": "rgb(179, 179, 179)", "text-halo-blur": 0.5}, "source-layer": "place_label"}, {"minzoom": 3, "layout": {"text-line-height": 1.1, "text-size": ["interpolate", ["cubic-bezier", 0.2, 0, 0.9, 1], ["zoom"], 3, ["step", ["get", "symbolrank"], 12, 9, 11, 10, 10.5, 12, 9.5, 14, 8.5, 16, 6.5, 17, 4], 13, ["step", ["get", "symbolrank"], 23, 9, 21, 10, 19, 11, 17, 12, 16, 13, 15, 15, 13]], "text-radial-offset": ["step", ["zoom"], ["match", ["get", "capital"], 2, 0.6, 0.55], 8, 0], "icon-image": ["step", ["zoom"], ["case", ["==", ["get", "capital"], 2], "border-dot-13", ["step", ["get", "symbolrank"], "dot-11", 9, "dot-10", 11, "dot-9"]], 8, ""], "text-font": ["DIN Pro Regular", "Arial Unicode MS Regular"], "text-justify": "auto", "visibility": "none", "text-anchor": ["step", ["zoom"], ["get", "text_anchor"], 8, "center"], "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "text-max-width": 7}, "metadata": {"mapbox:featureComponent": "place-labels", "mapbox:group": "Place labels, place-labels"}, "maxzoom": 13, "filter": ["all", ["<=", ["get", "filterrank"], 3], ["match", ["get", "class"], "settlement", ["match", ["get", "worldview"], ["all", "US"], true, false], "disputed_settlement", ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["step", ["zoom"], [">", ["get", "symbolrank"], 6], 1, [">=", ["get", "symbolrank"], 7], 2, [">=", ["get", "symbolrank"], 8], 3, [">=", ["get", "symbolrank"], 10], 4, [">=", ["get", "symbolrank"], 11], 5, [">=", ["get", "symbolrank"], 13], 6, [">=", ["get", "symbolrank"], 15]]], "type": "symbol", "source": "composite", "id": "settlement-minor-label", "paint": {"text-color": ["step", ["get", "symbolrank"], "rgb(128, 128, 128)", 11, "rgb(161, 161, 161)", 16, "rgb(184, 184, 184)"], "text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 1, "text-halo-blur": 1}, "source-layer": "place_label"}, {"minzoom": 3, "layout": {"text-line-height": 1.1, "text-size": ["interpolate", ["cubic-bezier", 0.2, 0, 0.9, 1], ["zoom"], 3, ["step", ["get", "symbolrank"], 13, 6, 12], 6, ["step", ["get", "symbolrank"], 16, 6, 15, 7, 14], 8, ["step", ["get", "symbolrank"], 18, 9, 17, 10, 15], 15, ["step", ["get", "symbolrank"], 23, 9, 22, 10, 20, 11, 18, 12, 16, 13, 15, 15, 13]], "text-radial-offset": ["step", ["zoom"], ["match", ["get", "capital"], 2, 0.6, 0.55], 8, 0], "icon-image": ["step", ["zoom"], ["case", ["==", ["get", "capital"], 2], "border-dot-13", ["step", ["get", "symbolrank"], "dot-11", 9, "dot-10", 11, "dot-9"]], 8, ""], "text-font": ["DIN Pro Medium", "Arial Unicode MS Regular"], "text-justify": ["step", ["zoom"], ["match", ["get", "text_anchor"], ["left", "bottom-left", "top-left"], "left", ["right", "bottom-right", "top-right"], "right", "center"], 8, "center"], "visibility": "none", "text-anchor": ["step", ["zoom"], ["get", "text_anchor"], 8, "center"], "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "text-max-width": 7}, "metadata": {"mapbox:featureComponent": "place-labels", "mapbox:group": "Place labels, place-labels"}, "maxzoom": 15, "filter": ["all", ["<=", ["get", "filterrank"], 3], ["match", ["get", "class"], "settlement", ["match", ["get", "worldview"], ["all", "US"], true, false], "disputed_settlement", ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], ["step", ["zoom"], false, 1, ["<=", ["get", "symbolrank"], 6], 2, ["<", ["get", "symbolrank"], 7], 3, ["<", ["get", "symbolrank"], 8], 4, ["<", ["get", "symbolrank"], 10], 5, ["<", ["get", "symbolrank"], 11], 6, ["<", ["get", "symbolrank"], 13], 7, ["<", ["get", "symbolrank"], 15], 8, [">=", ["get", "symbolrank"], 11], 9, [">=", ["get", "symbolrank"], 15]]], "type": "symbol", "source": "composite", "id": "settlement-major-label", "paint": {"text-color": ["step", ["get", "symbolrank"], "rgb(128, 128, 128)", 11, "rgb(161, 161, 161)", 16, "rgb(184, 184, 184)"], "text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 1, "text-halo-blur": 1}, "source-layer": "place_label"}, {"minzoom": 3, "layout": {"text-size": ["interpolate", ["cubic-bezier", 0.85, 0.7, 0.65, 1], ["zoom"], 4, ["step", ["get", "symbolrank"], 10, 6, 9.5, 7, 9], 9, ["step", ["get", "symbolrank"], 21, 6, 16, 7, 13]], "text-transform": "uppercase", "text-font": ["DIN Pro Bold", "Arial Unicode MS Bold"], "text-field": ["step", ["zoom"], ["step", ["get", "symbolrank"], ["coalesce", ["get", "name_en"], ["get", "name"]], 5, ["coalesce", ["get", "abbr"], ["get", "name_en"], ["get", "name"]]], 5, ["coalesce", ["get", "name_en"], ["get", "name"]]], "text-letter-spacing": 0.15, "text-max-width": 6, "visibility": "none"}, "metadata": {"mapbox:featureComponent": "place-labels", "mapbox:group": "Place labels, place-labels"}, "maxzoom": 9, "filter": ["match", ["get", "class"], "state", ["match", ["get", "worldview"], ["all", "US"], true, false], "disputed_state", ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], "type": "symbol", "source": "composite", "id": "state-label", "paint": {"text-color": "rgb(184, 184, 184)", "text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 1}, "source-layer": "place_label"}, {"minzoom": 1, "layout": {"text-line-height": 1.1, "text-size": ["interpolate", ["cubic-bezier", 0.2, 0, 0.7, 1], ["zoom"], 1, ["step", ["get", "symbolrank"], 11, 4, 9, 5, 8], 9, ["step", ["get", "symbolrank"], 22, 4, 19, 5, 17]], "text-radial-offset": ["step", ["zoom"], 0.6, 8, 0], "icon-image": "", "text-font": ["DIN Pro Medium", "Arial Unicode MS Regular"], "text-justify": ["step", ["zoom"], ["match", ["get", "text_anchor"], ["left", "bottom-left", "top-left"], "left", ["right", "bottom-right", "top-right"], "right", "center"], 7, "auto"], "visibility": "none", "text-field": ["coalesce", ["get", "name_en"], ["get", "name"]], "text-max-width": 6}, "metadata": {"mapbox:featureComponent": "place-labels", "mapbox:group": "Place labels, place-labels"}, "maxzoom": 10, "filter": ["match", ["get", "class"], "country", ["match", ["get", "worldview"], ["all", "US"], true, false], "disputed_country", ["all", ["==", ["get", "disputed"], "true"], ["match", ["get", "worldview"], ["all", "US"], true, false]], false], "type": "symbol", "source": "composite", "id": "country-label", "paint": {"icon-opacity": ["step", ["zoom"], ["case", ["has", "text_anchor"], 1, 0], 7, 0], "text-color": "rgb(128, 128, 128)", "text-halo-color": "rgb(255, 255, 255)", "text-halo-width": 1.25}, "source-layer": "place_label"}], "created": "2021-11-05T16:12:04.822Z", "modified": "2021-11-25T13:58:04.167Z", "id": "ckvmksrpd4n0a14pfdo5heqzr", "owner": "commaai", "visibility": "private", "protected": false, "draft": false} \ No newline at end of file diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py index 7dc002727..e64907149 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -13,7 +13,7 @@ from system.swaglog import cloudlog from system.hardware import HARDWARE from common.file_helpers import atomic_write_in_dir from system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty -from selfdrive.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S +from system.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S class METRIC_TYPE: diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 8cc996c28..281c90293 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -2,8 +2,9 @@ import os import time from functools import wraps -from system.hardware import PC +import cereal.messaging as messaging from selfdrive.manager.process_config import managed_processes +from system.hardware import PC from system.version import training_version, terms_version @@ -15,6 +16,11 @@ def set_params_enabled(): params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("Passive", False) + # valid calib + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = 20 + msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] + params.put("CalibrationParams", msg.to_bytes()) def phone_only(f): @wraps(f) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 4021e27de..22ddb6e9c 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -1,11 +1,14 @@ #!/usr/bin/env python3 +import math import json import os +import shutil import subprocess import time import numpy as np import unittest from collections import Counter, defaultdict +from functools import cached_property from pathlib import Path from cereal import car @@ -15,7 +18,7 @@ from common.basedir import BASEDIR from common.timeout import Timeout from common.params import Params from selfdrive.controls.lib.events import EVENTS, ET -from selfdrive.loggerd.config import ROOT +from system.loggerd.config import ROOT from selfdrive.test.helpers import set_params_enabled, release_only from tools.lib.logreader import LogReader @@ -25,8 +28,8 @@ PROCS = { "./loggerd": 10.0, "./encoderd": 17.0, "./camerad": 14.5, - "./locationd": 9.1, - "selfdrive.controls.plannerd": 11.7, + "./locationd": 11.0, + "selfdrive.controls.plannerd": 16.5, "./_ui": 19.2, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 12.0, @@ -45,6 +48,15 @@ PROCS = { "./ubloxd": 0.02, "selfdrive.tombstoned": 0, "./logcatd": 0, + "system.micd": 10.0, + "system.timezoned": 0, + "system.sensord.pigeond": 6.0, + "selfdrive.boardd.pandad": 0, + "selfdrive.statsd": 0.4, + "selfdrive.navd.navd": 0.4, + "system.loggerd.uploader": 4.0, + "system.loggerd.deleter": 0.1, + "selfdrive.locationd.laikad": None, # TODO: laikad cpu usage is sporadic } TIMINGS = { @@ -71,48 +83,6 @@ def cputime_total(ct): return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem -def check_cpu_usage(proclogs): - result = "\n" - result += "------------------------------------------------\n" - result += "------------------ CPU Usage -------------------\n" - result += "------------------------------------------------\n" - - plogs_by_proc = defaultdict(list) - for pl in proclogs: - for x in pl.procLog.procs: - if len(x.cmdline) > 0: - n = list(x.cmdline)[0] - plogs_by_proc[n].append(x) - - print(plogs_by_proc.keys()) - - r = True - dt = (proclogs[-1].logMonoTime - proclogs[0].logMonoTime) / 1e9 - for proc_name, expected_cpu in PROCS.items(): - err = "" - cpu_usage = 0. - x = plogs_by_proc[proc_name] - if len(x) > 2: - cpu_time = cputime_total(x[-1]) - cputime_total(x[0]) - cpu_usage = cpu_time / dt * 100. - if cpu_usage > max(expected_cpu * 1.15, expected_cpu + 5.0): - # cpu usage is high while playing sounds - if not (proc_name == "./_soundd" and cpu_usage < 65.): - err = "using more CPU than normal" - elif cpu_usage < min(expected_cpu * 0.65, max(expected_cpu - 1.0, 0.0)): - err = "using less CPU than normal" - else: - err = "NO METRICS FOUND" - - result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({expected_cpu:5.2f}%) {err}\n" - if len(err) > 0: - r = False - - result += "------------------------------------------------\n" - print(result) - return r - - class TestOnroad(unittest.TestCase): @classmethod @@ -120,19 +90,22 @@ class TestOnroad(unittest.TestCase): if "DEBUG" in os.environ: segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog")), Path(ROOT).iterdir()) segs = sorted(segs, key=lambda x: x.stat().st_mtime) - print(segs[-2]) - cls.lr = list(LogReader(os.path.join(segs[-2], "rlog"))) + print(segs[-3]) + cls.lr = list(LogReader(os.path.join(segs[-3], "rlog"))) return # setup env + os.environ['PASSIVE'] = "0" os.environ['REPLAY'] = "1" os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" - os.environ['LOGPRINT'] = 'debug' + os.environ['LOGPRINT'] = "debug" params = Params() params.clear_all() set_params_enabled() + if os.path.exists(ROOT): + shutil.rmtree(ROOT) # Make sure athena isn't running os.system("pkill -9 -f athena") @@ -177,6 +150,25 @@ class TestOnroad(unittest.TestCase): # use the second segment by default as it's the first full segment cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog"))) + @cached_property + def service_msgs(self): + msgs = defaultdict(list) + for m in self.lr: + msgs[m.which()].append(m) + return msgs + + def test_service_frequencies(self): + for s, msgs in self.service_msgs.items(): + if s in ('initData', 'sentinel'): + continue + + # skip gps services for now + if s in ('ubloxGnss', 'ubloxRaw', 'gnssMeasurements'): + continue + + with self.subTest(service=s): + assert len(msgs) >= math.floor(service_list[s].frequency*55) + def test_cloudlog_size(self): msgs = [m for m in self.lr if m.which() == 'logMessage'] @@ -193,7 +185,7 @@ class TestOnroad(unittest.TestCase): result += "-------------- UI Draw Timing ------------------\n" result += "------------------------------------------------\n" - ts = [m.uiDebug.drawTimeMillis for m in self.lr if m.which() == 'uiDebug'] + ts = [m.uiDebug.drawTimeMillis for m in self.service_msgs['uiDebug']] result += f"min {min(ts):.2f}ms\n" result += f"max {max(ts):.2f}ms\n" result += f"std {np.std(ts):.2f}ms\n" @@ -201,15 +193,60 @@ class TestOnroad(unittest.TestCase): result += "------------------------------------------------\n" print(result) - self.assertGreater(len(ts), 20*50, "insufficient samples") #self.assertLess(max(ts), 30.) self.assertLess(np.mean(ts), 10.) #self.assertLess(np.std(ts), 5.) def test_cpu_usage(self): - proclogs = [m for m in self.lr if m.which() == 'procLog'] - self.assertGreater(len(proclogs), service_list['procLog'].frequency * 45, "insufficient samples") - cpu_ok = check_cpu_usage(proclogs) + result = "\n" + result += "------------------------------------------------\n" + result += "------------------ CPU Usage -------------------\n" + result += "------------------------------------------------\n" + + plogs_by_proc = defaultdict(list) + for pl in self.service_msgs['procLog']: + for x in pl.procLog.procs: + if len(x.cmdline) > 0: + n = list(x.cmdline)[0] + plogs_by_proc[n].append(x) + print(plogs_by_proc.keys()) + + cpu_ok = True + dt = (self.service_msgs['procLog'][-1].logMonoTime - self.service_msgs['procLog'][0].logMonoTime) / 1e9 + for proc_name, expected_cpu in PROCS.items(): + + err = "" + cpu_usage = 0. + x = plogs_by_proc[proc_name] + if len(x) > 2: + cpu_time = cputime_total(x[-1]) - cputime_total(x[0]) + cpu_usage = cpu_time / dt * 100. + + if expected_cpu is None: + result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({expected_cpu}) SKIPPED\n" + continue + elif cpu_usage > max(expected_cpu * 1.15, expected_cpu + 5.0): + # cpu usage is high while playing sounds + if not (proc_name == "./_soundd" and cpu_usage < 65.): + err = "using more CPU than normal" + elif cpu_usage < min(expected_cpu * 0.65, max(expected_cpu - 1.0, 0.0)): + err = "using less CPU than normal" + else: + err = "NO METRICS FOUND" + + result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({expected_cpu:5.2f}%) {err}\n" + if len(err) > 0: + cpu_ok = False + + # Ensure there's no missing procs + all_procs = set([p.name for p in self.service_msgs['managerState'][0].managerState.processes if p.shouldBeRunning]) + for p in all_procs: + with self.subTest(proc=p): + assert any(p in pp for pp in PROCS.keys()), f"Expected CPU usage missing for {p}" + + result += "------------------------------------------------\n" + print(result) + self.assertTrue(cpu_ok) def test_camera_processing_time(self): @@ -234,7 +271,7 @@ class TestOnroad(unittest.TestCase): cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)] for (s, instant_max, avg_max) in cfgs: - ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s] + ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.service_msgs[s]] self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" @@ -254,7 +291,7 @@ class TestOnroad(unittest.TestCase): ("driverStateV2", 0.050, 0.026), ] for (s, instant_max, avg_max) in cfgs: - ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s] + ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.service_msgs[s]] self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" @@ -270,7 +307,7 @@ class TestOnroad(unittest.TestCase): result += "----------------- Service Timings --------------\n" result += "------------------------------------------------\n" for s, (maxmin, rsd) in TIMINGS.items(): - msgs = [m.logMonoTime for m in self.lr if m.which() == s] + msgs = [m.logMonoTime for m in self.service_msgs[s]] if not len(msgs): raise Exception(f"missing {s}") @@ -305,6 +342,17 @@ class TestOnroad(unittest.TestCase): expected = EVENTS[car.CarEvent.EventName.startup][ET.PERMANENT].alert_text_1 self.assertEqual(startup_alert, expected, "wrong startup alert") + def test_engagable(self): + no_entries = Counter() + for m in self.service_msgs['carEvents']: + for evt in m.carEvents: + if evt.noEntry: + no_entries[evt.name] += 1 + + eng = [m.controlsState.engageable for m in self.service_msgs['controlsState']] + assert all(eng), \ + f"Not engageable for whole segment:\n- controlsState.engageable: {Counter(eng)}\n- No entry events: {no_entries}" + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index 2094faeaa..f3e822da5 100644 --- a/selfdrive/thermald/fan_controller.py +++ b/selfdrive/thermald/fan_controller.py @@ -8,7 +8,7 @@ from selfdrive.controls.lib.pid import PIDController class BaseFanController(ABC): @abstractmethod - def update(self, max_cpu_temp: float, ignition: bool) -> int: + def update(self, cur_temp: float, ignition: bool) -> int: pass @@ -18,19 +18,19 @@ class TiciFanController(BaseFanController): cloudlog.info("Setting up TICI fan handler") self.last_ignition = False - self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) + self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_TRML)) - def update(self, max_cpu_temp: float, ignition: bool) -> int: - self.controller.neg_limit = -(80 if ignition else 30) + def update(self, cur_temp: float, ignition: bool) -> int: + self.controller.neg_limit = -(100 if ignition else 30) self.controller.pos_limit = -(30 if ignition else 0) if ignition != self.last_ignition: self.controller.reset() - error = 70 - max_cpu_temp + error = 70 - cur_temp fan_pwr_out = -int(self.controller.update( error=error, - feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80]) + feedforward=interp(cur_temp, [60.0, 100.0], [0, -100]) )) self.last_ignition = ignition diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 85e9510eb..06e2b5e8f 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -17,6 +17,7 @@ VBATT_PAUSE_CHARGING = 11.8 # Lower limit on the LPF car battery volta VBATT_INSTANT_PAUSE_CHARGING = 7.0 # Lower limit on the instant car battery voltage measurements to avoid triggering on instant power loss MAX_TIME_OFFROAD_S = 30*3600 MIN_ON_TIME_S = 3600 +DELAY_SHUTDOWN_TIME_S = 300 # Wait at least DELAY_SHUTDOWN_TIME_S seconds after offroad_time to shutdown. VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 60 class PowerMonitoring: @@ -124,6 +125,7 @@ class PowerMonitoring: should_shutdown &= not ignition should_shutdown &= (not self.params.get_bool("DisablePowerDown")) should_shutdown &= in_car + should_shutdown &= offroad_time > DELAY_SHUTDOWN_TIME_S should_shutdown |= self.params.get_bool("ForcePowerDown") should_shutdown &= started_seen or (now > MIN_ON_TIME_S) return should_shutdown diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index eedeff31f..57387e518 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -18,7 +18,7 @@ from common.params import Params from common.realtime import DT_TRML, sec_since_boot from selfdrive.controls.lib.alertmanager import set_offroad_alert from system.hardware import HARDWARE, TICI, AGNOS -from selfdrive.loggerd.config import get_available_percent +from system.loggerd.config import get_available_percent from selfdrive.statsd import statlog from system.swaglog import cloudlog from selfdrive.thermald.power_monitoring import PowerMonitoring @@ -313,9 +313,6 @@ def thermald_thread(end_event, hw_queue): should_start = should_start and all(startup_conditions.values()) if should_start != should_start_prev or (count == 0): - params.put_bool("IsOnroad", should_start) - params.put_bool("IsOffroad", not should_start) - params.put_bool("IsEngaged", False) engaged_prev = False HARDWARE.set_power_save(not should_start) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 61a575f14..65fb45b67 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -10,7 +10,7 @@ import glob from typing import NoReturn from common.file_helpers import mkdirs_exists_ok -from selfdrive.loggerd.config import ROOT +from system.loggerd.config import ROOT import selfdrive.sentry as sentry from system.swaglog import cloudlog from system.version import get_commit diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 0f28f5ccc..a8c8463bd 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -42,7 +42,6 @@ assets_src = "#selfdrive/assets/assets.qrc" qt_env.Command(assets, assets_src, f"rcc $SOURCES -o $TARGET") qt_env.Depends(assets, Glob('#selfdrive/assets/*', exclude=[assets, assets_src, "#selfdrive/assets/assets.o"])) asset_obj = qt_env.Object("assets", assets) -Export('asset_obj') # build soundd qt_env.Program("soundd/_soundd", ["soundd/main.cc", "soundd/sound.cc"], LIBS=qt_libs) diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index dd30b5fef..abbf434d6 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -24,8 +24,6 @@ const float MAX_PITCH = 50; const float MIN_PITCH = 0; const float MAP_SCALE = 2; -const float VALID_POS_STD = 50.0; // m - const QString ICON_SUFFIX = ".png"; MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) { @@ -125,42 +123,6 @@ void MapWindow::updateState(const UIState &s) { } } - // TODO should check a valid/status flag - if (sm.updated("gnssMeasurements") && sm["gnssMeasurements"].getGnssMeasurements().getGpsWeek() > 0){ - auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements(); - auto laikad_pos = laikad_location.getPositionECEF(); - auto laikad_pos_ecef = laikad_pos.getValue(); - auto laikad_pos_std = laikad_pos.getStd(); - auto laikad_velocity_ecef = laikad_location.getVelocityECEF().getValue(); - - laikad_valid = laikad_pos.getValid() && Eigen::Vector3d(laikad_pos_std[0], laikad_pos_std[1], laikad_pos_std[2]).norm() < VALID_POS_STD; - - if (laikad_valid && !locationd_valid) { - ECEF ecef = {.x = laikad_pos_ecef[0], .y = laikad_pos_ecef[1], .z = laikad_pos_ecef[2]}; - Geodetic laikad_pos_geodetic = ecef2geodetic(ecef); - last_position = QMapbox::Coordinate(laikad_pos_geodetic.lat, laikad_pos_geodetic.lon); - - // Compute NED velocity - LocalCoord converter(ecef); - ECEF next_ecef = {.x = ecef.x + laikad_velocity_ecef[0], .y = ecef.y + laikad_velocity_ecef[1], .z = ecef.z + laikad_velocity_ecef[2]}; - Eigen::VectorXd ned_vel = converter.ecef2ned(next_ecef).to_vector() - converter.ecef2ned(ecef).to_vector(); - - float velocity = ned_vel.norm(); - velocity_filter.update(velocity); - - // Convert NED velocity to angle - if (velocity > 1.0) { - float new_bearing = fmod(RAD2DEG(atan2(ned_vel[1], ned_vel[0])) + 360.0, 360.0); - if (last_bearing) { - float delta = 0.1 * angle_difference(*last_bearing, new_bearing); // Smooth heading - last_bearing = fmod(*last_bearing + delta + 360.0, 360.0); - } else { - last_bearing = new_bearing; - } - } - } - } - if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) { qWarning() << "Got new navRoute from navd. Opening map:" << allow_open; diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index 8d5d4e171..95db4f2bb 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -31,7 +31,7 @@ QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) { QMapbox::CoordinatesCollections model_to_collection( const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF, const cereal::LiveLocationKalman::Measurement::Reader &positionECEF, - const cereal::ModelDataV2::XYZTData::Reader &line){ + const cereal::XYZTData::Reader &line){ Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]); Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]); diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index 6bd5b0f06..f9c56107e 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -20,7 +20,7 @@ QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in); QMapbox::CoordinatesCollections model_to_collection( const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF, const cereal::LiveLocationKalman::Measurement::Reader &positionECEF, - const cereal::ModelDataV2::XYZTData::Reader &line); + const cereal::XYZTData::Reader &line); QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c); QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader &coordinate_list); QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList &coordinate_list); diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 2f366a9b3..327c7537a 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -27,7 +27,7 @@ void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) { } DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) { - face_img = loadPixmap("../assets/img_driver_face.png", {FACE_IMG_SIZE, FACE_IMG_SIZE}); + face_img = loadPixmap("../assets/img_driver_face_static.png", {FACE_IMG_SIZE, FACE_IMG_SIZE}); } void DriverViewScene::showEvent(QShowEvent* event) { diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc index f3e50b572..64cb35799 100644 --- a/selfdrive/ui/qt/offroad/onboarding.cc +++ b/selfdrive/ui/qt/offroad/onboarding.cc @@ -40,9 +40,6 @@ void TrainingGuide::mouseReleaseEvent(QMouseEvent *e) { } void TrainingGuide::showEvent(QShowEvent *event) { - img_path = width() == WIDE_WIDTH ? "../assets/training_wide/" : "../assets/training/"; - boundingRect = width() == WIDE_WIDTH ? boundingRectWide : boundingRectStandard; - currentIndex = 0; image.load(img_path + "step0.png"); click_timer.start(); diff --git a/selfdrive/ui/qt/offroad/onboarding.h b/selfdrive/ui/qt/offroad/onboarding.h index 48f409489..d347d1e61 100644 --- a/selfdrive/ui/qt/offroad/onboarding.h +++ b/selfdrive/ui/qt/offroad/onboarding.h @@ -25,56 +25,31 @@ private: int currentIndex = 0; // Bounding boxes for each training guide step - const QRect continueBtnStandard = {1620, 0, 300, 1080}; - QVector boundingRectStandard { - QRect(112, 804, 619, 166), - continueBtnStandard, - continueBtnStandard, - QRect(1476, 565, 253, 308), - QRect(1501, 529, 184, 108), - continueBtnStandard, - QRect(1613, 665, 178, 153), - QRect(1220, 0, 420, 730), - QRect(1335, 499, 440, 147), - QRect(112, 820, 996, 148), - QRect(1412, 199, 316, 333), - continueBtnStandard, - QRect(1237, 63, 683, 1017), - continueBtnStandard, - QRect(1455, 110, 313, 860), - QRect(1253, 519, 383, 228), - continueBtnStandard, - continueBtnStandard, - QRect(630, 804, 626, 164), - QRect(108, 804, 426, 164), - }; - - const QRect continueBtnWide = {1840, 0, 320, 1080}; - QVector boundingRectWide { + const QRect continueBtn = {1840, 0, 320, 1080}; + QVector boundingRect { QRect(112, 804, 618, 164), - continueBtnWide, - continueBtnWide, + continueBtn, + continueBtn, QRect(1641, 558, 210, 313), QRect(1662, 528, 184, 108), - continueBtnWide, + continueBtn, QRect(1814, 621, 211, 170), QRect(1350, 0, 497, 755), - QRect(1553, 516, 406, 112), + QRect(1540, 386, 468, 238), QRect(112, 804, 1126, 164), QRect(1598, 199, 316, 333), - continueBtnWide, + continueBtn, QRect(1364, 90, 796, 990), - continueBtnWide, + continueBtn, QRect(1593, 114, 318, 853), QRect(1379, 511, 391, 243), - continueBtnWide, - continueBtnWide, + continueBtn, + continueBtn, QRect(630, 804, 626, 164), QRect(108, 804, 426, 164), }; - QString img_path; - QVector boundingRect; + const QString img_path = "../assets/training/"; QElapsedTimer click_timer; signals: diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 63b87149d..b9fec9d62 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -43,10 +43,10 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { }, { "ExperimentalLongitudinalEnabled", - tr("Experimental openpilot Longitudinal Control"), - QString("%1
%2") - .arg(tr("WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB).")) - .arg(tr("On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control.")), + tr("openpilot Longitudinal Control (Alpha)"), + QString("%1

%2") + .arg(tr("WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).")) + .arg(tr("On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.")), "../assets/offroad/icon_speed_limit.png", }, { @@ -193,6 +193,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { connect(resetCalibBtn, &ButtonControl::clicked, [&]() { if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) { params.remove("CalibrationParams"); + params.remove("LiveTorqueParameters"); } }); addItem(resetCalibBtn); @@ -272,7 +273,7 @@ void DevicePanel::updateCalibDescription() { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(calib_bytes.data(), calib_bytes.size())); auto calib = cmsg.getRoot().getLiveCalibration(); - if (calib.getCalStatus() != 0) { + if (calib.getCalStatus() != cereal::LiveCalibrationData::Status::UNCALIBRATED) { double pitch = calib.getRpyCalib()[1] * (180 / M_PI); double yaw = calib.getRpyCalib()[2] * (180 / M_PI); desc += tr(" Your device is pointed %1° %2 and %3° %4.") @@ -342,7 +343,6 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QPushButton { font-size: 140px; padding-bottom: 20px; - font-weight: bold; border 1px grey solid; border-radius: 100px; background-color: #292929; @@ -378,22 +378,18 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QObject::connect(map_panel, &MapPanel::closeSettings, this, &SettingsWindow::closeSettings); #endif - const int padding = panels.size() > 3 ? 25 : 35; - nav_btns = new QButtonGroup(this); for (auto &[name, panel] : panels) { QPushButton *btn = new QPushButton(name); btn->setCheckable(true); btn->setChecked(nav_btns->buttons().size() == 0); - btn->setStyleSheet(QString(R"( + btn->setStyleSheet(R"( QPushButton { color: grey; border: none; background: none; font-size: 65px; font-weight: 500; - padding-top: %1px; - padding-bottom: %1px; } QPushButton:checked { color: white; @@ -401,8 +397,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QPushButton:pressed { color: #ADADAD; } - )").arg(padding)); - + )"); + btn->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); nav_btns->addButton(btn); sidebar_layout->addWidget(btn, 0, Qt::AlignRight); diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index fde864558..be9da34d4 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -415,7 +415,7 @@ void WifiManager::addTetheringConnection() { } void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { - int prime_type = uiState()->prime_type; + int prime_type = uiState()->primeType(); int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); if (!ipv4_forward) { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 33b1ea8e2..51c3f3dbb 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -89,7 +89,7 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { void OnroadWindow::offroadTransition(bool offroad) { #ifdef ENABLE_MAPS if (!offroad) { - if (map == nullptr && (uiState()->prime_type || !MAPBOX_TOKEN.isEmpty())) { + if (map == nullptr && (uiState()->primeType() || !MAPBOX_TOKEN.isEmpty())) { MapWindow * m = new MapWindow(get_mapbox_settings()); map = m; @@ -460,6 +460,7 @@ void AnnotatedCameraWidget::drawIcon(QPainter &p, int x, int y, QPixmap &img, QB p.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); p.setOpacity(opacity); p.drawPixmap(x - img.size().width() / 2, y - img.size().height() / 2, img); + p.setOpacity(1.0); } @@ -511,24 +512,34 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } // paint path - QLinearGradient bg(0, height(), 0, height() / 4); - float start_hue, end_hue; + QLinearGradient bg(0, height(), 0, 0); if (sm["controlsState"].getControlsState().getExperimentalMode()) { - const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration(); - float acceleration_future = 0; - if (acceleration.getZ().size() > 16) { - acceleration_future = acceleration.getX()[16]; // 2.5 seconds + // The first half of track_vertices are the points for the right side of the path + // and the indices match the positions of accel from uiPlan + const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); + const int max_len = std::min(scene.track_vertices.length() / 2, acceleration.size()); + + for (int i = 0; i < max_len; ++i) { + // Some points are out of frame + if (scene.track_vertices[i].y() < 0 || scene.track_vertices[i].y() > height()) continue; + + // Flip so 0 is bottom of frame + float lin_grad_point = (height() - scene.track_vertices[i].y()) / height(); + + // speed up: 120, slow down: 0 + float path_hue = fmax(fmin(60 + acceleration[i] * 35, 120), 0); + // FIXME: painter.drawPolygon can be slow if hue is not rounded + path_hue = int(path_hue * 100 + 0.5) / 100; + + float saturation = fmin(fabs(acceleration[i] * 1.5), 1); + float lightness = util::map_val(saturation, 0.0f, 1.0f, 0.95f, 0.62f); // lighter when grey + float alpha = util::map_val(lin_grad_point, 0.75f / 2.f, 0.75f, 0.4f, 0.0f); // matches previous alpha fade + bg.setColorAt(lin_grad_point, QColor::fromHslF(path_hue / 360., saturation, lightness, alpha)); + + // Skip a point, unless next is last + i += (i + 2) < max_len ? 1 : 0; } - start_hue = 60; - // speed up: 120, slow down: 0 - end_hue = fmax(fmin(start_hue + acceleration_future * 45, 148), 0); - // FIXME: painter.drawPolygon can be slow if hue is not rounded - end_hue = int(end_hue * 100 + 0.5) / 100; - - bg.setColorAt(0.0, QColor::fromHslF(start_hue / 360., 0.97, 0.56, 0.4)); - bg.setColorAt(0.5, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.35)); - bg.setColorAt(1.0, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.0)); } else { bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4)); bg.setColorAt(0.5, QColor::fromHslF(112 / 360., 1.0, 0.68, 0.35)); @@ -550,13 +561,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) int x = rightHandDM ? rect().right() - (btn_size - 24) / 2 - (bdr_s * 2) : (btn_size - 24) / 2 + (bdr_s * 2); int y = rect().bottom() - footer_h / 2; float opacity = dmActive ? 0.65 : 0.2; - drawIcon(painter, x, y, dm_img, blackColor(0), opacity); - - // circle background - painter.setOpacity(1.0); - painter.setPen(Qt::NoPen); - painter.setBrush(blackColor(70)); - painter.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); + drawIcon(painter, x, y, dm_img, blackColor(70), opacity); // face QPointF face_kpts_draw[std::size(default_face_kpts_3d)]; @@ -573,7 +578,10 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) const int arc_l = 133; const float arc_t_default = 6.7; const float arc_t_extend = 12.0; - QColor arc_color = QColor::fromRgbF(0.09, 0.945, 0.26, 0.4*(1.0-dm_fade_state)*(s->engaged())); + QColor arc_color = QColor::fromRgbF(0.545 - 0.445 * s->engaged(), + 0.545 + 0.4 * s->engaged(), + 0.545 - 0.285 * s->engaged(), + 0.4 * (1.0 - dm_fade_state)); float delta_x = -scene.driver_pose_sins[1] * arc_l / 2; float delta_y = -scene.driver_pose_sins[0] * arc_l / 2; painter.setPen(QPen(arc_color, arc_t_default+arc_t_extend*fmin(1.0, scene.driver_pose_diff[1] * 5.0), Qt::SolidLine, Qt::RoundCap)); @@ -645,16 +653,18 @@ void AnnotatedCameraWidget::paintGL() { } // Wide or narrow cam dependent on speed - float v_ego = sm["carState"].getCarState().getVEgo(); - if ((v_ego < 10) || s->wide_cam_only) { - wide_cam_requested = true; - } else if (v_ego > 15) { - wide_cam_requested = false; + bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD); + if (has_wide_cam) { + float v_ego = sm["carState"].getCarState().getVEgo(); + if ((v_ego < 10) || available_streams.size() == 1) { + wide_cam_requested = true; + } else if (v_ego > 15) { + wide_cam_requested = false; + } + wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode(); + // for replay of old routes, never go to widecam + wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid; } - wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode(); - // TODO: also detect when ecam vision stream isn't available - // for replay of old routes, never go to widecam - wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid; CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD; @@ -674,7 +684,7 @@ void AnnotatedCameraWidget::paintGL() { if (s->worldObjectsVisible()) { if (sm.rcv_frame("modelV2") > s->scene.started_frame) { - update_model(s, sm["modelV2"].getModelV2()); + update_model(s, sm["modelV2"].getModelV2(), sm["uiPlan"].getUiPlan()); if (sm.rcv_frame("radarState") > s->scene.started_frame) { update_leads(s, radar_state, sm["modelV2"].getModelV2().getPosition()); } diff --git a/selfdrive/ui/qt/qt_window.cc b/selfdrive/ui/qt/qt_window.cc index d630b560b..b83db7067 100644 --- a/selfdrive/ui/qt/qt_window.cc +++ b/selfdrive/ui/qt/qt_window.cc @@ -19,6 +19,10 @@ void setMainWindow(QWidget *w) { wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); wl_surface_commit(s); w->showFullScreen(); + + // ensure we have a valid eglDisplay, otherwise the ui will silently fail + void *egl = native->nativeResourceForWindow("egldisplay", w->windowHandle()); + assert(egl != nullptr); #endif } diff --git a/selfdrive/ui/qt/setup/reset.cc b/selfdrive/ui/qt/setup/reset.cc index 582217c1d..ccf9bf805 100644 --- a/selfdrive/ui/qt/setup/reset.cc +++ b/selfdrive/ui/qt/setup/reset.cc @@ -11,14 +11,11 @@ #define NVME "/dev/nvme0n1" #define USERDATA "/dev/disk/by-partlabel/userdata" -void Reset::doReset() { - // best effort to wipe nvme and sd card +void Reset::doErase() { + // best effort to wipe nvme std::system("sudo umount " NVME); std::system("yes | sudo mkfs.ext4 " NVME); - // we handle two cases here - // * user-prompted factory reset - // * recovering from a corrupt userdata by formatting int rm = std::system("sudo rm -rf /data/*"); std::system("sudo umount " USERDATA); int fmt = std::system("yes | sudo mkfs.ext4 " USERDATA); @@ -30,22 +27,26 @@ void Reset::doReset() { rebootBtn->show(); } +void Reset::startReset() { + body->setText(tr("Resetting device...\nThis may take up to a minute.")); + rejectBtn->hide(); + rebootBtn->hide(); + confirmBtn->hide(); +#ifdef __aarch64__ + QTimer::singleShot(100, this, &Reset::doErase); +#endif +} + void Reset::confirm() { const QString confirm_txt = tr("Are you sure you want to reset your device?"); if (body->text() != confirm_txt) { body->setText(confirm_txt); } else { - body->setText(tr("Resetting device...")); - rejectBtn->hide(); - rebootBtn->hide(); - confirmBtn->hide(); -#ifdef __aarch64__ - QTimer::singleShot(100, this, &Reset::doReset); -#endif + startReset(); } } -Reset::Reset(bool recover, QWidget *parent) : QWidget(parent) { +Reset::Reset(ResetMode mode, QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setContentsMargins(45, 220, 45, 45); main_layout->setSpacing(0); @@ -56,7 +57,7 @@ Reset::Reset(bool recover, QWidget *parent) : QWidget(parent) { main_layout->addSpacing(60); - body = new QLabel(tr("System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot.")); + body = new QLabel(tr("Press confirm to erase all content and settings. Press cancel to resume boot.")); body->setWordWrap(true); body->setStyleSheet("font-size: 80px; font-weight: light;"); main_layout->addWidget(body, 1, Qt::AlignTop | Qt::AlignLeft); @@ -82,10 +83,16 @@ Reset::Reset(bool recover, QWidget *parent) : QWidget(parent) { blayout->addWidget(confirmBtn); QObject::connect(confirmBtn, &QPushButton::clicked, this, &Reset::confirm); + bool recover = mode == ResetMode::RECOVER; rejectBtn->setVisible(!recover); rebootBtn->setVisible(recover); if (recover) { - body->setText(tr("Unable to mount data partition. Press confirm to reset your device.")); + body->setText(tr("Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.")); + } + + // automatically start if we're just finishing up an ABL reset + if (mode == ResetMode::FORMAT) { + startReset(); } setStyleSheet(R"( @@ -108,9 +115,17 @@ Reset::Reset(bool recover, QWidget *parent) : QWidget(parent) { } int main(int argc, char *argv[]) { - bool recover = argc > 1 && strcmp(argv[1], "--recover") == 0; + ResetMode mode = ResetMode::USER_RESET; + if (argc > 1) { + if (strcmp(argv[1], "--recover") == 0) { + mode = ResetMode::RECOVER; + } else if (strcmp(argv[1], "--format") == 0) { + mode = ResetMode::FORMAT; + } + } + QApplication a(argc, argv); - Reset reset(recover); + Reset reset(mode); setMainWindow(&reset); return a.exec(); } diff --git a/selfdrive/ui/qt/setup/reset.h b/selfdrive/ui/qt/setup/reset.h index 3a4994077..04a191d82 100644 --- a/selfdrive/ui/qt/setup/reset.h +++ b/selfdrive/ui/qt/setup/reset.h @@ -2,18 +2,25 @@ #include #include +enum ResetMode { + USER_RESET, // user initiated a factory reset from openpilot + RECOVER, // userdata is corrupt for some reason, give a chance to recover + FORMAT, // finish up an ABL factory reset +}; + class Reset : public QWidget { Q_OBJECT public: - explicit Reset(bool recover = false, QWidget *parent = 0); + explicit Reset(ResetMode mode, QWidget *parent = 0); private: QLabel *body; QPushButton *rejectBtn; QPushButton *rebootBtn; QPushButton *confirmBtn; - void doReset(); + void doErase(); + void startReset(); private slots: void confirm(); diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc index 392be68a1..de5021c8b 100644 --- a/selfdrive/ui/qt/setup/setup.cc +++ b/selfdrive/ui/qt/setup/setup.cc @@ -34,7 +34,7 @@ bool is_elf(char *fname) { void Setup::download(QString url) { CURL *curl = curl_easy_init(); if (!curl) { - emit finished(false); + emit finished(url, tr("Something went wrong. Reboot the device.")); return; } @@ -53,15 +53,24 @@ void Setup::download(QString url) { curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); curl_easy_setopt(curl, CURLOPT_USERAGENT, (USER_AGENT + version).c_str()); curl_easy_setopt(curl, CURLOPT_HTTPHEADER, list); + curl_easy_setopt(curl, CURLOPT_TIMEOUT, 30L); int ret = curl_easy_perform(curl); long res_status = 0; curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &res_status); - if (ret == CURLE_OK && res_status == 200 && is_elf(tmpfile)) { - rename(tmpfile, "/tmp/installer"); - emit finished(true); + + if (ret != CURLE_OK || res_status != 200) { + emit finished(url, tr("Ensure the entered URL is valid, and the device’s internet connection is good.")); + } else if (!is_elf(tmpfile)) { + emit finished(url, tr("No custom software found at this URL.")); } else { - emit finished(false); + rename(tmpfile, "/tmp/installer"); + + FILE *fp_url = fopen("/tmp/installer_url", "w"); + fprintf(fp_url, "%s", url.toStdString().c_str()); + fclose(fp_url); + + emit finished(url); } curl_slist_free_all(list); @@ -234,10 +243,10 @@ QWidget * Setup::downloading() { return widget; } -QWidget * Setup::download_failed() { +QWidget * Setup::download_failed(QLabel *url, QLabel *body) { QWidget *widget = new QWidget(); QVBoxLayout *main_layout = new QVBoxLayout(widget); - main_layout->setContentsMargins(55, 225, 55, 55); + main_layout->setContentsMargins(55, 185, 55, 55); main_layout->setSpacing(0); QLabel *title = new QLabel(tr("Download Failed")); @@ -246,7 +255,13 @@ QWidget * Setup::download_failed() { main_layout->addSpacing(67); - QLabel *body = new QLabel(tr("Ensure the entered URL is valid, and the device’s internet connection is good.")); + url->setWordWrap(true); + url->setAlignment(Qt::AlignTop | Qt::AlignLeft); + url->setStyleSheet("font-family: \"JetBrains Mono\"; font-size: 64px; font-weight: 400; margin-right: 100px;"); + main_layout->addWidget(url); + + main_layout->addSpacing(48); + body->setWordWrap(true); body->setAlignment(Qt::AlignTop | Qt::AlignLeft); body->setStyleSheet("font-size: 80px; font-weight: 300; margin-right: 100px;"); @@ -271,7 +286,7 @@ QWidget * Setup::download_failed() { restart->setProperty("primary", true); blayout->addWidget(restart); QObject::connect(restart, &QPushButton::clicked, this, [=]() { - setCurrentIndex(2); + setCurrentIndex(1); }); widget->setStyleSheet(R"( @@ -304,15 +319,19 @@ Setup::Setup(QWidget *parent) : QStackedWidget(parent) { downloading_widget = downloading(); addWidget(downloading_widget); - failed_widget = download_failed(); + QLabel *url_label = new QLabel(); + QLabel *body_label = new QLabel(); + failed_widget = download_failed(url_label, body_label); addWidget(failed_widget); - QObject::connect(this, &Setup::finished, [=](bool success) { - // hide setup on success - qDebug() << "finished" << success; - if (success) { + QObject::connect(this, &Setup::finished, [=](const QString &url, const QString &error) { + qDebug() << "finished" << url << error; + if (error.isEmpty()) { + // hide setup on success QTimer::singleShot(3000, this, &QWidget::hide); } else { + url_label->setText(url); + body_label->setText(error); setCurrentWidget(failed_widget); } }); diff --git a/selfdrive/ui/qt/setup/setup.h b/selfdrive/ui/qt/setup/setup.h index f990b5a6c..bf5d97070 100644 --- a/selfdrive/ui/qt/setup/setup.h +++ b/selfdrive/ui/qt/setup/setup.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -15,13 +16,13 @@ private: QWidget *getting_started(); QWidget *network_setup(); QWidget *downloading(); - QWidget *download_failed(); + QWidget *download_failed(QLabel *url, QLabel *body); QWidget *failed_widget; QWidget *downloading_widget; signals: - void finished(bool success); + void finished(const QString &url, const QString &error = ""); public slots: void nextPage(); diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 4f5231064..c844cb2ba 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -59,7 +59,8 @@ void configFont(QPainter &p, const QString &family, int size, const QString &sty } void clearLayout(QLayout* layout) { - while (QLayoutItem* item = layout->takeAt(0)) { + while (layout->count() > 0) { + QLayoutItem* item = layout->takeAt(0); if (QWidget* widget = item->widget()) { widget->deleteLater(); } @@ -110,7 +111,7 @@ void sigTermHandler(int s) { qApp->quit(); } -void initApp(int argc, char *argv[]) { +void initApp(int argc, char *argv[], bool disable_hidpi) { Hardware::set_display_power(true); Hardware::set_brightness(65); @@ -118,13 +119,13 @@ void initApp(int argc, char *argv[]) { std::signal(SIGINT, sigTermHandler); std::signal(SIGTERM, sigTermHandler); + if (disable_hidpi) { #ifdef __APPLE__ - { // Get the devicePixelRatio, and scale accordingly to maintain 1:1 rendering QApplication tmp(argc, argv); qputenv("QT_SCALE_FACTOR", QString::number(1.0 / tmp.devicePixelRatio() ).toLocal8Bit()); - } #endif + } setQtSurfaceFormat(); } diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h index 3188f3f9b..b86bbcf89 100644 --- a/selfdrive/ui/qt/util.h +++ b/selfdrive/ui/qt/util.h @@ -17,9 +17,10 @@ QMap getSupportedLanguages(); void configFont(QPainter &p, const QString &family, int size, const QString &style); void clearLayout(QLayout* layout); void setQtSurfaceFormat(); +void sigTermHandler(int s); QString timeAgo(const QDateTime &date); void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg); -void initApp(int argc, char *argv[]); +void initApp(int argc, char *argv[], bool disable_hidpi = true); QWidget* topWidget (QWidget* widget); QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); QPixmap bootstrapPixmap(const QString &id); diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 8c7a7072e..016129c34 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -96,8 +96,10 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : stream_name(stream_name), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { setAttribute(Qt::WA_OpaquePaintEvent); + qRegisterMetaType>("availableStreams"); QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection); QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection); + QObject::connect(this, &CameraWidget::vipcAvailableStreamsUpdated, this, &CameraWidget::availableStreamsUpdated, Qt::QueuedConnection); } CameraWidget::~CameraWidget() { @@ -112,6 +114,16 @@ CameraWidget::~CameraWidget() { doneCurrent(); } +// Qt uses device-independent pixels, depending on platform this may be +// different to what OpenGL uses +int CameraWidget::glWidth() { + return width() * devicePixelRatio(); +} + +int CameraWidget::glHeight() { + return height() * devicePixelRatio(); +} + void CameraWidget::initializeGL() { initializeOpenGLFunctions(); @@ -181,8 +193,12 @@ void CameraWidget::stopVipcThread() { } } +void CameraWidget::availableStreamsUpdated(std::set streams) { + available_streams = streams; +} + void CameraWidget::updateFrameMat() { - int w = width(), h = height(); + int w = glWidth(), h = glHeight(); if (zoomed_view) { if (active_stream_type == VISION_STREAM_DRIVER) { @@ -260,7 +276,7 @@ void CameraWidget::paintGL() { updateFrameMat(); - glViewport(0, 0, width(), height()); + glViewport(0, 0, glWidth(), glHeight()); glBindVertexArray(frame_vao); glUseProgram(program->programId()); glPixelStorei(GL_UNPACK_ALIGNMENT, 1); @@ -366,6 +382,13 @@ void CameraWidget::vipcThread() { if (!vipc_client->connected) { clearFrames(); + auto streams = VisionIpcClient::getAvailableStreams(stream_name, false); + if (streams.empty()) { + QThread::msleep(100); + continue; + } + emit vipcAvailableStreamsUpdated(streams); + if (!vipc_client->connect(false)) { QThread::msleep(100); continue; @@ -400,4 +423,5 @@ void CameraWidget::vipcThread() { void CameraWidget::clearFrames() { std::lock_guard lk(frame_lock); frames.clear(); + available_streams.clear(); } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index f8b97be03..8a140e529 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -41,6 +41,7 @@ signals: void clicked(); void vipcThreadConnected(VisionIpcClient *); void vipcThreadFrameReceived(); + void vipcAvailableStreamsUpdated(std::set); protected: void paintGL() override; @@ -53,6 +54,9 @@ protected: void vipcThread(); void clearFrames(); + int glWidth(); + int glHeight(); + bool zoomed_view; GLuint frame_vao, frame_vbo, frame_ibo; GLuint textures[2]; @@ -71,6 +75,7 @@ protected: int stream_stride = 0; std::atomic active_stream_type; std::atomic requested_stream_type; + std::set available_streams; QThread *vipc_thread = nullptr; // Calibration @@ -88,4 +93,7 @@ protected: protected slots: void vipcConnected(VisionIpcClient *vipc_client); void vipcFrameReceived(); + void availableStreamsUpdated(std::set streams); }; + +Q_DECLARE_METATYPE(std::set); diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index da2f4e60d..7f88b049a 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -277,7 +277,7 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { primeUser = new PrimeUserWidget; mainLayout->addWidget(primeUser); - mainLayout->setCurrentWidget(uiState()->prime_type ? (QWidget*)primeUser : (QWidget*)primeAd); + mainLayout->setCurrentWidget(uiState()->primeType() ? (QWidget*)primeUser : (QWidget*)primeAd); setFixedWidth(750); setStyleSheet(R"( @@ -312,7 +312,7 @@ void SetupWidget::replyFinished(const QString &response, bool success) { QJsonObject json = doc.object(); int prime_type = json["prime_type"].toInt(); - uiState()->prime_type = prime_type; + uiState()->setPrimeType(prime_type); if (!json["is_paired"].toBool()) { mainLayout->setCurrentIndex(0); diff --git a/selfdrive/ui/soundd/main.cc b/selfdrive/ui/soundd/main.cc index 64088deff..c6c7434ca 100644 --- a/selfdrive/ui/soundd/main.cc +++ b/selfdrive/ui/soundd/main.cc @@ -5,17 +5,13 @@ #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/soundd/sound.h" -void sigHandler(int s) { - qApp->quit(); -} - int main(int argc, char **argv) { qInstallMessageHandler(swagLogMessageHandler); setpriority(PRIO_PROCESS, 0, -20); QApplication a(argc, argv); - std::signal(SIGINT, sigHandler); - std::signal(SIGTERM, sigHandler); + std::signal(SIGINT, sigTermHandler); + std::signal(SIGTERM, sigTermHandler); Sound sound; return a.exec(); diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index b40812af7..2e4e833e0 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -576,18 +576,10 @@ location set Are you sure you want to reset your device? Bist du sicher, dass du das Gerät auf Werkseinstellungen zurücksetzen möchtest? - - Resetting device... - Gerät wird zurückgesetzt... - System Reset System auf Werkseinstellungen zurücksetzen - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - Zurücksetzen auf Werkseinstellungen wurde ausgewählt. Drücke Annehmen, um alle Inhalte und Einstellungen zu löschen. Drücke Abbrechen, um mit dem Starten des Gerätes fortzufahren. - Cancel Abbrechen @@ -601,8 +593,17 @@ location set Bestätigen - Unable to mount data partition. Press confirm to reset your device. - Datenpartition kann nicht geöffnet werden. Drücke Annehmen, um dein Gerät auf Werkseinstellungen zurückzusetzen. + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + + + + Press confirm to erase all content and settings. Press cancel to resume boot. + + + + Resetting device... +This may take up to a minute. + @@ -702,6 +703,14 @@ location set Start over Von neuem beginnen + + No custom software found at this URL. + + + + Something went wrong. Reboot the device. + + SetupWidget @@ -983,22 +992,10 @@ location set Experimental Mode Experimenteller Modus - - Experimental openpilot Longitudinal Control - Experimenteller Openpilot Tempomat - - - WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). - WARNUNG: Der Openpilot Tempomat ist für dieses Auto experimentell und deaktiviert den Notbremsassistenten. - Disengage on Accelerator Pedal Bei Gasbetätigung ausschalten - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - Bei diesem auto wird standardmäßig der im Auto eingebaute adaptive Tempomat anstelle des Openpilot Tempomats benutzt. Aktiviere diesen Schalter, um zum Openpilot Tempomaten zu wechseln. Es ist empfohlen den Experimentellen Modus bei Nutzung des Openpilot Tempomats zu aktivieren. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: Openpilot fährt standardmäßig im <b>entspannten Modus</b>. Der Experimentelle Modus aktiviert<b>Alpha-level Funktionen</b>, die noch nicht für den entspannten Modus bereit sind. Die experimentellen Funktionen sind die Folgenden: @@ -1035,6 +1032,18 @@ location set Enable experimental longitudinal control to allow Experimental mode. Aktiviere den experimentellen Openpilot Tempomaten für experimentelle Funktionen. + + openpilot Longitudinal Control (Alpha) + + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + + Updater diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index ed08a52df..06435d3b4 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -574,18 +574,10 @@ location set Are you sure you want to reset your device? 初期化してもよろしいですか？ - - Resetting device... - デバイスが初期化されます... - System Reset システムを初期化 - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - システムの初期化をリクエストしました。「確認」ボタンを押すとデバイスが初期化されます。「キャンセル」ボタンを押すと起動を続行します。 - Cancel キャンセル @@ -599,8 +591,17 @@ location set 確認 - Unable to mount data partition. Press confirm to reset your device. - 「data」パーティションをマウントできません。「確認」ボタンを押すとデバイスが初期化されます。 + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + + + + Press confirm to erase all content and settings. Press cancel to resume boot. + + + + Resetting device... +This may take up to a minute. + @@ -700,6 +701,14 @@ location set Start over 最初からやり直す + + No custom software found at this URL. + + + + Something went wrong. Reboot the device. + + SetupWidget @@ -953,10 +962,6 @@ location set Upload data from the driver facing camera and help improve the driver monitoring algorithm. 車内カメラの映像をアップロードし、ドライバー監視システムのアルゴリズムの向上に役立てます。 - - Experimental openpilot Longitudinal Control - 実験段階のopenpilotによるアクセル制御 - Disengage on Accelerator Pedal ｱｸｾﾙを踏むと openpilot を中断 @@ -985,14 +990,6 @@ location set Experimental Mode 実験モード - - WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). - 警告: この車種でのopenpilotによるアクセル制御は実験段階であり、衝突被害軽減ブレーキ(AEB)を無効化します。 - - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - openpilotはこの車の場合、車に内蔵されているACCを標準で利用します。この機能を有効にすることで実験段階のopenpilotによるアクセル制御を利用できます。実験モードと合わせて利用することをお勧めします。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilotは標準ではゆっくりとくつろげる運転を提供します。この実験モードを有効にすると、以下のくつろげる段階ではない開発中の機能を利用する事ができます。 @@ -1029,6 +1026,18 @@ location set Enable experimental longitudinal control to allow Experimental mode. 実験段階のopenpilotによるアクセル制御を有効にしてください。 + + openpilot Longitudinal Control (Alpha) + + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + + Updater diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 7b742a504..b55a56a06 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -403,7 +403,7 @@ location set Waiting for GPS - GPS를 기다리는 중 + GPS 수신중 입니다 @@ -574,18 +574,10 @@ location set Are you sure you want to reset your device? 장치를 초기화 하시겠습니까? - - Resetting device... - 장치 초기화중... - System Reset 장치 초기화 - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - 장치를 초기화 합니다. 확인버튼을 누르면 모든 내용과 설정이 초기화됩니다. 부팅을 재개하려면 취소를 누르세요. - Cancel 취소 @@ -599,8 +591,18 @@ location set 확인 - Unable to mount data partition. Press confirm to reset your device. - 데이터 파티션을 마운트할 수 없습니다. 확인 버튼을 눌러 장치를 리셋합니다. + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + 데이터 파티션을 마운트할 수 없습니다. 파티션이 손상되었을 수 있습니다. 장치를 초기화하려면 확인을 누르세요. + + + Press confirm to erase all content and settings. Press cancel to resume boot. + 모든 콘텐츠와 설정을 지우려면 확인을 누르세요. 부팅을 재개하려면 취소를 누르세요. + + + Resetting device... +This may take up to a minute. + 장치 초기화 중... +최대 1분이 소요될 수 있습니다. @@ -700,6 +702,14 @@ location set Start over 다시 시작 + + Something went wrong. Reboot the device. + 문제가 발생했습니다. 장치를 재부팅하세요. + + + No custom software found at this URL. + 이 URL에서 커스텀 소프트웨어를 찾을 수 없습니다. + SetupWidget @@ -953,10 +963,6 @@ location set Upload data from the driver facing camera and help improve the driver monitoring algorithm. 운전자 카메라에서 데이터를 업로드하고 운전자 모니터링 알고리즘을 개선합니다. - - Experimental openpilot Longitudinal Control - openpilot 롱컨트롤 (실험적) - Disengage on Accelerator Pedal 가속페달 조작시 해제 @@ -985,14 +991,6 @@ location set Experimental Mode 실험적 모드 - - WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). - 경고: openpilot 롱컨트롤은 실험적인 기능으로 차량의 자동긴급제동(AEB)를 비활성화합니다. - - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - 이 차량은 openpilot 롱컨트롤 대신 차량의 내장 ACC로 기본 설정됩니다. openpilot 롱컨트롤을 사용하려면 이 옵션을 활성화하세요. 실험적 openpilot 롱컨트롤을 사용하는 경우 실험적 모드를 활성화 하세요. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot은 기본적으로 <b>안정적 모드</b>로 주행합니다. 실험적 모드는 안정적 모드에 준비되지 않은 <b>알파 수준 기능</b>을 활성화 합니다. 실험적 모드의 특징은 아래에 나열되어 있습니다 @@ -1029,6 +1027,18 @@ location set Enable experimental longitudinal control to allow Experimental mode. 실험적 롱컨트롤을 사용하려면 실험적 모드를 활성화 하세요. + + openpilot Longitudinal Control (Alpha) + openpilot 롱컨트롤 (알파) + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + 경고: openpilot 롱컨트롤은 알파 기능으로 차량의 자동긴급제동(AEB)를 비활성화합니다. + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + 이 차량은 openpilot 롱컨트롤 대신 차량의 내장 ACC로 기본 설정됩니다. openpilot 롱컨트롤으로 전환하려면 이 기능을 활성화하세요. openpilot 롱컨트롤 알파를 활성화하는경우 실험적 모드 활성화를 권장합니다. + Updater diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index a25c51de3..34fbc8abf 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -578,18 +578,10 @@ trabalho definido Are you sure you want to reset your device? Tem certeza que quer resetar seu dispositivo? - - Resetting device... - Resetando dispositivo... - System Reset Resetar Sistema - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - Solicitado reset do sistema. Confirme para apagar todo conteúdo e configurações. Aperte cancelar para continuar boot. - Cancel Cancelar @@ -603,8 +595,18 @@ trabalho definido Confirmar - Unable to mount data partition. Press confirm to reset your device. - Não foi possível montar a partição de dados. Pressione confirmar para resetar seu dispositivo. + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + Não é possível montar a partição de dados. Partição corrompida. Confirme para apagar e redefinir o dispositivo. + + + Press confirm to erase all content and settings. Press cancel to resume boot. + Pressione confirmar para apagar todo o conteúdo e configurações. Pressione cancelar para voltar. + + + Resetting device... +This may take up to a minute. + Redefinindo o dispositivo +Isso pode levar até um minuto. @@ -704,6 +706,14 @@ trabalho definido Start over Inicializar + + No custom software found at this URL. + Não há software personalizado nesta URL. + + + Something went wrong. Reboot the device. + Algo deu errado. Reinicie o dispositivo. + SetupWidget @@ -957,10 +967,6 @@ trabalho definido Upload data from the driver facing camera and help improve the driver monitoring algorithm. Upload dados da câmera voltada para o motorista e ajude a melhorar o algoritmo de monitoramentor. - - Experimental openpilot Longitudinal Control - Controle longitudinal experimental openpilot - Disengage on Accelerator Pedal Desacionar com Pedal do Acelerador @@ -989,14 +995,6 @@ trabalho definido Experimental Mode Modo Experimental - - WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). - ATENÇÃO: o controle longitudinal do openpilot é experimental para este carro e desativará a Frenagem Automática de Emergência (AEB). - - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - Neste carro o penpilot por padrão utiliza o ACC nativo do veículo ao invés de controlar longitudinalmente. Ative isto para mudar para o controle longitudinal do openpilot. Ativar o Modo Experimental é recomendado quando em uso do controle longitudinal experimental do openpilot. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot por padrão funciona em <b>modo chill</b>. modo Experimental ativa <b>recursos de nível-alfa</b> que não estão prontos para o modo chill. Recursos experimentais estão listados abaixo: @@ -1033,6 +1031,18 @@ trabalho definido Enable experimental longitudinal control to allow Experimental mode. Ative o controle longitudinal experimental para permitir o modo Experimental. + + openpilot Longitudinal Control (Alpha) + Controle Longitudinal openpilot (Alpha) + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + AVISO: o controle longitudinal openpilot está em alfa para este carro e desativará a Frenagem Automática de Emergência (AEB). + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + Neste carro, o openpilot tem como padrão o ACC embutido do carro em vez do controle longitudinal do openpilot. Habilite isso para alternar para o controle longitudinal openpilot. Recomenda-se ativar o modo Experimental ao ativar o alfa de controle longitudinal openpilot. + Updater diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index ce394ecb9..45f91f1cd 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -311,18 +311,6 @@ Installing... กำลังติดตั้ง... - - Receiving objects: - กำลังรับข้อมูล: - - - Resolving deltas: - การแก้ไขเดลต้า: - - - Updating files: - กำลังอัปเดตไฟล์: - MapETA @@ -586,18 +574,10 @@ location set Are you sure you want to reset your device? คุณแน่ใจหรือไม่ว่าต้องการรีเซ็ตอุปกรณ์? - - Resetting device... - กำลังรีเซ็ตอุปกรณ์... - System Reset รีเซ็ตระบบ - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - มีการสั่งรีเซ็ตระบบ กดยืนยันเพื่อลบข้อมูลและการตั้งค่าทั้งหมด กดยกเลิกเพื่อบูตเข้าระบบตามปกติ - Cancel ยกเลิก @@ -611,8 +591,18 @@ location set ยืนยัน - Unable to mount data partition. Press confirm to reset your device. - ไม่สามารถเมานต์พาร์ติชั่นข้อมูล กดยืนยันเพื่อรีเซ็ตอุปกรณ์ของคุณ + Resetting device... +This may take up to a minute. + กำลังรีเซ็ตอุปกรณ์... +อาจใช้เวลาถึงหนึ่งนาที + + + Press confirm to erase all content and settings. Press cancel to resume boot. + กดยืนยันเพื่อลบข้อมูลและการตั้งค่าทั้งหมด กดยกเลิกเพื่อบูตต่อ + + + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + ไม่สามารถเมานต์พาร์ติชั่นข้อมูลได้ พาร์ติชั่นอาจเสียหาย กดยืนยันเพื่อลบและรีเซ็ตอุปกรณ์ของคุณ @@ -684,18 +674,6 @@ location set Waiting for internet กำลังรอสัญญาณอินเตอร์เน็ต - - Choose Software to Install - เลือกซอฟต์แวร์ที่จะติดตั้ง - - - Dashcam - กล้องติดรถยนต์ - - - Custom Software - ซอฟต์แวร์ที่กำหนดเอง - Enter URL ป้อน URL @@ -724,6 +702,14 @@ location set Start over เริ่มต้นใหม่ + + Something went wrong. Reboot the device. + มีบางอย่างผิดพลาด รีบูตอุปกรณ์ + + + No custom software found at this URL. + ไม่พบซอฟต์แวร์ที่กำหนดเองที่ URL นี้ + SetupWidget diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 68c1bb766..7cacc72f1 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -572,18 +572,10 @@ location set Are you sure you want to reset your device? 您确定要重置您的设备吗？ - - Resetting device... - 正在重置设备…… - System Reset 恢复出厂设置 - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - 已触发系统重置：确认以删除所有内容和设置。取消以正常启动设备。 - Cancel 取消 @@ -597,8 +589,17 @@ location set 确认 - Unable to mount data partition. Press confirm to reset your device. - 无法挂载数据分区。 确认以重置您的设备。 + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + + + + Press confirm to erase all content and settings. Press cancel to resume boot. + + + + Resetting device... +This may take up to a minute. + @@ -698,6 +699,14 @@ location set Start over 重来 + + No custom software found at this URL. + + + + Something went wrong. Reboot the device. + + SetupWidget @@ -951,10 +960,6 @@ location set Upload data from the driver facing camera and help improve the driver monitoring algorithm. 上传驾驶员摄像头的数据，帮助改进驾驶员监控算法。 - - Experimental openpilot Longitudinal Control - 试验性的openpilot纵向控制 - Disengage on Accelerator Pedal 踩油门时取消控制 @@ -983,14 +988,6 @@ location set Experimental Mode 测试模式 - - WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). - 警告： 此车辆的openpilot纵向控制是试验性功能，且将禁用AEB自动刹车功能。 - - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - 针对此车辆，openpilot默认使用车辆自带的ACC，而非openpilot的纵向控制。启用此选项将切换到openpilot纵向控制。当使用试验性的openpilot纵向控制时，建议同时启用试验模式。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot 默认 <b>轻松模式</b>驾驶车辆。试验模式启用一些轻松模式之外的 <b>试验性功能</b>。试验性功能包括： @@ -1027,6 +1024,18 @@ location set Enable experimental longitudinal control to allow Experimental mode. 启用试验性的纵向控制，以便允许使用试验模式。 + + openpilot Longitudinal Control (Alpha) + + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + + Updater diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 71315e118..e559ca88a 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -200,7 +200,7 @@ openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. - openpilot 需要將裝置固定在左右偏差 4° 以內，朝上偏差 5° 以内或朝下偏差 8° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。 + openpilot 需要將設備固定在左右偏差 4° 以內，朝上偏差 5° 以内或朝下偏差 8° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。 Your device is pointed %1° %2 and %3° %4. @@ -574,18 +574,10 @@ location set Are you sure you want to reset your device? 您確定要重置你的設備嗎？ - - Resetting device... - 重置設備中… - System Reset 系統重置 - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - 系統重置已觸發。請按確認刪除所有內容和設置。按取消恢復啟動。 - Cancel 取消 @@ -599,8 +591,17 @@ location set 確認 - Unable to mount data partition. Press confirm to reset your device. - 無法掛載數據分區。請按確認重置您的設備。 + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + 無法掛載資料分割區 分割區可能已經毀損 請確認是否要刪除並重新設定 + + + Press confirm to erase all content and settings. Press cancel to resume boot. + 按下確認以刪除所有內容及設定 按下取消來繼續開機 + + + Resetting device... +This may take up to a minute. + 設備重置中 此過程可能需要幾分鐘 @@ -700,6 +701,14 @@ location set Start over 重新開始 + + No custom software found at this URL. + 無法在此URL找到定制的軟體 + + + Something went wrong. Reboot the device. + 發生了一些錯誤 請重新啟動您的設備 + SetupWidget @@ -953,10 +962,6 @@ location set Upload data from the driver facing camera and help improve the driver monitoring algorithm. 上傳駕駛監控的錄像來協助我們提升駕駛監控的準確率。 - - Experimental openpilot Longitudinal Control - 使用 openpilot 縱向控制（實驗） - Disengage on Accelerator Pedal 油門取消控車 @@ -985,14 +990,6 @@ location set Experimental Mode 實驗模式 - - WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). - 警告：openpilot 縱向控制在這輛車上仍屬實驗性質，啟用後會喪失自動緊急煞車 (AEB) 功能。 - - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - 在本車輛中，openpilot預設將使用原車內建的ACC系統，而非openpilot縱向控制。開啟此開關來啟用openpilot縱向控制，使用此選項時建議一併啟用實驗模式。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot 預設以 <b>輕鬆模式</b> 駕駛。 實驗模式啟用了尚未準備好進入輕鬆模式的 <b>alpha 級功能</b>。實驗功能如下： @@ -1019,16 +1016,28 @@ location set openpilot longitudinal control may come in a future update. - + 未來可能會推出openpilot縱向控制 An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - + 在非發行分支中 可找到包含實驗模式的openpilot縱向控制測試版本 Enable experimental longitudinal control to allow Experimental mode. 啟用實驗性縱向控制以使用實驗模式。 + + openpilot Longitudinal Control (Alpha) + + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + + Updater diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 6c850b8ca..ac8caa0f2 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -35,7 +35,7 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, return false; } -int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { +int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) { const auto line_x = line.getX(); int max_idx = 0; for (int i = 1; i < TRAJECTORY_SIZE && line_x[i] <= path_height; ++i) { @@ -44,7 +44,7 @@ int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const return max_idx; } -void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line) { +void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) { for (int i = 0; i < 2; ++i) { auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); if (lead_data.getStatus()) { @@ -54,7 +54,7 @@ void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, con } } -void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, +void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) { const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); QPolygonF left_points, right_points; @@ -79,10 +79,15 @@ void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Rea *pvd = left_points + right_points; } -void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { +void update_model(UIState *s, + const cereal::ModelDataV2::Reader &model, + const cereal::UiPlan::Reader &plan) { UIScene &scene = s->scene; - auto model_position = model.getPosition(); - float max_distance = std::clamp(model_position.getX()[TRAJECTORY_SIZE - 1], + auto plan_position = plan.getPosition(); + if (plan_position.getX().size() < TRAJECTORY_SIZE){ + plan_position = model.getPosition(); + } + float max_distance = std::clamp(plan_position.getX()[TRAJECTORY_SIZE - 1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE); // update lane lines @@ -108,8 +113,8 @@ void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { const float lead_d = lead_one.getDRel() * 2.; max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } - max_idx = get_path_length_idx(model_position, max_distance); - update_line_data(s, model_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); + max_idx = get_path_length_idx(plan_position, max_distance); + update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); } void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) { @@ -174,7 +179,7 @@ static void update_state(UIState *s) { scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i,j); } } - scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; + scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; scene.calibration_wide_valid = wfde_list.size() == 3; } if (sm.updated("pandaStates")) { @@ -229,29 +234,20 @@ void UIState::updateStatus() { if (scene.started) { status = STATUS_DISENGAGED; scene.started_frame = sm->frame; - wide_cam_only = Params().getBool("WideCameraOnly"); } started_prev = scene.started; emit offroadTransition(!scene.started); } - - // Handle prime type change - if (prime_type != prime_type_prev) { - prime_type_prev = prime_type; - emit primeTypeChanged(prime_type); - Params().put("PrimeType", std::to_string(prime_type)); - } } UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", - "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", }); Params params; - wide_cam_only = params.getBool("WideCameraOnly"); prime_type = std::atoi(params.get("PrimeType").c_str()); language = QString::fromStdString(params.get("LanguageSetting")); @@ -272,6 +268,14 @@ void UIState::update() { emit uiUpdate(*this); } +void UIState::setPrimeType(int type) { + if (type != prime_type) { + prime_type = type; + Params().put("PrimeType", std::to_string(prime_type)); + emit primeTypeChanged(prime_type); + } +} + Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QObject(parent) { setAwake(true); resetInteractiveTimout(); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index e3eb97a76..2df047003 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -133,10 +133,13 @@ public: void updateStatus(); inline bool worldObjectsVisible() const { return sm->rcv_frame("liveCalibration") > scene.started_frame; - }; + } inline bool engaged() const { return scene.started && (*sm)["controlsState"].getControlsState().getEnabled(); - }; + } + + void setPrimeType(int type); + inline int primeType() const { return prime_type; } int fb_w = 0, fb_h = 0; @@ -146,11 +149,9 @@ public: UIScene scene = {}; bool awake; - int prime_type; QString language; QTransform car_space_transform; - bool wide_cam_only; signals: void uiUpdate(const UIState &s); @@ -163,7 +164,7 @@ private slots: private: QTimer *timer; bool started_prev = false; - int prime_type_prev = -1; + int prime_type = -1; }; UIState *uiState(); @@ -198,9 +199,11 @@ public slots: }; void ui_update_params(UIState *s); -int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height); -void update_model(UIState *s, const cereal::ModelDataV2::Reader &model); +int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height); +void update_model(UIState *s, + const cereal::ModelDataV2::Reader &model, + const cereal::UiPlan::Reader &plan); void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd); -void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line); -void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, +void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); +void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert); diff --git a/selfdrive/ui/update_translations.py b/selfdrive/ui/update_translations.py index e15d4c343..5964eb49d 100755 --- a/selfdrive/ui/update_translations.py +++ b/selfdrive/ui/update_translations.py @@ -19,7 +19,7 @@ def update_translations(vanish=False, plural_only=None, translations_dir=TRANSLA for file in translation_files.values(): tr_file = os.path.join(translations_dir, f"{file}.ts") - args = f"lupdate -locations none -recursive {UI_DIR} -ts {tr_file}" + args = f"lupdate -locations none -recursive {UI_DIR} -ts {tr_file} -I {BASEDIR}" if vanish: args += " -no-obsolete" if file in plural_only: diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 7ee373805..9ef5d89a4 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -157,9 +157,6 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setHighConversionGain(frame_data.high_conversion_gain); framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction); framed.setTargetGreyFraction(frame_data.target_grey_fraction); - framed.setLensPos(frame_data.lens_pos); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); framed.setProcessingTime(frame_data.processing_time); const float ev = c->cur_ev[frame_data.frame_id % 3]; diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 088b9f793..07d1291a2 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -66,11 +66,6 @@ typedef struct FrameMetadata { float measured_grey_fraction; float target_grey_fraction; - // Focus - unsigned int lens_pos; - float lens_err; - float lens_true_pos; - float processing_time; } FrameMetadata; diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 92b3bde41..4325eccde 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -71,7 +71,7 @@ const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1; const float TARGET_GREY_FACTOR_AR0231 = 1.0; -const float TARGET_GREY_FACTOR_OX03C10 = 0.02; +const float TARGET_GREY_FACTOR_OX03C10 = 0.01; const float sensor_analog_gains_AR0231[] = { 1.0/8.0, 2.0/8.0, 2.0/7.0, 3.0/7.0, // 0, 1, 2, 3 @@ -101,7 +101,7 @@ const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1; const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0; const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; -const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x11; // 2.5x +const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x0; // 1x const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1; const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4; @@ -1175,23 +1175,18 @@ void CameraState::set_camera_exposure(float grey_frac) { // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD uint32_t hcg_time = exposure_time; uint32_t lcg_time = hcg_time; - uint32_t spd_time = exposure_time_max + VS_TIME_MAX_OX03C10; + uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10); uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; - uint32_t min_gain = ox03c10_analog_gains_reg[0]; - uint32_t spd_gain = 0xF00; struct i2c_random_wr_payload exp_reg_array[] = { {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, - {0x35c1, vs_time>>8}, {0x35c2, vs_time&0xFF}, + {0x35c2, vs_time&0xFF}, {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, - {0x3588, min_gain>>8}, {0x3589, min_gain&0xFF}, - {0x3548, spd_gain>>8}, {0x3549, spd_gain&0xFF}, - {0x35c8, min_gain>>8}, {0x35c9, min_gain&0xFF}, }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); } diff --git a/system/camerad/cameras/real_debayer.cl b/system/camerad/cameras/real_debayer.cl index cff5ae455..e15a873d6 100644 --- a/system/camerad/cameras/real_debayer.cl +++ b/system/camerad/cameras/real_debayer.cl @@ -18,6 +18,9 @@ float3 color_correct(float3 rgb) { x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782); #endif + #if IS_OX + return -0.507089*exp(-12.54124638*x)+0.9655*powr(x,0.5)-0.472597*x+0.507089; + #else // tone mapping params const float gamma_k = 0.75; const float gamma_b = 0.125; @@ -28,6 +31,7 @@ float3 color_correct(float3 rgb) { return (x > mp) ? ((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) : ((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b); + #endif } float get_vignetting_s(float r) { diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index 83fcb8f7a..9170c5183 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -58,7 +58,7 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { // SC ctrl {0x3001, 0x03}, // io_pad_oen - {0x3002, 0xf8}, // io_pad_oen + {0x3002, 0xfc}, // io_pad_oen {0x3005, 0x80}, // io_pad_out {0x3007, 0x01}, // io_pad_sel {0x3008, 0x80}, // io_pad_sel @@ -85,6 +85,9 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x3882, 0x8}, {0x3883, 0x0D}, {0x3836, 0x1F}, {0x3837, 0x40}, + {0x3892, 0x44}, + {0x3823, 0x48}, + {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI {0x3020, 0x05}, // SC_CTRL_20 @@ -179,8 +182,8 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x3820, 0x04}, {0x3821, 0x19}, - {0x3832, 0x00}, - {0x3834, 0x00}, + {0x3832, 0xF0}, + {0x3834, 0xF0}, {0x384c, 0x02}, {0x384d, 0x0d}, {0x3850, 0x00}, @@ -749,6 +752,12 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x5486, 0x08}, {0x5487, 0xDE}, {0x5686, 0x08}, {0x5687, 0xDE}, {0x5886, 0x08}, {0x5887, 0xDE}, + + // fixed gains + {0x3588, 0x01}, {0x3589, 0x00}, + {0x35c8, 0x01}, {0x35c9, 0x00}, + {0x3548, 0x0F}, {0x3549, 0x00}, + {0x35c1, 0x00}, }; struct i2c_random_wr_payload init_array_ar0231[] = { diff --git a/system/hardware/base.h b/system/hardware/base.h index 6cfc1d874..546009972 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -18,6 +18,10 @@ public: static std::string get_serial() { return "cccccc"; } + static std::map get_init_logs() { + return {}; + } + static void reboot() {} static void poweroff() {} static void set_brightness(int percent) {} diff --git a/system/hardware/base.py b/system/hardware/base.py index 31df1babe..0b6ca44c3 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -135,6 +135,9 @@ class HardwareBase(ABC): def get_networks(self): pass + def has_internal_panda(self) -> bool: + return False + def reset_internal_panda(self): pass diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 7876b1af1..6730f8270 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -1,19 +1,19 @@ [ { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a.img.xz", - "hash": "72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a", - "hash_raw": "72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a", - "size": 14780416, + "url": "https://commadist.azureedge.net/agnosupdate/boot-7d953f5e1bc606984e4d49c6f957421a4172f72b4ebd359baa689ef43b7e911c.img.xz", + "hash": "7d953f5e1bc606984e4d49c6f957421a4172f72b4ebd359baa689ef43b7e911c", + "hash_raw": "7d953f5e1bc606984e4d49c6f957421a4172f72b4ebd359baa689ef43b7e911c", + "size": 15153152, "sparse": false, "full_check": true, "has_ab": true }, { "name": "abl", - "url": "https://commadist.azureedge.net/agnosupdate/abl-ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82.img.xz", - "hash": "ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82", - "hash_raw": "ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82", + "url": "https://commadist.azureedge.net/agnosupdate/abl-50329ac734ff7a6c20c3f552dce9b13f84b3eb2e73faa64b9810049d9b406602.img.xz", + "hash": "50329ac734ff7a6c20c3f552dce9b13f84b3eb2e73faa64b9810049d9b406602", + "hash_raw": "50329ac734ff7a6c20c3f552dce9b13f84b3eb2e73faa64b9810049d9b406602", "size": 274432, "sparse": false, "full_check": true, @@ -21,29 +21,49 @@ }, { "name": "xbl", - "url": "https://commadist.azureedge.net/agnosupdate/xbl-2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35.img.xz", - "hash": "2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35", - "hash_raw": "2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35", - "size": 3670016, + "url": "https://commadist.azureedge.net/agnosupdate/xbl-dc297986b38f50c47584bd8549b188b37b1d6a0c77b3255859dd675c177b5c15.img.xz", + "hash": "dc297986b38f50c47584bd8549b188b37b1d6a0c77b3255859dd675c177b5c15", + "hash_raw": "dc297986b38f50c47584bd8549b188b37b1d6a0c77b3255859dd675c177b5c15", + "size": 3282672, "sparse": false, "full_check": true, "has_ab": true }, { "name": "xbl_config", - "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad.img.xz", - "hash": "3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad", - "hash_raw": "3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad", - "size": 364544, + "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-b73fbbb42934aabc6d4f16ce84ac6c8c0205bc70e0a85412a771f3cc1d62cc40.img.xz", + "hash": "b73fbbb42934aabc6d4f16ce84ac6c8c0205bc70e0a85412a771f3cc1d62cc40", + "hash_raw": "b73fbbb42934aabc6d4f16ce84ac6c8c0205bc70e0a85412a771f3cc1d62cc40", + "size": 98124, + "sparse": false, + "full_check": true, + "has_ab": true + }, + { + "name": "devcfg", + "url": "https://commadist.azureedge.net/agnosupdate/devcfg-2d3063d106813006ac9ceeaf8818a31d4b33996873e81178ac5129f5e1b82bca.img.xz", + "hash": "2d3063d106813006ac9ceeaf8818a31d4b33996873e81178ac5129f5e1b82bca", + "hash_raw": "2d3063d106813006ac9ceeaf8818a31d4b33996873e81178ac5129f5e1b82bca", + "size": 40336, + "sparse": false, + "full_check": true, + "has_ab": true + }, + { + "name": "aop", + "url": "https://commadist.azureedge.net/agnosupdate/aop-d69450d5438b3e5e2ba5b77db1ae49e1cf9cab17836f563aa57192b5b3a4ac3e.img.xz", + "hash": "d69450d5438b3e5e2ba5b77db1ae49e1cf9cab17836f563aa57192b5b3a4ac3e", + "hash_raw": "d69450d5438b3e5e2ba5b77db1ae49e1cf9cab17836f563aa57192b5b3a4ac3e", + "size": 184364, "sparse": false, "full_check": true, "has_ab": true }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-9efdc9368f05e06008a7a1dbbee21b564e89988dc94d6ddee3a3a88e42268f0e.img.xz", - "hash": "48209ce7e8cc2fff4ec024f0cd82fc2e3e097b5c0629be2b292acf64e6701449", - "hash_raw": "9efdc9368f05e06008a7a1dbbee21b564e89988dc94d6ddee3a3a88e42268f0e", + "url": "https://commadist.azureedge.net/agnosupdate/system-4a8311dd591006e0c2a6f60060d6ef579ceec9b3d688e8438a9aef4e230ae028.img.xz", + "hash": "23c9f111f81fc3ee83f85016cb320e03a46aad6721a85e1b4a3f04b6a764e934", + "hash_raw": "4a8311dd591006e0c2a6f60060d6ef579ceec9b3d688e8438a9aef4e230ae028", "size": 10737418240, "sparse": true, "full_check": false, diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py old mode 100644 new mode 100755 index 8233834d1..5b656a40f --- a/system/hardware/tici/amplifier.py +++ b/system/hardware/tici/amplifier.py @@ -1,5 +1,7 @@ +import time from smbus2 import SMBus from collections import namedtuple +from typing import List # https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf @@ -24,13 +26,10 @@ BASE_CONFIG = [ AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000), AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000), AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011), - AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), - AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000), AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000), AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100), AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100), - AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111), AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000), AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000), AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111), @@ -41,7 +40,6 @@ BASE_CONFIG = [ AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000), AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000), AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001), - AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010), AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000), AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111), AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000), @@ -64,6 +62,11 @@ BASE_CONFIG = [ CONFIGS = { "tici": [ + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111), + AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010), + *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)), *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)), *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)), @@ -72,11 +75,13 @@ CONFIGS = { ], "tizi": [ AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111), + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011), - AmpConfig("Left speaker output volume", 0x1F, 0x3D, 0, 0b00011111), - AmpConfig("Right speaker output volume", 0x1F, 0x3E, 0, 0b00011111), - AmpConfig("DAI1 attenuation (DV1)", 0x4, 0x2F, 0, 0b00001111), - AmpConfig("DAI2 attenuation (DV2)", 0x4, 0x31, 0, 0b00001111), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111), + AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111), + + AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010), AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001), AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000), AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111), @@ -91,12 +96,6 @@ CONFIGS = { AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010), AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001), AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000), - - *configs_from_eq_params(0x84, EQParams(0x3084, 0xC023, 0x3D60, 0x042B, 0x1222)), - *configs_from_eq_params(0x8E, EQParams(0x2FB2, 0xC05C, 0x3BD3, 0x06C5, 0x16BB)), - *configs_from_eq_params(0x98, EQParams(0x21F5, 0xDF73, 0x2DFE, 0x371A, 0x2C80)), - *configs_from_eq_params(0xA2, EQParams(0x2A5A, 0x0AD0, 0x14FA, 0x3F14, 0x3C76)), - *configs_from_eq_params(0xAC, EQParams(0x1577, 0x3FAE, 0xEE60, 0x0664, 0x3D86)), ], } @@ -107,28 +106,51 @@ class Amplifier: def __init__(self, debug=False): self.debug = debug - def set_config(self, config): + def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig: + return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000) + + def _set_configs(self, configs: List[AmpConfig]) -> None: with SMBus(self.AMP_I2C_BUS) as bus: - if self.debug: - print(f"Setting \"{config.name}\" to {config.value}:") + for config in configs: + if self.debug: + print(f"Setting \"{config.name}\" to {config.value}:") - old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True) - new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask) - bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True) + old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True) + new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask) + bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True) - if self.debug: - print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}") + if self.debug: + print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}") - def set_global_shutdown(self, amp_disabled): - self.set_config(AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000)) + def set_configs(self, configs: List[AmpConfig]) -> bool: + # retry in case panda is using the amp + tries = 15 + for i in range(15): + try: + self._set_configs(configs) + return True + except OSError: + print(f"Failed to set amp config, {tries - i - 1} retries left") + time.sleep(0.02) + return False - def initialize_configuration(self, model): - self.set_global_shutdown(amp_disabled=True) + def set_global_shutdown(self, amp_disabled: bool) -> bool: + return self.set_configs([self._get_shutdown_config(amp_disabled), ]) - for config in BASE_CONFIG: - self.set_config(config) + def initialize_configuration(self, model: str) -> bool: + cfgs = [ + self._get_shutdown_config(True), + *BASE_CONFIG, + *CONFIGS[model], + self._get_shutdown_config(False), + ] + return self.set_configs(cfgs) - for config in CONFIGS[model]: - self.set_config(config) - self.set_global_shutdown(amp_disabled=False) +if __name__ == "__main__": + with open("/sys/firmware/devicetree/base/model") as f: + model = f.read().strip('\x00') + model = model.split('comma ')[-1] + + amp = Amplifier() + amp.initialize_configuration(model) diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index 5f6fb2dc5..fba67d96c 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -16,8 +16,16 @@ public: static std::string get_os_version() { return "AGNOS " + util::read_file("/VERSION"); }; - static std::string get_name() { return "tici"; }; - static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::TICI; }; + + static std::string get_name() { + std::string devicetree_model = util::read_file("/sys/firmware/devicetree/base/model"); + return (devicetree_model.find("tizi") != std::string::npos) ? "tizi" : "tici"; + }; + + static cereal::InitData::DeviceType get_device_type() { + return (get_name() == "tizi") ? cereal::InitData::DeviceType::TIZI : cereal::InitData::DeviceType::TICI; + }; + static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }; static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }; @@ -65,6 +73,31 @@ public: std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); } + + static std::map get_init_logs() { + std::map ret = { + {"/BUILD", util::read_file("/BUILD")}, + }; + + std::string bs = util::check_output("abctl --boot_slot"); + ret["boot slot"] = bs.substr(0, bs.find_first_of("\n")); + + std::string temp = util::read_file("/dev/disk/by-partlabel/ssd"); + temp.erase(temp.find_last_not_of(std::string("\0\r\n", 3))+1); + ret["boot temp"] = temp; + + // TODO: log something from system and boot + for (std::string part : {"xbl", "abl", "aop", "devcfg", "xbl_config"}) { + for (std::string slot : {"a", "b"}) { + std::string partition = part + "_" + slot; + std::string hash = util::check_output("sha256sum /dev/disk/by-partlabel/" + partition); + ret[partition] = hash.substr(0, hash.find_first_of(" ")); + } + } + + return ret; + } + static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }; static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }; }; diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 9c1cc930c..d52710e95 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -4,11 +4,11 @@ import os import subprocess import time from enum import IntEnum -from functools import cached_property +from functools import cached_property, lru_cache from pathlib import Path from cereal import log -from common.gpio import gpio_set, gpio_init +from common.gpio import gpio_set, gpio_init, get_irq_for_action from system.hardware.base import HardwareBase, ThermalConfig from system.hardware.tici import iwlist from system.hardware.tici.pins import GPIO @@ -63,8 +63,14 @@ MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14 def sudo_write(val, path): os.system(f"sudo su -c 'echo {val} > {path}'") -def affine_irq(val, irq): - sudo_write(str(val), f"/proc/irq/{irq}/smp_affinity_list") + +def affine_irq(val, action): + irq = get_irq_for_action(action) + if len(irq) == 0: + print(f"No IRQs found for '{action}'") + return + for i in irq: + sudo_write(str(val), f"/proc/irq/{i}/smp_affinity_list") class Tici(HardwareBase): @@ -85,8 +91,12 @@ class Tici(HardwareBase): def amplifier(self): return Amplifier() - @cached_property - def model(self): + def get_os_version(self): + with open("/VERSION") as f: + return f.read().strip() + + @lru_cache + def get_device_type(self): with open("/sys/firmware/devicetree/base/model") as f: model = f.read().strip('\x00') model = model.split('comma ')[-1] @@ -95,13 +105,6 @@ class Tici(HardwareBase): model = 'tici' return model - def get_os_version(self): - with open("/VERSION") as f: - return f.read().strip() - - def get_device_type(self): - return "tici" - def get_sound_card_online(self): return (os.path.isfile('/proc/asound/card0/state') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') @@ -418,7 +421,7 @@ class Tici(HardwareBase): # amplifier, 100mW at idle self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) if not powersave_enabled: - self.amplifier.initialize_configuration(self.model) + self.amplifier.initialize_configuration(self.get_device_type()) # *** CPU config *** @@ -432,12 +435,20 @@ class Tici(HardwareBase): sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor') # *** IRQ config *** - affine_irq(5, 565) # kgsl-3d0 - affine_irq(4, 126) # SPI goes on boardd core - affine_irq(4, 740) # xhci-hcd:usb1 goes on the boardd core - affine_irq(4, 1069) # xhci-hcd:usb3 goes on the boardd core - for irq in range(237, 246): - affine_irq(5, irq) # camerad + + # GPU + affine_irq(5, "kgsl-3d0") + + # boardd core + affine_irq(4, "spi_geni") # SPI + affine_irq(4, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB) + if "tici" in self.get_device_type(): + affine_irq(4, "xhci-hcd:usb1") # internal panda USB + + # camerad core + camera_irqs = ("cci", "cpas_camnoc", "cpas-cdm", "csid", "ife", "csid", "csid-lite", "ife-lite") + for n in camera_irqs: + affine_irq(5, n) def get_gpu_usage_percent(self): try: @@ -447,17 +458,23 @@ class Tici(HardwareBase): return 0 def initialize_hardware(self): - self.amplifier.initialize_configuration(self.model) + self.amplifier.initialize_configuration(self.get_device_type()) # Allow thermald to write engagement status to kmsg os.system("sudo chmod a+w /dev/kmsg") + # Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL + gpio_init(GPIO.SOM_ST_IO, True) + gpio_set(GPIO.SOM_ST_IO, 1) + # *** IRQ config *** # move these off the default core - affine_irq(1, 7) # msm_drm - affine_irq(1, 250) # msm_vidc - affine_irq(1, 8) # i2c_geni (sensord) + affine_irq(1, "msm_drm") + affine_irq(1, "msm_vidc") + affine_irq(1, "i2c_geni") + + # mask off big cluster from default affinity sudo_write("f", "/proc/irq/default_smp_affinity") # *** GPU config *** @@ -543,6 +560,9 @@ class Tici(HardwareBase): except Exception: return -1, -1 + def has_internal_panda(self): + return True + def reset_internal_panda(self): gpio_init(GPIO.STM_RST_N, True) @@ -556,8 +576,9 @@ class Tici(HardwareBase): gpio_set(GPIO.STM_RST_N, 1) gpio_set(GPIO.STM_BOOT0, 1) - time.sleep(2) + time.sleep(1) gpio_set(GPIO.STM_RST_N, 0) + time.sleep(1) gpio_set(GPIO.STM_BOOT0, 0) diff --git a/system/hardware/tici/pins.py b/system/hardware/tici/pins.py index fe31b9311..082a402f1 100644 --- a/system/hardware/tici/pins.py +++ b/system/hardware/tici/pins.py @@ -10,6 +10,8 @@ class GPIO: STM_RST_N = 124 STM_BOOT0 = 134 + SOM_ST_IO = 49 + LTE_RST_N = 50 LTE_PWRKEY = 116 LTE_BOOT = 52 diff --git a/selfdrive/loggerd/.gitignore b/system/loggerd/.gitignore similarity index 100% rename from selfdrive/loggerd/.gitignore rename to system/loggerd/.gitignore diff --git a/selfdrive/loggerd/SConscript b/system/loggerd/SConscript similarity index 100% rename from selfdrive/loggerd/SConscript rename to system/loggerd/SConscript diff --git a/selfdrive/loggerd/__init__.py b/system/loggerd/__init__.py similarity index 100% rename from selfdrive/loggerd/__init__.py rename to system/loggerd/__init__.py diff --git a/selfdrive/loggerd/bootlog.cc b/system/loggerd/bootlog.cc similarity index 86% rename from selfdrive/loggerd/bootlog.cc rename to system/loggerd/bootlog.cc index e882e4cf8..becd293c0 100644 --- a/selfdrive/loggerd/bootlog.cc +++ b/system/loggerd/bootlog.cc @@ -2,8 +2,9 @@ #include #include "cereal/messaging/messaging.h" +#include "common/params.h" #include "common/swaglog.h" -#include "selfdrive/loggerd/logger.h" +#include "system/loggerd/logger.h" static kj::Array build_boot_log() { @@ -48,7 +49,8 @@ static kj::Array build_boot_log() { } int main(int argc, char** argv) { - const std::string path = LOG_ROOT + "/boot/" + logger_get_route_name(); + const std::string timestr = logger_get_route_name(); + const std::string path = LOG_ROOT + "/boot/" + timestr; LOGW("bootlog to %s", path.c_str()); // Open bootlog @@ -61,5 +63,8 @@ int main(int argc, char** argv) { // Write bootlog file.write(build_boot_log().asBytes()); + // Write out bootlog param to match routes with bootlog + Params().put("CurrentBootlog", timestr.c_str()); + return 0; } diff --git a/selfdrive/loggerd/config.py b/system/loggerd/config.py similarity index 100% rename from selfdrive/loggerd/config.py rename to system/loggerd/config.py diff --git a/selfdrive/loggerd/deleter.py b/system/loggerd/deleter.py similarity index 89% rename from selfdrive/loggerd/deleter.py rename to system/loggerd/deleter.py index 560628802..5fb2b9eb4 100644 --- a/selfdrive/loggerd/deleter.py +++ b/system/loggerd/deleter.py @@ -3,8 +3,8 @@ import os import shutil import threading from system.swaglog import cloudlog -from selfdrive.loggerd.config import ROOT, get_available_bytes, get_available_percent -from selfdrive.loggerd.uploader import listdir_by_creation +from system.loggerd.config import ROOT, get_available_bytes, get_available_percent +from system.loggerd.uploader import listdir_by_creation MIN_BYTES = 5 * 1024 * 1024 * 1024 MIN_PERCENT = 10 diff --git a/selfdrive/loggerd/encoder/encoder.cc b/system/loggerd/encoder/encoder.cc similarity index 98% rename from selfdrive/loggerd/encoder/encoder.cc rename to system/loggerd/encoder/encoder.cc index 943f37803..08eaf0ff0 100644 --- a/selfdrive/loggerd/encoder/encoder.cc +++ b/system/loggerd/encoder/encoder.cc @@ -1,5 +1,5 @@ #include -#include "selfdrive/loggerd/encoder/encoder.h" +#include "system/loggerd/encoder/encoder.h" VideoEncoder::~VideoEncoder() {} diff --git a/selfdrive/loggerd/encoder/encoder.h b/system/loggerd/encoder/encoder.h similarity index 97% rename from selfdrive/loggerd/encoder/encoder.h rename to system/loggerd/encoder/encoder.h index 21ef65cf1..88dd5cee2 100644 --- a/selfdrive/loggerd/encoder/encoder.h +++ b/system/loggerd/encoder/encoder.h @@ -7,7 +7,7 @@ #include "cereal/messaging/messaging.h" #include "cereal/visionipc/visionipc.h" #include "common/queue.h" -#include "selfdrive/loggerd/video_writer.h" +#include "system/loggerd/video_writer.h" #include "system/camerad/cameras/camera_common.h" #define V4L2_BUF_FLAG_KEYFRAME 8 diff --git a/selfdrive/loggerd/encoder/ffmpeg_encoder.cc b/system/loggerd/encoder/ffmpeg_encoder.cc similarity index 98% rename from selfdrive/loggerd/encoder/ffmpeg_encoder.cc rename to system/loggerd/encoder/ffmpeg_encoder.cc index 5f8d140e8..275da34f1 100644 --- a/selfdrive/loggerd/encoder/ffmpeg_encoder.cc +++ b/system/loggerd/encoder/ffmpeg_encoder.cc @@ -1,6 +1,6 @@ #pragma clang diagnostic ignored "-Wdeprecated-declarations" -#include "selfdrive/loggerd/encoder/ffmpeg_encoder.h" +#include "system/loggerd/encoder/ffmpeg_encoder.h" #include #include diff --git a/selfdrive/loggerd/encoder/ffmpeg_encoder.h b/system/loggerd/encoder/ffmpeg_encoder.h similarity index 92% rename from selfdrive/loggerd/encoder/ffmpeg_encoder.h rename to system/loggerd/encoder/ffmpeg_encoder.h index 497a28b65..9095a6e81 100644 --- a/selfdrive/loggerd/encoder/ffmpeg_encoder.h +++ b/system/loggerd/encoder/ffmpeg_encoder.h @@ -11,8 +11,8 @@ extern "C" { #include } -#include "selfdrive/loggerd/encoder/encoder.h" -#include "selfdrive/loggerd/loggerd.h" +#include "system/loggerd/encoder/encoder.h" +#include "system/loggerd/loggerd.h" class FfmpegEncoder : public VideoEncoder { public: diff --git a/selfdrive/loggerd/encoder/v4l_encoder.cc b/system/loggerd/encoder/v4l_encoder.cc similarity index 99% rename from selfdrive/loggerd/encoder/v4l_encoder.cc rename to system/loggerd/encoder/v4l_encoder.cc index 88aeb2125..16e7246ff 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.cc +++ b/system/loggerd/encoder/v4l_encoder.cc @@ -2,7 +2,7 @@ #include #include -#include "selfdrive/loggerd/encoder/v4l_encoder.h" +#include "system/loggerd/encoder/v4l_encoder.h" #include "common/util.h" #include "common/timing.h" diff --git a/selfdrive/loggerd/encoder/v4l_encoder.h b/system/loggerd/encoder/v4l_encoder.h similarity index 95% rename from selfdrive/loggerd/encoder/v4l_encoder.h rename to system/loggerd/encoder/v4l_encoder.h index c2a53dd6e..d4b0a1211 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.h +++ b/system/loggerd/encoder/v4l_encoder.h @@ -1,7 +1,7 @@ #pragma once #include "common/queue.h" -#include "selfdrive/loggerd/encoder/encoder.h" +#include "system/loggerd/encoder/encoder.h" #define BUF_IN_COUNT 7 #define BUF_OUT_COUNT 6 diff --git a/selfdrive/loggerd/encoderd.cc b/system/loggerd/encoderd.cc similarity index 99% rename from selfdrive/loggerd/encoderd.cc rename to system/loggerd/encoderd.cc index db5f4b61a..12b58b659 100644 --- a/selfdrive/loggerd/encoderd.cc +++ b/system/loggerd/encoderd.cc @@ -1,4 +1,4 @@ -#include "selfdrive/loggerd/loggerd.h" +#include "system/loggerd/loggerd.h" ExitHandler do_exit; diff --git a/selfdrive/loggerd/logger.cc b/system/loggerd/logger.cc similarity index 95% rename from selfdrive/loggerd/logger.cc rename to system/loggerd/logger.cc index aaf267e52..a3152aa88 100644 --- a/selfdrive/loggerd/logger.cc +++ b/system/loggerd/logger.cc @@ -1,4 +1,4 @@ -#include "selfdrive/loggerd/logger.h" +#include "system/loggerd/logger.h" #include #include @@ -70,7 +70,9 @@ kj::Array logger_build_init_data() { "df -h", // usage for all filesystems }; - auto commands = init.initCommands().initEntries(log_commands.size()); + auto hw_logs = Hardware::get_init_logs(); + + auto commands = init.initCommands().initEntries(log_commands.size() + hw_logs.size()); for (int i = 0; i < log_commands.size(); i++) { auto lentry = commands[i]; @@ -80,6 +82,14 @@ kj::Array logger_build_init_data() { lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size())); } + int i = log_commands.size(); + for (auto [key, value] : hw_logs) { + auto lentry = commands[i]; + lentry.setKey(key); + lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size())); + i++; + } + return capnp::messageToFlatArray(msg); } diff --git a/selfdrive/loggerd/logger.h b/system/loggerd/logger.h similarity index 100% rename from selfdrive/loggerd/logger.h rename to system/loggerd/logger.h diff --git a/selfdrive/loggerd/loggerd.cc b/system/loggerd/loggerd.cc similarity index 99% rename from selfdrive/loggerd/loggerd.cc rename to system/loggerd/loggerd.cc index e09cdfaa9..a7f7db480 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/system/loggerd/loggerd.cc @@ -1,5 +1,5 @@ -#include "selfdrive/loggerd/loggerd.h" -#include "selfdrive/loggerd/video_writer.h" +#include "system/loggerd/loggerd.h" +#include "system/loggerd/video_writer.h" ExitHandler do_exit; diff --git a/selfdrive/loggerd/loggerd.h b/system/loggerd/loggerd.h similarity index 92% rename from selfdrive/loggerd/loggerd.h rename to system/loggerd/loggerd.h index 1fa634982..1b8f9e0d2 100644 --- a/selfdrive/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -23,13 +23,13 @@ #include "common/util.h" #include "system/hardware/hw.h" -#include "selfdrive/loggerd/encoder/encoder.h" -#include "selfdrive/loggerd/logger.h" +#include "system/loggerd/encoder/encoder.h" +#include "system/loggerd/logger.h" #ifdef QCOM2 -#include "selfdrive/loggerd/encoder/v4l_encoder.h" +#include "system/loggerd/encoder/v4l_encoder.h" #define Encoder V4LEncoder #else -#include "selfdrive/loggerd/encoder/ffmpeg_encoder.h" +#include "system/loggerd/encoder/ffmpeg_encoder.h" #define Encoder FfmpegEncoder #endif diff --git a/selfdrive/loggerd/uploader.py b/system/loggerd/uploader.py similarity index 78% rename from selfdrive/loggerd/uploader.py rename to system/loggerd/uploader.py index f97bafecb..245e5cbcf 100644 --- a/selfdrive/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -9,6 +9,7 @@ import threading import time import traceback from pathlib import Path +from typing import BinaryIO, Iterator, List, Optional, Tuple, Union from cereal import log import cereal.messaging as messaging @@ -16,8 +17,8 @@ from common.api import Api from common.params import Params from common.realtime import set_core_affinity from system.hardware import TICI -from selfdrive.loggerd.xattr_cache import getxattr, setxattr -from selfdrive.loggerd.config import ROOT +from system.loggerd.xattr_cache import getxattr, setxattr +from system.loggerd.config import ROOT from system.swaglog import cloudlog NetworkType = log.DeviceState.NetworkType @@ -31,10 +32,23 @@ force_wifi = os.getenv("FORCEWIFI") is not None fake_upload = os.getenv("FAKEUPLOAD") is not None -def get_directory_sort(d): +class FakeRequest: + def __init__(self): + self.headers = {"Content-Length": "0"} + + +class FakeResponse: + def __init__(self): + self.status_code = 200 + self.request = FakeRequest() + + +UploadResponse = Union[requests.Response, FakeResponse] + +def get_directory_sort(d: str) -> List[str]: return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1))) -def listdir_by_creation(d): +def listdir_by_creation(d: str) -> List[str]: try: paths = os.listdir(d) paths = sorted(paths, key=get_directory_sort) @@ -43,7 +57,7 @@ def listdir_by_creation(d): cloudlog.exception("listdir_by_creation failed") return list() -def clear_locks(root): +def clear_locks(root: str) -> None: for logname in os.listdir(root): path = os.path.join(root, logname) try: @@ -54,16 +68,14 @@ def clear_locks(root): cloudlog.exception("clear_locks failed") -class Uploader(): - def __init__(self, dongle_id, root): +class Uploader: + def __init__(self, dongle_id: str, root: str): self.dongle_id = dongle_id self.api = Api(dongle_id) self.root = root - self.upload_thread = None - - self.last_resp = None - self.last_exc = None + self.last_resp: Optional[UploadResponse] = None + self.last_exc: Optional[Tuple[Exception, str]] = None self.immediate_size = 0 self.immediate_count = 0 @@ -76,12 +88,12 @@ class Uploader(): self.immediate_folders = ["crash/", "boot/"] self.immediate_priority = {"qlog": 0, "qlog.bz2": 0, "qcamera.ts": 1} - def get_upload_sort(self, name): + def get_upload_sort(self, name: str) -> int: if name in self.immediate_priority: return self.immediate_priority[name] return 1000 - def list_upload_files(self): + def list_upload_files(self) -> Iterator[Tuple[str, str, str]]: if not os.path.isdir(self.root): return @@ -103,7 +115,7 @@ class Uploader(): fn = os.path.join(path, name) # skip files already uploaded try: - is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) + is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE except OSError: cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn) is_uploaded = True # deleter could have deleted @@ -117,22 +129,22 @@ class Uploader(): except OSError: pass - yield (name, key, fn) + yield name, key, fn - def next_file_to_upload(self): + def next_file_to_upload(self) -> Optional[Tuple[str, str, str]]: upload_files = list(self.list_upload_files()) for name, key, fn in upload_files: if any(f in fn for f in self.immediate_folders): - return (name, key, fn) + return name, key, fn for name, key, fn in upload_files: if name in self.immediate_priority: - return (name, key, fn) + return name, key, fn return None - def do_upload(self, key, fn): + def do_upload(self, key: str, fn: str) -> None: try: url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token()) if url_resp.status_code == 412: @@ -146,17 +158,13 @@ class Uploader(): if fake_upload: cloudlog.debug(f"*** WARNING, THIS IS A FAKE UPLOAD TO {url} ***") - - class FakeResponse(): - def __init__(self): - self.status_code = 200 - self.last_resp = FakeResponse() else: with open(fn, "rb") as f: + data: BinaryIO if key.endswith('.bz2') and not fn.endswith('.bz2'): - data = bz2.compress(f.read()) - data = io.BytesIO(data) + compressed = bz2.compress(f.read()) + data = io.BytesIO(compressed) else: data = f @@ -165,7 +173,7 @@ class Uploader(): self.last_exc = (e, traceback.format_exc()) raise - def normal_upload(self, key, fn): + def normal_upload(self, key: str, fn: str) -> Optional[UploadResponse]: self.last_resp = None self.last_exc = None @@ -176,7 +184,7 @@ class Uploader(): return self.last_resp - def upload(self, name, key, fn, network_type, metered): + def upload(self, name: str, key: str, fn: str, network_type: int, metered: bool) -> bool: try: sz = os.path.getsize(fn) except OSError: @@ -197,9 +205,14 @@ class Uploader(): if stat is not None and stat.status_code in (200, 201, 401, 403, 412): self.last_filename = fn self.last_time = time.monotonic() - start_time - self.last_speed = (sz / 1e6) / self.last_time + if stat.status_code == 412: + self.last_speed = 0 + cloudlog.event("upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) + else: + content_length = int(stat.request.headers.get("Content-Length", 0)) + self.last_speed = (content_length / 1e6) / self.last_time + cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length, network_type=network_type, metered=metered, speed=self.last_speed) success = True - cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) else: success = False cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) @@ -224,7 +237,7 @@ class Uploader(): return msg -def uploader_fn(exit_event): +def uploader_fn(exit_event: threading.Event) -> None: try: set_core_affinity([0, 1, 2, 3]) except Exception: @@ -279,7 +292,7 @@ def uploader_fn(exit_event): pm.send("uploaderState", uploader.get_msg()) -def main(): +def main() -> None: uploader_fn(threading.Event()) diff --git a/selfdrive/loggerd/video_writer.cc b/system/loggerd/video_writer.cc similarity index 98% rename from selfdrive/loggerd/video_writer.cc rename to system/loggerd/video_writer.cc index 4f79ccafc..91bf09355 100644 --- a/selfdrive/loggerd/video_writer.cc +++ b/system/loggerd/video_writer.cc @@ -2,7 +2,7 @@ #include #include -#include "selfdrive/loggerd/video_writer.h" +#include "system/loggerd/video_writer.h" #include "common/swaglog.h" #include "common/util.h" diff --git a/selfdrive/loggerd/video_writer.h b/system/loggerd/video_writer.h similarity index 100% rename from selfdrive/loggerd/video_writer.h rename to system/loggerd/video_writer.h diff --git a/selfdrive/loggerd/xattr_cache.py b/system/loggerd/xattr_cache.py similarity index 100% rename from selfdrive/loggerd/xattr_cache.py rename to system/loggerd/xattr_cache.py diff --git a/system/sensord/.gitignore b/system/sensord/.gitignore new file mode 100644 index 000000000..e9b8071b4 --- /dev/null +++ b/system/sensord/.gitignore @@ -0,0 +1 @@ +_sensord diff --git a/selfdrive/sensord/SConscript b/system/sensord/SConscript similarity index 100% rename from selfdrive/sensord/SConscript rename to system/sensord/SConscript diff --git a/selfdrive/sensord/pigeond.py b/system/sensord/pigeond.py similarity index 99% rename from selfdrive/sensord/pigeond.py rename to system/sensord/pigeond.py index 9d0a62bd3..c9ad7ff22 100755 --- a/selfdrive/sensord/pigeond.py +++ b/system/sensord/pigeond.py @@ -183,6 +183,7 @@ def initialize_pigeon(pigeon: TTYPigeon) -> bool: pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C") pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70") pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x35\x01\x41\xAD") cloudlog.debug("pigeon configured") # try restoring almanac backup diff --git a/selfdrive/sensord/rawgps/compare.py b/system/sensord/rawgps/compare.py similarity index 100% rename from selfdrive/sensord/rawgps/compare.py rename to system/sensord/rawgps/compare.py diff --git a/selfdrive/sensord/rawgps/modemdiag.py b/system/sensord/rawgps/modemdiag.py similarity index 100% rename from selfdrive/sensord/rawgps/modemdiag.py rename to system/sensord/rawgps/modemdiag.py diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/system/sensord/rawgps/rawgpsd.py similarity index 97% rename from selfdrive/sensord/rawgps/rawgpsd.py rename to system/sensord/rawgps/rawgpsd.py index 3fa5e927a..8f243af9a 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/system/sensord/rawgps/rawgpsd.py @@ -15,8 +15,8 @@ from common.gpio import gpio_init, gpio_set from laika.gps_time import GPSTime from system.hardware.tici.pins import GPIO from system.swaglog import cloudlog -from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv -from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist, +from system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv +from system.sensord.rawgps.structs import (dict_unpacker, position_report, relist, gps_measurement_report, gps_measurement_report_sv, glonass_measurement_report, glonass_measurement_report_sv, oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report, @@ -270,7 +270,6 @@ def main() -> NoReturn: msg = messaging.new_message('gpsLocation') gps = msg.gpsLocation - gps.flags = 1 gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi gps.altitude = report["q_FltFinalPosAlt"] @@ -283,6 +282,8 @@ def main() -> NoReturn: gps.verticalAccuracy = report["q_FltVdop"] gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma])) + # quectel gps verticalAccuracy is clipped to 500, set invalid if so + gps.flags = 1 if gps.verticalAccuracy != 500 else 0 pm.send('gpsLocation', msg) diff --git a/selfdrive/sensord/rawgps/structs.py b/system/sensord/rawgps/structs.py similarity index 100% rename from selfdrive/sensord/rawgps/structs.py rename to system/sensord/rawgps/structs.py diff --git a/selfdrive/sensord/rawgps/test_rawgps.py b/system/sensord/rawgps/test_rawgps.py similarity index 100% rename from selfdrive/sensord/rawgps/test_rawgps.py rename to system/sensord/rawgps/test_rawgps.py diff --git a/selfdrive/sensord/sensord b/system/sensord/sensord similarity index 100% rename from selfdrive/sensord/sensord rename to system/sensord/sensord diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/system/sensord/sensors/bmx055_accel.cc similarity index 100% rename from selfdrive/sensord/sensors/bmx055_accel.cc rename to system/sensord/sensors/bmx055_accel.cc diff --git a/selfdrive/sensord/sensors/bmx055_accel.h b/system/sensord/sensors/bmx055_accel.h similarity index 96% rename from selfdrive/sensord/sensors/bmx055_accel.h rename to system/sensord/sensors/bmx055_accel.h index 8ef660a99..2cc316e99 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.h +++ b/system/sensord/sensors/bmx055_accel.h @@ -1,6 +1,6 @@ #pragma once -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define BMX055_ACCEL_I2C_ADDR 0x18 diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/system/sensord/sensors/bmx055_gyro.cc similarity index 100% rename from selfdrive/sensord/sensors/bmx055_gyro.cc rename to system/sensord/sensors/bmx055_gyro.cc diff --git a/selfdrive/sensord/sensors/bmx055_gyro.h b/system/sensord/sensors/bmx055_gyro.h similarity index 95% rename from selfdrive/sensord/sensors/bmx055_gyro.h rename to system/sensord/sensors/bmx055_gyro.h index 80b93f128..7be3e5656 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.h +++ b/system/sensord/sensors/bmx055_gyro.h @@ -1,6 +1,6 @@ #pragma once -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define BMX055_GYRO_I2C_ADDR 0x68 diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc similarity index 100% rename from selfdrive/sensord/sensors/bmx055_magn.cc rename to system/sensord/sensors/bmx055_magn.cc diff --git a/selfdrive/sensord/sensors/bmx055_magn.h b/system/sensord/sensors/bmx055_magn.h similarity index 97% rename from selfdrive/sensord/sensors/bmx055_magn.h rename to system/sensord/sensors/bmx055_magn.h index e4a79bc7e..15c4e734b 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.h +++ b/system/sensord/sensors/bmx055_magn.h @@ -1,7 +1,7 @@ #pragma once #include -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define BMX055_MAGN_I2C_ADDR 0x10 diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/system/sensord/sensors/bmx055_temp.cc similarity index 94% rename from selfdrive/sensord/sensors/bmx055_temp.cc rename to system/sensord/sensors/bmx055_temp.cc index 95b8068ac..68ee0da1d 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.cc +++ b/system/sensord/sensors/bmx055_temp.cc @@ -2,7 +2,7 @@ #include -#include "selfdrive/sensord/sensors/bmx055_accel.h" +#include "system/sensord/sensors/bmx055_accel.h" #include "common/swaglog.h" #include "common/timing.h" diff --git a/selfdrive/sensord/sensors/bmx055_temp.h b/system/sensord/sensors/bmx055_temp.h similarity index 71% rename from selfdrive/sensord/sensors/bmx055_temp.h rename to system/sensord/sensors/bmx055_temp.h index 0b6802dea..a2eabae39 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.h +++ b/system/sensord/sensors/bmx055_temp.h @@ -1,7 +1,7 @@ #pragma once -#include "selfdrive/sensord/sensors/bmx055_accel.h" -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/bmx055_accel.h" +#include "system/sensord/sensors/i2c_sensor.h" class BMX055_Temp : public I2CSensor { uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} diff --git a/selfdrive/sensord/sensors/constants.h b/system/sensord/sensors/constants.h similarity index 100% rename from selfdrive/sensord/sensors/constants.h rename to system/sensord/sensors/constants.h diff --git a/selfdrive/sensord/sensors/file_sensor.cc b/system/sensord/sensors/file_sensor.cc similarity index 100% rename from selfdrive/sensord/sensors/file_sensor.cc rename to system/sensord/sensors/file_sensor.cc diff --git a/selfdrive/sensord/sensors/file_sensor.h b/system/sensord/sensors/file_sensor.h similarity index 88% rename from selfdrive/sensord/sensors/file_sensor.h rename to system/sensord/sensors/file_sensor.h index 39d695167..07d7e8f94 100644 --- a/selfdrive/sensord/sensors/file_sensor.h +++ b/system/sensord/sensors/file_sensor.h @@ -4,7 +4,7 @@ #include #include "cereal/gen/cpp/log.capnp.h" -#include "selfdrive/sensord/sensors/sensor.h" +#include "system/sensord/sensors/sensor.h" class FileSensor : public Sensor { protected: diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/system/sensord/sensors/i2c_sensor.cc similarity index 100% rename from selfdrive/sensord/sensors/i2c_sensor.cc rename to system/sensord/sensors/i2c_sensor.cc diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/system/sensord/sensors/i2c_sensor.h similarity index 93% rename from selfdrive/sensord/sensors/i2c_sensor.h rename to system/sensord/sensors/i2c_sensor.h index 08ca6f09c..ccac526c1 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.h +++ b/system/sensord/sensors/i2c_sensor.h @@ -8,8 +8,8 @@ #include "common/gpio.h" #include "common/swaglog.h" -#include "selfdrive/sensord/sensors/constants.h" -#include "selfdrive/sensord/sensors/sensor.h" +#include "system/sensord/sensors/constants.h" +#include "system/sensord/sensors/sensor.h" int16_t read_12_bit(uint8_t lsb, uint8_t msb); int16_t read_16_bit(uint8_t lsb, uint8_t msb); diff --git a/selfdrive/sensord/sensors/light_sensor.cc b/system/sensord/sensors/light_sensor.cc similarity index 92% rename from selfdrive/sensord/sensors/light_sensor.cc rename to system/sensord/sensors/light_sensor.cc index 58c602ea3..99e321b47 100644 --- a/selfdrive/sensord/sensors/light_sensor.cc +++ b/system/sensord/sensors/light_sensor.cc @@ -3,7 +3,7 @@ #include #include "common/timing.h" -#include "selfdrive/sensord/sensors/constants.h" +#include "system/sensord/sensors/constants.h" LightSensor::LightSensor(std::string filename) : FileSensor(filename) {} diff --git a/selfdrive/sensord/sensors/light_sensor.h b/system/sensord/sensors/light_sensor.h similarity index 100% rename from selfdrive/sensord/sensors/light_sensor.h rename to system/sensord/sensors/light_sensor.h diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/system/sensord/sensors/lsm6ds3_accel.cc similarity index 100% rename from selfdrive/sensord/sensors/lsm6ds3_accel.cc rename to system/sensord/sensors/lsm6ds3_accel.cc diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.h b/system/sensord/sensors/lsm6ds3_accel.h similarity index 97% rename from selfdrive/sensord/sensors/lsm6ds3_accel.h rename to system/sensord/sensors/lsm6ds3_accel.h index c3f66f580..69667cb75 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.h +++ b/system/sensord/sensors/lsm6ds3_accel.h @@ -1,6 +1,6 @@ #pragma once -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define LSM6DS3_ACCEL_I2C_ADDR 0x6A diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/system/sensord/sensors/lsm6ds3_gyro.cc similarity index 100% rename from selfdrive/sensord/sensors/lsm6ds3_gyro.cc rename to system/sensord/sensors/lsm6ds3_gyro.cc diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.h b/system/sensord/sensors/lsm6ds3_gyro.h similarity index 96% rename from selfdrive/sensord/sensors/lsm6ds3_gyro.h rename to system/sensord/sensors/lsm6ds3_gyro.h index 220e6b0ce..adaae62dd 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.h +++ b/system/sensord/sensors/lsm6ds3_gyro.h @@ -1,6 +1,6 @@ #pragma once -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define LSM6DS3_GYRO_I2C_ADDR 0x6A diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/system/sensord/sensors/lsm6ds3_temp.cc similarity index 100% rename from selfdrive/sensord/sensors/lsm6ds3_temp.cc rename to system/sensord/sensors/lsm6ds3_temp.cc diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.h b/system/sensord/sensors/lsm6ds3_temp.h similarity index 92% rename from selfdrive/sensord/sensors/lsm6ds3_temp.h rename to system/sensord/sensors/lsm6ds3_temp.h index 1d6bcc228..1b5b62181 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.h +++ b/system/sensord/sensors/lsm6ds3_temp.h @@ -1,6 +1,6 @@ #pragma once -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define LSM6DS3_TEMP_I2C_ADDR 0x6A diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/system/sensord/sensors/mmc5603nj_magn.cc similarity index 100% rename from selfdrive/sensord/sensors/mmc5603nj_magn.cc rename to system/sensord/sensors/mmc5603nj_magn.cc diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.h b/system/sensord/sensors/mmc5603nj_magn.h similarity index 94% rename from selfdrive/sensord/sensors/mmc5603nj_magn.h rename to system/sensord/sensors/mmc5603nj_magn.h index a364c7c37..fce3f3fec 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.h +++ b/system/sensord/sensors/mmc5603nj_magn.h @@ -1,6 +1,6 @@ #pragma once -#include "selfdrive/sensord/sensors/i2c_sensor.h" +#include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define MMC5603NJ_I2C_ADDR 0x30 diff --git a/selfdrive/sensord/sensors/sensor.h b/system/sensord/sensors/sensor.h similarity index 100% rename from selfdrive/sensord/sensors/sensor.h rename to system/sensord/sensors/sensor.h diff --git a/selfdrive/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc similarity index 90% rename from selfdrive/sensord/sensors_qcom2.cc rename to system/sensord/sensors_qcom2.cc index fc8dc8620..349c67f49 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/system/sensord/sensors_qcom2.cc @@ -12,17 +12,17 @@ #include "common/swaglog.h" #include "common/timing.h" #include "common/util.h" -#include "selfdrive/sensord/sensors/bmx055_accel.h" -#include "selfdrive/sensord/sensors/bmx055_gyro.h" -#include "selfdrive/sensord/sensors/bmx055_magn.h" -#include "selfdrive/sensord/sensors/bmx055_temp.h" -#include "selfdrive/sensord/sensors/constants.h" -#include "selfdrive/sensord/sensors/light_sensor.h" -#include "selfdrive/sensord/sensors/lsm6ds3_accel.h" -#include "selfdrive/sensord/sensors/lsm6ds3_gyro.h" -#include "selfdrive/sensord/sensors/lsm6ds3_temp.h" -#include "selfdrive/sensord/sensors/mmc5603nj_magn.h" -#include "selfdrive/sensord/sensors/sensor.h" +#include "system/sensord/sensors/bmx055_accel.h" +#include "system/sensord/sensors/bmx055_gyro.h" +#include "system/sensord/sensors/bmx055_magn.h" +#include "system/sensord/sensors/bmx055_temp.h" +#include "system/sensord/sensors/constants.h" +#include "system/sensord/sensors/light_sensor.h" +#include "system/sensord/sensors/lsm6ds3_accel.h" +#include "system/sensord/sensors/lsm6ds3_gyro.h" +#include "system/sensord/sensors/lsm6ds3_temp.h" +#include "system/sensord/sensors/mmc5603nj_magn.h" +#include "system/sensord/sensors/sensor.h" #define I2C_BUS_IMU 1 diff --git a/system/ubloxd/.gitignore b/system/ubloxd/.gitignore new file mode 100644 index 000000000..05263ff67 --- /dev/null +++ b/system/ubloxd/.gitignore @@ -0,0 +1,2 @@ +ubloxd +tests/test_glonass_runner diff --git a/system/ubloxd/SConscript b/system/ubloxd/SConscript new file mode 100644 index 000000000..fff0986ef --- /dev/null +++ b/system/ubloxd/SConscript @@ -0,0 +1,20 @@ +Import('env', 'common', 'cereal', 'messaging') + +loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread'] + +if GetOption('kaitai'): + generated = Dir('generated').srcnode().abspath + cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" + env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd) + env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) + glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd) + + # kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910 + patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES') + env.Depends(patch, glonass) + +glonass_obj = env.Object('generated/glonass.cpp') +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs) + +if GetOption('test'): + env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs]) \ No newline at end of file diff --git a/selfdrive/locationd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp similarity index 100% rename from selfdrive/locationd/generated/glonass.cpp rename to system/ubloxd/generated/glonass.cpp diff --git a/selfdrive/locationd/generated/glonass.h b/system/ubloxd/generated/glonass.h similarity index 100% rename from selfdrive/locationd/generated/glonass.h rename to system/ubloxd/generated/glonass.h diff --git a/selfdrive/locationd/generated/gps.cpp b/system/ubloxd/generated/gps.cpp similarity index 100% rename from selfdrive/locationd/generated/gps.cpp rename to system/ubloxd/generated/gps.cpp diff --git a/selfdrive/locationd/generated/gps.h b/system/ubloxd/generated/gps.h similarity index 100% rename from selfdrive/locationd/generated/gps.h rename to system/ubloxd/generated/gps.h diff --git a/selfdrive/locationd/generated/ubx.cpp b/system/ubloxd/generated/ubx.cpp similarity index 67% rename from selfdrive/locationd/generated/ubx.cpp rename to system/ubloxd/generated/ubx.cpp index 34fe1e52c..81b82ccaf 100644 --- a/selfdrive/locationd/generated/ubx.cpp +++ b/system/ubloxd/generated/ubx.cpp @@ -40,6 +40,11 @@ void ubx_t::_read() { m_body = new rxm_sfrbx_t(m__io, this, m__root); break; } + case 309: { + n_body = false; + m_body = new nav_sat_t(m__io, this, m__root); + break; + } case 2571: { n_body = false; m_body = new mon_hw2_t(m__io, this, m__root); @@ -70,9 +75,9 @@ void ubx_t::_clean_up() { ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; - m_measurements = 0; - m__raw_measurements = 0; - m__io__raw_measurements = 0; + m_meas = 0; + m__raw_meas = 0; + m__io__raw_meas = 0; try { _read(); @@ -89,15 +94,15 @@ void ubx_t::rxm_rawx_t::_read() { m_num_meas = m__io->read_u1(); m_rec_stat = m__io->read_u1(); m_reserved1 = m__io->read_bytes(3); - m__raw_measurements = new std::vector(); - m__io__raw_measurements = new std::vector(); - m_measurements = new std::vector(); - const int l_measurements = num_meas(); - for (int i = 0; i < l_measurements; i++) { - m__raw_measurements->push_back(m__io->read_bytes(32)); - kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1)); - m__io__raw_measurements->push_back(io__raw_measurements); - m_measurements->push_back(new meas_t(io__raw_measurements, this, m__root)); + m__raw_meas = new std::vector(); + m__io__raw_meas = new std::vector(); + m_meas = new std::vector(); + const int l_meas = num_meas(); + for (int i = 0; i < l_meas; i++) { + m__raw_meas->push_back(m__io->read_bytes(32)); + kaitai::kstream* io__raw_meas = new kaitai::kstream(m__raw_meas->at(m__raw_meas->size() - 1)); + m__io__raw_meas->push_back(io__raw_meas); + m_meas->push_back(new measurement_t(io__raw_meas, this, m__root)); } } @@ -106,24 +111,24 @@ ubx_t::rxm_rawx_t::~rxm_rawx_t() { } void ubx_t::rxm_rawx_t::_clean_up() { - if (m__raw_measurements) { - delete m__raw_measurements; m__raw_measurements = 0; + if (m__raw_meas) { + delete m__raw_meas; m__raw_meas = 0; } - if (m__io__raw_measurements) { - for (std::vector::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) { + if (m__io__raw_meas) { + for (std::vector::iterator it = m__io__raw_meas->begin(); it != m__io__raw_meas->end(); ++it) { delete *it; } - delete m__io__raw_measurements; m__io__raw_measurements = 0; + delete m__io__raw_meas; m__io__raw_meas = 0; } - if (m_measurements) { - for (std::vector::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) { + if (m_meas) { + for (std::vector::iterator it = m_meas->begin(); it != m_meas->end(); ++it) { delete *it; } - delete m_measurements; m_measurements = 0; + delete m_meas; m_meas = 0; } } -ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { +ubx_t::rxm_rawx_t::measurement_t::measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; @@ -135,7 +140,7 @@ ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__ } } -void ubx_t::rxm_rawx_t::meas_t::_read() { +void ubx_t::rxm_rawx_t::measurement_t::_read() { m_pr_mes = m__io->read_f8le(); m_cp_mes = m__io->read_f8le(); m_do_mes = m__io->read_f4le(); @@ -152,11 +157,11 @@ void ubx_t::rxm_rawx_t::meas_t::_read() { m_reserved3 = m__io->read_bytes(1); } -ubx_t::rxm_rawx_t::meas_t::~meas_t() { +ubx_t::rxm_rawx_t::measurement_t::~measurement_t() { _clean_up(); } -void ubx_t::rxm_rawx_t::meas_t::_clean_up() { +void ubx_t::rxm_rawx_t::measurement_t::_clean_up() { } ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { @@ -198,6 +203,89 @@ void ubx_t::rxm_sfrbx_t::_clean_up() { } } +ubx_t::nav_sat_t::nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + m_svs = 0; + m__raw_svs = 0; + m__io__raw_svs = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::nav_sat_t::_read() { + m_itow = m__io->read_u4le(); + m_version = m__io->read_u1(); + m_num_svs = m__io->read_u1(); + m_reserved = m__io->read_bytes(2); + m__raw_svs = new std::vector(); + m__io__raw_svs = new std::vector(); + m_svs = new std::vector(); + const int l_svs = num_svs(); + for (int i = 0; i < l_svs; i++) { + m__raw_svs->push_back(m__io->read_bytes(12)); + kaitai::kstream* io__raw_svs = new kaitai::kstream(m__raw_svs->at(m__raw_svs->size() - 1)); + m__io__raw_svs->push_back(io__raw_svs); + m_svs->push_back(new nav_t(io__raw_svs, this, m__root)); + } +} + +ubx_t::nav_sat_t::~nav_sat_t() { + _clean_up(); +} + +void ubx_t::nav_sat_t::_clean_up() { + if (m__raw_svs) { + delete m__raw_svs; m__raw_svs = 0; + } + if (m__io__raw_svs) { + for (std::vector::iterator it = m__io__raw_svs->begin(); it != m__io__raw_svs->end(); ++it) { + delete *it; + } + delete m__io__raw_svs; m__io__raw_svs = 0; + } + if (m_svs) { + for (std::vector::iterator it = m_svs->begin(); it != m_svs->end(); ++it) { + delete *it; + } + delete m_svs; m_svs = 0; + } +} + +ubx_t::nav_sat_t::nav_t::nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::nav_sat_t::nav_t::_read() { + m_gnss_id = static_cast(m__io->read_u1()); + m_sv_id = m__io->read_u1(); + m_cno = m__io->read_u1(); + m_elev = m__io->read_s1(); + m_azim = m__io->read_s2le(); + m_pr_res = m__io->read_s2le(); + m_flags = m__io->read_u4le(); +} + +ubx_t::nav_sat_t::nav_t::~nav_t() { + _clean_up(); +} + +void ubx_t::nav_sat_t::nav_t::_clean_up() { +} + ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; diff --git a/selfdrive/locationd/generated/ubx.h b/system/ubloxd/generated/ubx.h similarity index 80% rename from selfdrive/locationd/generated/ubx.h rename to system/ubloxd/generated/ubx.h index 6be4ce8c4..022108489 100644 --- a/selfdrive/locationd/generated/ubx.h +++ b/system/ubloxd/generated/ubx.h @@ -16,6 +16,7 @@ class ubx_t : public kaitai::kstruct { public: class rxm_rawx_t; class rxm_sfrbx_t; + class nav_sat_t; class nav_pvt_t; class mon_hw2_t; class mon_hw_t; @@ -42,7 +43,7 @@ public: class rxm_rawx_t : public kaitai::kstruct { public: - class meas_t; + class measurement_t; rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); @@ -53,18 +54,18 @@ public: public: ~rxm_rawx_t(); - class meas_t : public kaitai::kstruct { + class measurement_t : public kaitai::kstruct { public: - meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0); + measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0); private: void _read(); void _clean_up(); public: - ~meas_t(); + ~measurement_t(); private: double m_pr_mes; @@ -110,11 +111,11 @@ public: uint8_t m_num_meas; uint8_t m_rec_stat; std::string m_reserved1; - std::vector* m_measurements; + std::vector* m_meas; ubx_t* m__root; ubx_t* m__parent; - std::vector* m__raw_measurements; - std::vector* m__io__raw_measurements; + std::vector* m__raw_meas; + std::vector* m__io__raw_meas; public: double rcv_tow() const { return m_rcv_tow; } @@ -123,11 +124,11 @@ public: uint8_t num_meas() const { return m_num_meas; } uint8_t rec_stat() const { return m_rec_stat; } std::string reserved1() const { return m_reserved1; } - std::vector* measurements() const { return m_measurements; } + std::vector* meas() const { return m_meas; } ubx_t* _root() const { return m__root; } ubx_t* _parent() const { return m__parent; } - std::vector* _raw_measurements() const { return m__raw_measurements; } - std::vector* _io__raw_measurements() const { return m__io__raw_measurements; } + std::vector* _raw_meas() const { return m__raw_meas; } + std::vector* _io__raw_meas() const { return m__io__raw_meas; } }; class rxm_sfrbx_t : public kaitai::kstruct { @@ -170,6 +171,79 @@ public: ubx_t* _parent() const { return m__parent; } }; + class nav_sat_t : public kaitai::kstruct { + + public: + class nav_t; + + nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~nav_sat_t(); + + class nav_t : public kaitai::kstruct { + + public: + + nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~nav_t(); + + private: + gnss_type_t m_gnss_id; + uint8_t m_sv_id; + uint8_t m_cno; + int8_t m_elev; + int16_t m_azim; + int16_t m_pr_res; + uint32_t m_flags; + ubx_t* m__root; + ubx_t::nav_sat_t* m__parent; + + public: + gnss_type_t gnss_id() const { return m_gnss_id; } + uint8_t sv_id() const { return m_sv_id; } + uint8_t cno() const { return m_cno; } + int8_t elev() const { return m_elev; } + int16_t azim() const { return m_azim; } + int16_t pr_res() const { return m_pr_res; } + uint32_t flags() const { return m_flags; } + ubx_t* _root() const { return m__root; } + ubx_t::nav_sat_t* _parent() const { return m__parent; } + }; + + private: + uint32_t m_itow; + uint8_t m_version; + uint8_t m_num_svs; + std::string m_reserved; + std::vector* m_svs; + ubx_t* m__root; + ubx_t* m__parent; + std::vector* m__raw_svs; + std::vector* m__io__raw_svs; + + public: + uint32_t itow() const { return m_itow; } + uint8_t version() const { return m_version; } + uint8_t num_svs() const { return m_num_svs; } + std::string reserved() const { return m_reserved; } + std::vector* svs() const { return m_svs; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + std::vector* _raw_svs() const { return m__raw_svs; } + std::vector* _io__raw_svs() const { return m__io__raw_svs; } + }; + class nav_pvt_t : public kaitai::kstruct { public: diff --git a/selfdrive/locationd/ublox_msg.cc b/system/ubloxd/ublox_msg.cc similarity index 79% rename from selfdrive/locationd/ublox_msg.cc rename to system/ubloxd/ublox_msg.cc index b74698946..a01187d56 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/system/ubloxd/ublox_msg.cc @@ -65,23 +65,8 @@ inline bool UbloxMsgParser::valid_so_far() { return true; } -inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) { - // convert time to year (conversion from A3.1.3) - int J = 0; - if (1 <= Nt && Nt <= 366) { - J = 1; - } else if (367 <= Nt && Nt <= 731) { - J = 2; - } else if (732 <= Nt && Nt <= 1096) { - J = 3; - } else if (1097 <= Nt && Nt <= 1461) { - J = 4; - } - uint16_t year = 1996 + 4*(N4 -1) + (J - 1); - return year; -} - -bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { +bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { + last_log_time = log_time; int needed = needed_bytes(); if(needed > 0) { bytes_consumed = std::min((uint32_t)needed, incoming_data_len ); @@ -126,6 +111,9 @@ std::pair> UbloxMsgParser::gen_msg() { return {"ubloxGnss", gen_mon_hw(static_cast(body))}; case 0x0a0b: return {"ubloxGnss", gen_mon_hw2(static_cast(body))}; + case 0x0135: + // TODO return {"ubloxGnss", gen_nav_sat(static_cast(body))}; + return {"ubloxGnss", kj::Array()}; default: LOGE("Unknown message type %x", ubx_message.msg_type()); return {"ubloxGnss", kj::Array()}; @@ -183,7 +171,7 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m gps_t subframe(&stream); int subframe_id = subframe.how()->subframe_id(); - if (subframe_id > 3) { + if (subframe_id > 3 || subframe_id < 1) { // dont parse almanac subframes return kj::Array(); } @@ -199,6 +187,7 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m int iode_s2 = 0; int iode_s3 = 0; int iodc_lsb = 0; + int week; // Subframe 1 { @@ -206,7 +195,14 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m gps_t subframe(&stream); gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); - eph.setGpsWeek(subframe_1->week_no()); + // Each message is incremented to be greater or equal than week 1877 (2015-12-27). + // To skip this use the current_time argument + week = subframe_1->week_no(); + week += 1024; + if (week < 1877) { + week += 1024; + } + //eph.setGpsWeek(subframe_1->week_no()); eph.setTgd(subframe_1->t_gd() * pow(2, -31)); eph.setToc(subframe_1->t_oc() * pow(2, 4)); eph.setAf2(subframe_1->af_2() * pow(2, -55)); @@ -223,6 +219,12 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m gps_t subframe(&stream); gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); + // GPS week refers to current week, the ephemeris can be valid for the next + // if toe equals 0, this can be verified by the TOW count if it is within the + // last 2 hours of the week (gps ephemeris valid for 4hours) + if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){ + week += 1; + } eph.setCrs(subframe_2->c_rs() * pow(2, -5)); eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); @@ -252,6 +254,9 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m iode_s3 = subframe_3->iode(); } + eph.setToeWeek(week); + eph.setTocWeek(week); + gps_subframes[msg->sv_id()].clear(); if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) { // data set cutover, reject ephemeris @@ -263,12 +268,8 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m } kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) { - if (msg->sv_id() == 255) { - // data can be decoded before identifying the SV number, in this case 255 - // is returned, which means "unknown" (ublox p32) - return kj::Array(); - } - + // This parser assumes that no 2 satellites of the same frequency + // can be in view at the same time auto body = *msg->body(); assert(body.size() == 4); { @@ -281,39 +282,68 @@ kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ kaitai::kstream stream(string_data); glonass_t gl_string(&stream); - int string_number = gl_string.string_number(); - if (string_number > 5 || gl_string.idle_chip()) { + if (string_number < 1 || string_number > 5 || gl_string.idle_chip()) { // dont parse non immediate data, idle_chip == 0 return kj::Array(); } - // immediate data is the same within one superframe - if (glonass_superframes[msg->sv_id()] != gl_string.superframe_number()) { - glonass_strings[msg->sv_id()].clear(); - glonass_superframes[msg->sv_id()] = gl_string.superframe_number(); + // Check if new string either has same superframe_id or log transmission times make sense + bool superframe_unknown = false; + bool needs_clear = false; + for (int i = 1; i <= 5; i++) { + if (glonass_strings[msg->freq_id()].find(i) == glonass_strings[msg->freq_id()].end()) + continue; + if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) { + superframe_unknown = true; + } + else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) { + needs_clear = true; + } + // Check if string times add up to being from the same frame + // If superframe is known this is redundant + // Strings are sent 2s apart and frames are 30s apart + if (superframe_unknown && + std::abs((glonass_string_times[msg->freq_id()][i] - 2.0 * i) - (last_log_time - 2.0 * string_number)) > 10) + needs_clear = true; } - glonass_strings[msg->sv_id()][string_number] = string_data; + if (needs_clear) { + glonass_strings[msg->freq_id()].clear(); + glonass_string_superframes[msg->freq_id()].clear(); + glonass_string_times[msg->freq_id()].clear(); + } + glonass_strings[msg->freq_id()][string_number] = string_data; + glonass_string_superframes[msg->freq_id()][string_number] = gl_string.superframe_number(); + glonass_string_times[msg->freq_id()][string_number] = last_log_time; + } + if (msg->sv_id() == 255) { + // data can be decoded before identifying the SV number, in this case 255 + // is returned, which means "unknown" (ublox p32) + return kj::Array(); } // publish if strings 1-5 have been collected - if (glonass_strings[msg->sv_id()].size() != 5) { + if (glonass_strings[msg->freq_id()].size() != 5) { return kj::Array(); } MessageBuilder msg_builder; auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris(); eph.setSvId(msg->sv_id()); + eph.setFreqNum(msg->freq_id() - 7); + uint16_t current_day = 0; + uint16_t tk = 0; // string number 1 { - kaitai::kstream stream(glonass_strings[msg->sv_id()][1]); + kaitai::kstream stream(glonass_strings[msg->freq_id()][1]); glonass_t gl_stream(&stream); glonass_t::string_1_t* data = static_cast(gl_stream.data()); eph.setP1(data->p1()); - eph.setTk(data->t_k()); + tk = data->t_k(); + eph.setTkDEPRECATED(tk); eph.setXVel(data->x_vel() * pow(2, -20)); eph.setXAccel(data->x_accel() * pow(2, -30)); eph.setX(data->x() * pow(2, -11)); @@ -321,7 +351,7 @@ kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ // string number 2 { - kaitai::kstream stream(glonass_strings[msg->sv_id()][2]); + kaitai::kstream stream(glonass_strings[msg->freq_id()][2]); glonass_t gl_stream(&stream); glonass_t::string_2_t* data = static_cast(gl_stream.data()); @@ -335,7 +365,7 @@ kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ // string number 3 { - kaitai::kstream stream(glonass_strings[msg->sv_id()][3]); + kaitai::kstream stream(glonass_strings[msg->freq_id()][3]); glonass_t gl_stream(&stream); glonass_t::string_3_t* data = static_cast(gl_stream.data()); @@ -349,11 +379,12 @@ kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ // string number 4 { - kaitai::kstream stream(glonass_strings[msg->sv_id()][4]); + kaitai::kstream stream(glonass_strings[msg->freq_id()][4]); glonass_t gl_stream(&stream); glonass_t::string_4_t* data = static_cast(gl_stream.data()); current_day = data->n_t(); + eph.setNt(current_day); eph.setTauN(data->tau_n() * pow(2, -30)); eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30)); eph.setAge(data->e_n()); @@ -367,36 +398,18 @@ kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ // string number 5 { - kaitai::kstream stream(glonass_strings[msg->sv_id()][5]); + kaitai::kstream stream(glonass_strings[msg->freq_id()][5]); glonass_t gl_stream(&stream); glonass_t::string_5_t* data = static_cast(gl_stream.data()); // string5 parsing is only needed to get the year, this can be removed and // the year can be fetched later in laika (note rollovers and leap year) - uint8_t n_4 = data->n_4(); - uint16_t year = get_glonass_year(n_4, current_day); - if (current_day > 1461) { - // impossible day within last 4 year, reject ephemeris - // TODO: check if this can be detected via hamming code - LOGE("INVALID DATA: current day out of range: %d, %d", current_day, n_4); - glonass_strings[msg->sv_id()].clear(); - return kj::Array(); - } - - uint16_t last_leap_year = 1996 + 4*(n_4-1); - uint16_t days_till_this_year = (year - last_leap_year)*365; - if (days_till_this_year != 0) { - days_till_this_year++; - } - - eph.setYear(year); - eph.setDayInYear(current_day - days_till_this_year); - eph.setHour((eph.getTk()>>7) & 0x1F); - eph.setMinute((eph.getTk()>>1) & 0x3F); - eph.setSecond((eph.getTk() & 0x1) * 30); + eph.setN4(data->n_4()); + int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30; + eph.setTkSeconds(tk_seconds); } - glonass_strings[msg->sv_id()].clear(); + glonass_strings[msg->freq_id()].clear(); return capnp::messageToFlatArray(msg_builder); } @@ -421,7 +434,7 @@ kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { mr.setGpsWeek(msg->week()); auto mb = mr.initMeasurements(msg->num_meas()); - auto measurements = *msg->measurements(); + auto measurements = *msg->meas(); for(int8_t i = 0; i < msg->num_meas(); i++) { mb[i].setSvId(measurements[i]->sv_id()); mb[i].setPseudorange(measurements[i]->pr_mes()); @@ -450,6 +463,22 @@ kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { return capnp::messageToFlatArray(msg_builder); } +kj::Array UbloxMsgParser::gen_nav_sat(ubx_t::nav_sat_t *msg) { + MessageBuilder msg_builder; + auto sr = msg_builder.initEvent().initUbloxGnss().initSatReport(); + sr.setITow(msg->itow()); + + auto svs = sr.initSvs(msg->num_svs()); + auto svs_data = *msg->svs(); + for(int8_t i = 0; i < msg->num_svs(); i++) { + svs[i].setSvId(svs_data[i]->sv_id()); + svs[i].setGnssId(svs_data[i]->gnss_id()); + svs[i].setFlagsBitfield(svs_data[i]->flags()); + } + + return capnp::messageToFlatArray(msg_builder); +} + kj::Array UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) { MessageBuilder msg_builder; auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus(); diff --git a/selfdrive/locationd/ublox_msg.h b/system/ubloxd/ublox_msg.h similarity index 83% rename from selfdrive/locationd/ublox_msg.h rename to system/ubloxd/ublox_msg.h index 6988f20b7..a52a7db3e 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/system/ubloxd/ublox_msg.h @@ -9,12 +9,17 @@ #include "cereal/messaging/messaging.h" #include "common/util.h" -#include "selfdrive/locationd/generated/gps.h" -#include "selfdrive/locationd/generated/glonass.h" -#include "selfdrive/locationd/generated/ubx.h" +#include "system/ubloxd/generated/gps.h" +#include "system/ubloxd/generated/glonass.h" +#include "system/ubloxd/generated/ubx.h" using namespace std::string_literals; +const int SECS_IN_MIN = 60; +const int SECS_IN_HR = 60 * SECS_IN_MIN; +const int SECS_IN_DAY = 24 * SECS_IN_HR; +const int SECS_IN_WEEK = 7 * SECS_IN_DAY; + // protocol constants namespace ublox { const uint8_t PREAMBLE1 = 0xb5; @@ -86,7 +91,7 @@ namespace ublox { class UbloxMsgParser { public: - bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); + bool add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); inline void reset() {bytes_in_parse_buf = 0;} inline int needed_bytes(); inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);} @@ -97,18 +102,19 @@ class UbloxMsgParser { kj::Array gen_rxm_rawx(ubx_t::rxm_rawx_t *msg); kj::Array gen_mon_hw(ubx_t::mon_hw_t *msg); kj::Array gen_mon_hw2(ubx_t::mon_hw2_t *msg); + kj::Array gen_nav_sat(ubx_t::nav_sat_t *msg); private: inline bool valid_cheksum(); inline bool valid(); inline bool valid_so_far(); - inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt); kj::Array parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); std::unordered_map> gps_subframes; + float last_log_time = 0.0; size_t bytes_in_parse_buf = 0; uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE]; @@ -119,5 +125,6 @@ class UbloxMsgParser { {11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}}; std::unordered_map> glonass_strings; - std::unordered_map glonass_superframes; + std::unordered_map> glonass_string_times; + std::unordered_map> glonass_string_superframes; }; diff --git a/selfdrive/locationd/ubloxd.cc b/system/ubloxd/ubloxd.cc similarity index 88% rename from selfdrive/locationd/ubloxd.cc rename to system/ubloxd/ubloxd.cc index d9b3e7647..1dae6dc86 100644 --- a/selfdrive/locationd/ubloxd.cc +++ b/system/ubloxd/ubloxd.cc @@ -5,7 +5,7 @@ #include "cereal/messaging/messaging.h" #include "common/swaglog.h" #include "common/util.h" -#include "selfdrive/locationd/ublox_msg.h" +#include "system/ubloxd/ublox_msg.h" ExitHandler do_exit; using namespace ublox; @@ -35,6 +35,7 @@ int main() { capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); cereal::Event::Reader event = cmsg.getRoot(); auto ubloxRaw = event.getUbloxRaw(); + float log_time = 1e-9 * event.getLogMonoTime(); const uint8_t *data = ubloxRaw.begin(); size_t len = ubloxRaw.size(); @@ -42,7 +43,7 @@ int main() { while(bytes_consumed < len && !do_exit) { size_t bytes_consumed_this_time = 0U; - if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { + if(parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { try { auto ublox_msg = parser.gen_msg(); diff --git a/tinygrad_repo/accel/opencl/conv.cl b/tinygrad_repo/accel/opencl/conv.cl deleted file mode 100644 index c92a78ac3..000000000 --- a/tinygrad_repo/accel/opencl/conv.cl +++ /dev/null @@ -1,154 +0,0 @@ -//PREFIX - -__kernel void image_conv( - write_only image2d_t output, - read_only image2d_t input, - read_only image2d_t weights -#ifndef NOARGS - ,short numPackedInputChannelsForGroup, - short totalNumPackedInputChannels, - short numPackedOutputChannelsForGroup, - short totalNumPackedOutputChannels, - short numOutputColumns, - short numOutputRows, short numInputRows -#endif - /*short filterSizeX, short filterSizeY, - short paddingX, short paddingY, - short strideX, short strideY, - short dilationX, short dilationY*/ - //ARGS - ) { - - //SHORTS - - const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; - - float4 outputValues[NUM_OUTPUTS]; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] = (float4)(0, 0, 0, 0); - } - - short packedOutputChannel = get_global_id(0); - int2 weightLocation; - weightLocation.x = 0; - weightLocation.y = packedOutputChannel; - - short groupNum = (packedOutputChannel / numPackedOutputChannelsForGroup); - short startPackedInputChannel = mul24(groupNum, numPackedInputChannelsForGroup); - short startOutputColumn = mul24((short)get_global_id(1), NUM_OUTPUTS); - short startX = mad24(mad24(startOutputColumn, strideX, -paddingX), totalNumPackedInputChannels, startPackedInputChannel); - short strideWithChannels = mul24(strideX, totalNumPackedInputChannels); - - int outputRow = get_global_id(2); - int2 inputLocation; - -#ifdef BATCH - // TODO: this doesn't work with y padding - inputLocation.y = mad24(outputRow % numOutputRows, strideY, -paddingY); - int batchOffset = (outputRow / numOutputRows) * numInputRows; - inputLocation.y += batchOffset; -#else - inputLocation.y = mad24(outputRow, strideY, -paddingY); -#endif - -#ifdef DEPTHWISE_UNSTRIDED - for (short rfRow = 0; rfRow < filterSizeY; ++rfRow) { - float4 inputValues[4]; - inputLocation.x = startX; - for (short i = 1; i < 4; ++i) { - inputValues[i] = read_imagef(input, smp, INPUT_LOCATION); - inputLocation.x += totalNumPackedOutputChannels; - } - for (short rfColumn = 0; rfColumn < filterSizeX; ++rfColumn) { - inputValues[0] = inputValues[1]; - inputValues[1] = inputValues[2]; - inputValues[2] = inputValues[3]; - inputValues[3] = read_imagef(input, smp, INPUT_LOCATION); - inputLocation.x += totalNumPackedInputChannels; - float4 weightValues = read_imagef(weights, smp, WEIGHT_LOCATION); - ++weightLocation.x; - outputValues[0] += inputValues[0] * weightValues; - outputValues[1] += inputValues[1] * weightValues; - outputValues[2] += inputValues[2] * weightValues; - outputValues[3] += inputValues[3] * weightValues; - } - ++inputLocation.y; - } -#else - - for (short rfRow = 0; rfRow < filterSizeY; ++rfRow) { - // numPackedInputChannelsForGroup is 1 in depthwise - for (short packedInputChannel = 0; packedInputChannel < numPackedInputChannelsForGroup; ++packedInputChannel) { - short startXForChannel = startX + packedInputChannel; - for (short rfColumn = 0; rfColumn < filterSizeX; ++rfColumn) { - - short dilatedStepX = mul24(totalNumPackedInputChannels, dilationX); - inputLocation.x = mad24(rfColumn, dilatedStepX, startXForChannel); - float4 inputValues[NUM_OUTPUTS]; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - inputValues[i] = read_imagef(input, smp, INPUT_LOCATION); - inputLocation.x += strideWithChannels; - } - -#ifdef DEPTHWISE - float4 weightValues = read_imagef(weights, smp, WEIGHT_LOCATION); - ++weightLocation.x; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] += inputValues[i] * weightValues; - } -#else - float4 weightValues[4]; - for (short outChIdx = 0; outChIdx < 4; ++outChIdx) { - weightValues[outChIdx] = read_imagef(weights, smp, WEIGHT_LOCATION); - ++weightLocation.x; - } - - for (short i = 0; i < NUM_OUTPUTS; ++i) { - // this is marginally faster than using dot - float4 curOutputValues = outputValues[i]; - curOutputValues.x += inputValues[i].x * weightValues[0].x; - curOutputValues.x += inputValues[i].y * weightValues[0].y; - curOutputValues.x += inputValues[i].z * weightValues[0].z; - curOutputValues.x += inputValues[i].w * weightValues[0].w; - curOutputValues.y += inputValues[i].x * weightValues[1].x; - curOutputValues.y += inputValues[i].y * weightValues[1].y; - curOutputValues.y += inputValues[i].z * weightValues[1].z; - curOutputValues.y += inputValues[i].w * weightValues[1].w; - curOutputValues.z += inputValues[i].x * weightValues[2].x; - curOutputValues.z += inputValues[i].y * weightValues[2].y; - curOutputValues.z += inputValues[i].z * weightValues[2].z; - curOutputValues.z += inputValues[i].w * weightValues[2].w; - curOutputValues.w += inputValues[i].x * weightValues[3].x; - curOutputValues.w += inputValues[i].y * weightValues[3].y; - curOutputValues.w += inputValues[i].z * weightValues[3].z; - curOutputValues.w += inputValues[i].w * weightValues[3].w; - outputValues[i] = curOutputValues; - } -#endif - } - } - inputLocation.y += dilationY; - } -#endif - - int2 outputLocation; - outputLocation.y = outputRow; - - // do binops - short outputColumn = startOutputColumn; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputLocation.x = mad24(outputColumn, totalNumPackedOutputChannels, packedOutputChannel); - //BINOP - ++outputColumn; - } - - // output to memory - outputColumn = startOutputColumn; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputLocation.x = mad24(outputColumn, totalNumPackedOutputChannels, packedOutputChannel); - if (outputColumn < numOutputColumns) { - write_imagef(output, OUTPUT_LOCATION, outputValues[i]); - } - ++outputColumn; - } -} diff --git a/tinygrad_repo/accel/opencl/matmul.cl b/tinygrad_repo/accel/opencl/matmul.cl deleted file mode 100644 index 4f878e022..000000000 --- a/tinygrad_repo/accel/opencl/matmul.cl +++ /dev/null @@ -1,49 +0,0 @@ -//PREFIX - -__kernel void matmul( - write_only image2d_t output, - __local float *outputScratch, - read_only image2d_t input, - read_only image2d_t weights - //ARGS - ) { - - //SHORTS - - const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; - short packedOutputChannel = get_global_id(2); - short scratchOffset = mad24((short)get_local_id(1), 4, (short)get_local_id(0)); - short weightIndex = (short)get_global_id(0); - - // fast path precompute (32x speedup) - float outputValue = 0.0f; - for (short inputSet = (short)get_global_id(1); inputSet < numPackedInputChannelsForGroup; inputSet += get_global_size(1)) { - int2 inputLocation = (int2)(inputSet, 0); - float4 inputValues = read_imagef(input, smp, INPUT_LOCATION); - int2 weightLocation = (int2)(mad24(inputSet, 4, weightIndex), packedOutputChannel); - float4 weightValues = read_imagef(weights, smp, WEIGHT_LOCATION); - outputValue += dot(inputValues, weightValues); - } - - short scratchIndex = mad24((short)get_local_id(2), mul24((short)get_local_size(1), 4), scratchOffset); - outputScratch[scratchIndex] = outputValue; - - barrier(CLK_LOCAL_MEM_FENCE); - - if (scratchOffset == 0) { - float4 outputValues = (float4)(0, 0, 0, 0); - - // fast path - for (short i = 0; i < (short)get_global_size(1); ++i) { - outputValues += vload4(0, &outputScratch[scratchIndex]); - scratchIndex += 4; - } - - // insert unary and binary ops here - int2 outputLocation = (int2)(packedOutputChannel, 0); - //BINOP - - // output to memory - write_imagef(output, OUTPUT_LOCATION, outputValues); - } -} diff --git a/tinygrad_repo/accel/opencl/ops_opencl.py b/tinygrad_repo/accel/opencl/ops_opencl.py deleted file mode 100644 index f39434ea0..000000000 --- a/tinygrad_repo/accel/opencl/ops_opencl.py +++ /dev/null @@ -1,431 +0,0 @@ -# type: ignore - -from __future__ import annotations -import os -from tinygrad.llops.ops_gpu import GPUBuffer, CL, CLProgram, CLBuffer -from tinygrad.ops import ProcessingOps, ReduceOps, UnaryOps, BinaryOps, MovementOps, get_buffers, get_lazyops, get_lazyop_info, LazyOp, Op -from tinygrad.helpers import prod, ConvArgs, dedup -from typing import List, Tuple, Optional, Dict, Set, Union -import numpy as np -import pyopencl as cl - -UNSAFE_FLOAT4 = int(os.getenv("UNSAFE_FLOAT4", 0)) -NATIVE_EXPLOG = int(os.getenv("NATIVE_EXPLOG", 0)) # this is needed as a switch for the tests to pass -FLOAT16 = int(os.getenv("FLOAT16", 0)) - -import pathlib -def load(x): - with open(x) as f: - ret = f.read() - return ret -CONV_SRC = load(pathlib.Path(__file__).resolve().parent.parent.parent / 'accel/opencl/conv.cl') -MATMUL_SRC = load(pathlib.Path(__file__).resolve().parent.parent.parent / 'accel/opencl/matmul.cl') - -class CLImage: - fmt = cl.ImageFormat(cl.channel_order.RGBA, cl.channel_type.HALF_FLOAT if FLOAT16 else cl.channel_type.FLOAT) - - def __init__(self, shape): - self.max_hw = min(CL().cl_ctx.devices[0].image2d_max_width, CL.cl_ctx.devices[0].image2d_max_height) - self.shape = shape - self.n_tile = int(np.ceil(max(shape) / self.max_hw).item()) - # if n_tile > 1, we can't fit the image into a CL image at native size, - # and need to internally store it as a set of disjoint tiles - if self.n_tile * min(shape) > self.max_hw: - raise Exception(f"shape {shape} exceeds Metal image limits, even after tiling") - if shape[0] >= shape[1]: - # wider than it is tall; extra tiles overflow on y - self.tile_axis, tiled_width, tiled_height = 1, min(shape[0], self.max_hw), self.n_tile * shape[1] - else: - # taller than it is wide; extra tiles overflow on x - self.tile_axis, tiled_width, tiled_height = 0, self.n_tile * shape[0], min(shape[1], self.max_hw) - self.cl = cl.Image(CL.cl_ctx, cl.mem_flags.READ_WRITE, CLImage.fmt, shape=(tiled_width, tiled_height)) - CL.mem_used += self.cl.row_pitch * self.cl.height - - def pos_to_sample_pos(self, l="l", check_bounds=True): - if self.n_tile == 1: - # happy path where no indexing ops are needed - return l - # sad tiled path; need to adjust indices, and manually check bounds for the tiled axis - if self.tile_axis == 1: - sample_pos = f"((int2)({l}.x % {self.max_hw}, ({l}.x / {self.max_hw}) * {self.shape[1]} + {l}.y))" - in_bounds = f"((0 <= {l}.y) && ({l}.y < {self.shape[1]}))" - else: - sample_pos = f"((int2)(({l}.y / {self.max_hw}) * {self.shape[0]} + {l}.x, {l}.y % {self.max_hw}))" - in_bounds = f"((0 <= {l}.x) && ({l}.x < {self.shape[0]}))" - if check_bounds: - return f"({in_bounds} ? {sample_pos} : (int2)(-1, -1))" - return sample_pos - - def __del__(self): - if hasattr(self, "cl"): - CL.mem_used -= self.cl.row_pitch * self.cl.height - -def get_replacements(prg_src:str, opencl_type:List[str]) -> Dict[str, str]: - middle_code = [] - - """ - vv = "xyzw" - for i in range(4): - acc = f"outputValues[i].{vv[i%4]}" - args = [x.split(" ")[-1].replace("*", "") for x in opencl_type] - args = [f"(outputRow * get_image_width(output) + outputLocation.x)*4+{i}", acc]+args - middle_code.append(f"{acc} = _ewop("+', '.join(args)+");\n") - """ - acc = "outputValues[i]" - args = [x.split(" ")[-1].replace("*", "") for x in opencl_type] - args = ["smp", "outputLocation", "(outputLocation.y * get_image_width(output) + outputLocation.x)*4", acc]+args - middle_code.append(f"{acc} = _ewop("+', '.join(args)+");\n") - - replacements = {} - replacements["//PREFIX"] = prg_src - replacements["//BINOP"] = ''.join(middle_code) - if len(opencl_type) != 0: - replacements["//ARGS"] = ","+','.join(opencl_type) - return replacements - -def get_getters(ewbufs, ret): - fakebufs = [] - ewtypes = [] - getters = [] - for name, buf in ewbufs: - view, unfolded, _ = buf.contiguous_view_constant_fold(name) - if not unfolded: - getters.append(view) - fakebufs.append(name) - getters.append(f"inline float4 get4_{name}(int gid) {{"+ - f"return (float4)(get_{name}(gid+0), get_{name}(gid+1), get_{name}(gid+2), get_{name}(gid+3)); }}") - elif buf.is_image() and buf.shape == ret.shape and buf.st.contiguous: - # use an image here - ewtypes.append(f"read_only image2d_t {name}_g") - getters.append(f"inline float4 get4_{name}(read_only image2d_t x, const sampler_t smp, int2 loc, int gid) {{ return read_imagef(x, smp, {buf._image.pos_to_sample_pos('loc')}); }}") - elif buf.st.contiguous: - # use float4 - ewtypes.append(f"__global const float4 *{name}_g") - getters.append(f"inline float4 get4_{name}(__global const float4 *x, const sampler_t smp, int2 loc, int gid) {{ return x[gid/4]; }}") - elif UNSAFE_FLOAT4: - # aggressive constant folding - fakebufs.append(name) - prt = buf._backing.reshape((-1, 4)) - cc = [] - for ii in range(prt.shape[0]): - cc.append("(float4)(%ff, %ff, %ff, %ff)" % (prt[ii][0], prt[ii][1], prt[ii][2], prt[ii][3])) - getters.append(f"const __constant float4 const_{name}[] = {{"+', '.join(cc)+"};") - getters.append(f"inline float4 get4_{name}(int gid) {{"+ - "int idx = gid;"+buf.st.expr()+";"+ - f"return const_{name}[idx/4]; }}") - """ - # use float4 indexed (HACK!) - # TODO: work out when this is okay - ewtypes.append(f"__global const float4 *{name}_g") - getters.append(f"inline float4 get4_{name}(__global const float4 *x, const sampler_t smp, int2 loc, int gid) {{"+ - "int valid = 1; int idx = gid;"+buf.st.expr()+";"+ - f"return x[idx/4]; }}") - """ - else: - # fallback to float - getters.append(view) - ewtypes.append(f"__global const float *{name}_g") - getters.append(f"inline float4 get4_{name}(__global const float *x, const sampler_t smp, int2 loc, int gid) {{"+ - f"return (float4)(get_{name}(x,gid+0), get_{name}(x,gid+1), get_{name}(x,gid+2), get_{name}(x,gid+3)); }}") - return fakebufs, ewtypes, getters - -def roundup(x, n=4): return (x+(n-1))//n * n -class OpenCLBuffer(GPUBuffer): - code_for_op = { - UnaryOps.NOOP: "(A)", UnaryOps.NEG: "(-(A))", UnaryOps.RELU: "max(A, (float)0.)", UnaryOps.SIGN: "sign(A)", - UnaryOps.EXP: "native_exp(A)" if NATIVE_EXPLOG else "exp(A)", - UnaryOps.LOG: "native_log(A)" if NATIVE_EXPLOG else "log(A)", - UnaryOps.RECIPROCAL: "native_recip(A)" if NATIVE_EXPLOG else "((float)1.0/A)", - BinaryOps.ADD: "(A+B)", BinaryOps.SUB: "(A-B)", BinaryOps.MUL: "(A*B)", BinaryOps.DIV: "(A/B)", BinaryOps.POW: "pow(A,B)", BinaryOps.CMPEQ: "(A==B)", - ReduceOps.SUM: "(acc + A)", ReduceOps.MAX: "max(A, acc)", MovementOps.RESHAPE: "(A)" - } - start_for_op = {ReduceOps.SUM: "0.0", ReduceOps.MAX: "-INFINITY"} - def __init__(self, shape, hostbuf:Optional[OpenCLBuffer]=None, backing:Optional[np.ndarray]=None): - self._image = hostbuf._image if hostbuf is not None else None - self.copied_backing = False - super().__init__(shape, hostbuf, backing) - assert not (self._image and self._buf) - - @staticmethod - def fromCPU(x): return OpenCLBuffer(x.shape, backing=x.view(np.ndarray).astype(np.float32).ravel()) - - def __repr__(self): return f"" - - @property - def cl(self): - if self._buf is None: - if self._backing is not None and not self.copied_backing: - self._buf = CLBuffer(4*roundup(prod(self._backing.shape))) - CL.enqueue_copy(self._buf.cl, self._backing, is_blocking=False) - self.copied_backing = True - elif self.st.contiguous: - self._buf = CLBuffer(4*roundup(prod(self.shape))) - - if self._image is not None: - self._buf = CLBuffer(4*roundup(prod(self._image.shape)*4)) - if self._backing is not None and not self.copied_backing: - CL.enqueue_copy(self._buf.cl, self._backing, is_blocking=False) - self.copied_backing = True - #print(f"converting {self.shape} back to buffer, image shape is {self._image.shape}") - CLProgram("from_image", f""" - __kernel void from_image( - __global float4 *out, - read_only image2d_t in) {{ - const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; - int2 l; - l.y = get_global_id(1); - l.x = get_global_id(0); - int2 l_smp = {self._image.pos_to_sample_pos('l')}; - int W = {str(self._image.shape[0])}; - out[l.y*W + l.x] = read_imagef(in, smp, l_smp); - }} - """)(self._image.shape, None, self._buf.cl, self._image.cl) - self._image = None - return self._buf.cl - - def is_image(self): return self._image is not None - - @property - def image(self): - if self._image is None: - assert len(self.shape) == 3 and self.shape[2] == 4, f"bad shape for image {self.shape}" - assert self.st.contiguous, f"{self} is not contiguous" - self._image = CLImage(shape=(self.shape[1], self.shape[0])) - if self._buf is not None: - assert prod(self.shape) <= prod(self._image.cl.shape)*4 - #print(f"converting {self.shape} to image with shape {self._image.shape}") - CLProgram("to_image", f""" - __kernel void to_image( - write_only image2d_t out, - __global const float4 *in) {{ - int2 l; - l.y = get_global_id(1); - l.x = get_global_id(0); - int2 l_out = {self._image.pos_to_sample_pos('l', check_bounds=False)}; - int W = {str(self._image.shape[0])}; - write_imagef(out, l_out, in[l.y*W + l.x]); - }} - """)(self._image.shape, None, self._image.cl, self._buf.cl) - self._buf = None - return self._image.cl - - SUPPORTS_PADDING = True - def processing_op(x, op:ProcessingOps, w:GPUBuffer, C:ConvArgs): - assert op == ProcessingOps.CONV, f"{op} isn't supported" - return type(x)(C.out_shape)._processing_op([("input", x.contiguous()), ("weight", w.contiguous())], "acc", C) - - def contiguous_view_constant_fold(x, name:str, reduce:Optional[int]=None) -> Tuple[str, Optional[str], str]: - # this will only be for convs, for reduce we have to fall back to cl - if x.is_image() and reduce is None: - #print("is image") - return f"""inline float get_{name}(const sampler_t smp, read_only image2d_t x, int gid) {{ - int valid = 1; int idx = gid; {x.st.expr().replace('//', '/')}; - int2 l; - int W = {str(x._image.shape[0])}; - l.y = idx / (W*4); - l.x = (idx/4) % W; - int idx4 = idx % 4; - int2 l_smp = {x._image.pos_to_sample_pos('l')}; - float4 dat = read_imagef(x, smp, l_smp); - return valid ? (idx4 == 0 ? dat.x : (idx4 == 1 ? dat.y : (idx4 == 2 ? dat.z : dat.w))) : 0.0; - }}""", f"read_only image2d_t {name}_g", f"get_{name}(smp, {name}_g, gid);" - else: - idx_getter = f"int valid = 1; {'long' if prod(x.shape) >= 2**31 else 'int'} idx = gid; {'idx *= '+str(reduce)+'; idx += subidx;' if reduce is not None else ''} {x.st.expr().replace('//', '/')};" - constant = x._backing[0] if x._base_shape == (1,) and x._backing is not None else None - args = (["__global const float *x"] if constant is None else []) + ["int gid"] + (["int subidx"] if reduce is not None else []) - return f"inline float get_{name}({','.join(args)}) {{ {idx_getter} return valid ? {constant if constant is not None else 'x[idx]'} : 0.0;}}", \ - f"__global const float *{name}_g" if constant is None else None, \ - f"get_{name}({name+'_g, ' if constant is None else ''}gid{', subidx' if reduce is not None else ''});" - - @classmethod - def exec_ast(cls, ast:LazyOp): - # copied from llvm - bufs = dedup(get_buffers(ast)) - reduceops = dedup([x for x in get_lazyops(ast) if isinstance(x.op, ReduceOps) or isinstance(x.op, ProcessingOps)]) - assert len(reduceops) <= 1, f"max one reduce op in an ast, {reduceops}" - earlybufs = dedup(get_buffers(reduceops[0])) if len(reduceops) > 0 else [] - reduce_shape = (earlybufs[0].shape, reduceops[0].arg) if len(reduceops) > 0 and isinstance(reduceops[0].op, ReduceOps) else None - info = get_lazyop_info(ast) - ret = cls(info.shape) - - buf_names : Dict[GPUBuffer, str] = {x:f"arg_{i}" for i,x in enumerate(bufs)} - - # special names for input and weight - if len(reduceops) > 0 and isinstance(reduceops[0].op, ProcessingOps): - buf_names[reduceops[0].src[0]] = "input" - buf_names[reduceops[0].src[1]] = "weight" - - def _ast(x: Union[GPUBuffer, LazyOp], buf_names: Dict[GPUBuffer, str], code_for_op: Dict[Op, str], allow_reduce=False) -> str: - if isinstance(x, GPUBuffer): - return buf_names[x] - if not allow_reduce and type(x.op) in [ProcessingOps, ReduceOps]: - return "acc" - srcs_code = [_ast(src, buf_names, code_for_op) for src in x.src] - code = code_for_op[x.op] - if len(srcs_code) >= 1: - code = code.replace("A", srcs_code[0]) - if len(srcs_code) >= 2: - code = code.replace("B", srcs_code[1]) - return code - - earlycode = _ast(reduceops[0], buf_names, cls.code_for_op, allow_reduce=True) if len(reduceops) > 0 and isinstance(reduceops[0].op, ReduceOps) else "acc" - code = _ast(ast, buf_names, cls.code_for_op) - - C = reduceops[0].arg if len(reduceops) > 0 and isinstance(reduceops[0].op, ProcessingOps) else None - reduce_op = reduceops[0].op if len(reduceops) > 0 and isinstance(reduceops[0].op, ReduceOps) else ReduceOps.SUM - return ret._processing_op([(buf_names[x], x) for x in bufs], code, C, reduce_op, reduce_shape, set(buf_names[x] for x in earlybufs), earlycode, info.flops) - - def _simple_processing_op(ret, bufs: List[Tuple[str, GPUBuffer]]=[], code:str="acc", C:Optional[ConvArgs]=None, op=ReduceOps.SUM, reduce_shape=None, earlybufs:Set[str]=set(), earlycode:str="acc", op_estimate=0) -> GPUBuffer: - assert C is None, f"conv isn't handled by GPU anymore {C}" - - # get the input/output shape and the reduce amount - reduce_shape = (bufs[0][1].shape, ret.shape) if reduce_shape is None else reduce_shape - red = prod([s for s,n in zip(*reduce_shape) if n == 1]) - assert red < 2**31, f"reduce must be under 2**31, {red} isn't" - - # if it's a partial reduce, assert last non reduced axis is before the first reduced axis - if red > 1 and prod(ret.shape) != 1: - assert max([i for i,(s,n) in enumerate(zip(*reduce_shape)) if s == n and n != 1]) < min([i for i,(s,n) in enumerate(zip(*reduce_shape)) if s != 1 and n == 1]) - - kernel_name = "reduce" if red > 1 else "elementwise" - early_views = {name:buf.contiguous_view_constant_fold(name, red) for name, buf in bufs if name in earlybufs} - late_views = {name:buf.contiguous_view_constant_fold(name) for name, buf in bufs if name not in earlybufs} - views = {**early_views, **late_views} - - buf_types : List[str] = [views[name][1] for name, _ in bufs if views[name][1] is not None] # type: ignore - buf_cl = [buf.cl if 'image2d_t' not in views[name][1] else buf.image for name, buf in bufs if views[name][1] is not None] # type: ignore - - # use local memory if it's a multistage reduce - inter_red = 256 if (prod(ret.shape) < 8192 and red >= 256) else 1 - if inter_red > 1: - buf_cl.append(cl.LocalMemory(inter_red*4)) - - reduce_loop = f"int mid = get_global_id(1); for (int subidx = {red//inter_red + 1} * mid; subidx < min({red}, {red//inter_red + 1} * (mid+1)); subidx++)" if inter_red > 1 else f"for (int subidx = 0; subidx < {red}; subidx++)" - conv_prg = CLProgram(kernel_name, f"""{chr(10).join([x[0] for x in views.values()])} - __kernel void {kernel_name}({','.join(["__global float* restrict output"] + buf_types + (["__local float *temp"] if inter_red > 1 else []))}) {{ - const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; - float acc = {GPUBuffer.start_for_op[op]}; - int gid = get_global_id(0); - {reduce_loop} {{ -{chr(10).join([f' float {name} = ' + early_views[name][2] for name in early_views])} - acc = {earlycode}; - }}"""+(f""" - temp[mid] = acc; barrier(CLK_LOCAL_MEM_FENCE); - if (mid == 0) {{ acc = {GPUBuffer.start_for_op[op]}; - for (int rdx = 0; rdx < {inter_red}; rdx++) {{ - acc = {GPUBuffer.code_for_op[op].replace('A', 'temp[rdx]')}; - }}""" if inter_red != 1 else "{")+f""" -{chr(10).join([f' float {name} = ' + late_views[name][2] for name in late_views])} - output[gid] = {code}; - }} - }}""", op_estimate=op_estimate) - - conv_prg([prod(ret.shape), inter_red, 1], [1, inter_red, 1] if inter_red > 1 else None, ret.cl, *buf_cl) - return ret - - def _processing_op(ret, bufs: List[Tuple[str, OpenCLBuffer]]=[], code:str="acc", C=None, op=ReduceOps.SUM, reduce_shape=None, earlybufs:Set[str]=set(), earlycode:str="acc", op_estimate=0): - if C is None or earlycode != "acc": - # TODO: handle an opencl conv without the conv part - return ret._simple_processing_op(bufs, code, C, op, reduce_shape, earlybufs, earlycode, op_estimate) - assert earlycode == "acc" - - x = [x for x in bufs if x[0] == "input"][0][1] - w = [x for x in bufs if x[0] == "weight"][0][1] - ewbufs = [x for x in bufs if x[0] not in ["input", "weight"]] - - # remove fakebufs - fakebufs, ewtypes, getters = get_getters(ewbufs, ret) - ewbufs = [x for x in ewbufs if x[0] not in fakebufs] - - elementwise_prefix = '\n'.join(getters)+ \ - "\n\ninline float4 _ewop("+','.join(["const sampler_t smp", "int2 loc", "int gid", "float4 acc"]+ewtypes)+") {\n"+ \ - ''.join([f"float4 {name} = get4_{name}(gid);\n" for name in fakebufs])+ \ - ''.join([f"float4 {name} = get4_{name}({name}_g, smp, loc, gid);\n" for name, _ in ewbufs])+ \ - f"return {code}; }}" - - replacements = get_replacements(elementwise_prefix, ewtypes) - - (x.image, w.image, ret.image) - # fix sampling - replacements["INPUT_LOCATION"] = x._image.pos_to_sample_pos("inputLocation") - replacements["WEIGHT_LOCATION"] = w._image.pos_to_sample_pos("weightLocation") - replacements["OUTPUT_LOCATION"] = ret._image.pos_to_sample_pos("outputLocation", check_bounds=False) - # fix widths - replacements["get_image_width(output)"] = f"({ret._image.shape[0]})" - - x, w = x.contiguous(), w.contiguous() - options = [] - if C.bs > 1: - options.append("-DBATCH") - assert C.py == 0, "batched conv doesn't work with y-padding" - if C.sx == 1 and C.sy == 1 and C.dx == 1 and C.dy == 1 and C.cin == 1: - options.append("-DDEPTHWISE_UNSTRIDED") - elif C.cin == 1: - options.append("-DDEPTHWISE") - if C.groups == 1 and C.H == 1 and C.W == 1 and C.iy == 1 and C.ix == 1 and C.oy == 1 and C.ox == 1 and C.sx == 1 and C.sy == 1 and C.dx == 1 and C.dy == 1 and C.bs == 1: - options.append("-DMATMUL") - # NOTE: this is not actually a matmul, it's a vector * matrix - - conv_args = [] - conv_short_names = ["numPackedInputChannelsForGroup", "totalNumPackedInputChannels", "numPackedOutputChannelsForGroup", "totalNumPackedOutputChannels", "numOutputColumns", "numOutputRows", "numInputRows"] - conv_shorts = [max(1, C.cin//4), C.groups*C.cin//4, max(1, C.rcout//4), C.cout//4, C.ox, C.oy, C.iy] - - conv_src = MATMUL_SRC - replacements["//SHORTS"] = ''.join([f"short {name} = {val};" for name,val in zip(conv_short_names, conv_shorts)]) - if "//BINOP" in replacements: - replacements["//BINOP"] = replacements["//BINOP"].replace("outputValues[i]", "outputValues") - for k,v in replacements.items(): - conv_src = conv_src.replace(k, v) - - #print(conv_src) - conv_prg = CLProgram("matmul", conv_src, - options=tuple(options), - argdtypes=tuple([None, None, None, None] + [np.int16]*len(conv_args) + [None]*len(ewbufs)), - op_estimate=op_estimate - ) - global_work_size = [4, 16, C.cout//4] - - # must be even - lw = CL.cl_ctx.devices[0].max_work_group_size // (global_work_size[0] * global_work_size[1]) - while global_work_size[2] % lw != 0: - lw -= 1 - local_work_size = [4, global_work_size[1], lw] - - #print(global_work_size, local_work_size) - conv_prg(global_work_size, local_work_size, ret.image, cl.LocalMemory(4 * local_work_size[0] * local_work_size[1] * lw), x.image, w.image, *conv_args, *[buf.image if 'image2d_t' in typ else buf.cl for typ, (_, buf) in zip(ewtypes, ewbufs)]) - return ret - - # this option is unused - if C.H == 1 and C.W == 1: - options.append("-DONLY_1X1_CONV") - - assert C.cout%4 == 0 - conv_src = CONV_SRC - conv_short_names = ["filterSizeX", "filterSizeY", "paddingX", "paddingY", "strideX", "strideY", "dilationX", "dilationY"] - conv_shorts = [C.W, C.H, C.px, C.py, C.sx, C.sy, C.dx, C.dy] - conv_arg_names = ["numPackedInputChannelsForGroup", "totalNumPackedInputChannels", "numPackedOutputChannelsForGroup", "totalNumPackedOutputChannels", "numOutputColumns", "numOutputRows", "numInputRows"] - conv_args = [max(1, C.cin//4), C.groups*C.cin//4, max(1, C.rcout//4), C.cout//4, C.ox, C.oy, C.iy] - - NUM_OUTPUTS = 4 - options.append(f"-DNUM_OUTPUTS={NUM_OUTPUTS}") - - # comment out for args - conv_short_names += conv_arg_names - conv_shorts += conv_args - conv_args = [] - options.append("-DNOARGS") - - replacements["//SHORTS"] = ''.join([f"short {name} = {val};" for name,val in zip(conv_short_names, conv_shorts)]) - for k,v in replacements.items(): - conv_src = conv_src.replace(k, v) - #print(conv_src) - conv_prg = CLProgram("image_conv", conv_src, - options=tuple(options), - argdtypes=tuple([None, None, None] + [np.int16]*len(conv_args) + [None]*len(ewbufs)), - op_estimate=op_estimate - ) - global_work_size = [C.cout//4, (C.ox+NUM_OUTPUTS-1)//NUM_OUTPUTS, C.bs*C.oy] - conv_prg(global_work_size, None, ret.image, x.image, w.image, *conv_args, *[buf.image if 'image2d_t' in typ else buf.cl for typ, (_, buf) in zip(ewtypes, ewbufs)]) - return ret - -GPUBuffer = OpenCLBuffer diff --git a/tinygrad_repo/accel/opencl/preprocessing.py b/tinygrad_repo/accel/opencl/preprocessing.py deleted file mode 100644 index 12df69cba..000000000 --- a/tinygrad_repo/accel/opencl/preprocessing.py +++ /dev/null @@ -1,88 +0,0 @@ -from tinygrad.ops import MovementOps, ProcessingOps - -# input format is N, H x W, C//4 x 4 -# dweight format is oc//4 x ch, cw x 4(oc) -# weight format is oc//4 x ch, ic//4, cw, 4(oc) x 4(ic) -def preprocessing_op(x,w,C,make_image=True): - w = w.movement_op(MovementOps.RESHAPE, (C.groups, C.rcout, C.cin, C.H, C.W)) - #print(x.shape, w.shape) - - if C.bs > 1 and C.py > 0: - # explicitly add y-padding for batched inputs - # N C H W - xs = [(0, 0) for _ in x.shape] - xs[2] = (C.py, C.py) - x = x.movement_op(MovementOps.PAD, xs) - C = C._replace(iy=C.iy + C.py*2, py=0) - - # hack for non multiples of 4 on C.cin - if C.cin % 4 != 0 and not (C.cin == 1 and C.groups%4 == 0): - to_add = 4 - (C.cin % 4) - ws = [(0, 0) for _ in w.shape] - ws[2] = (0, to_add) - w = w.movement_op(MovementOps.PAD, ws) - - x = x.movement_op(MovementOps.RESHAPE, (C.bs, C.groups, C.cin, C.iy, C.ix)) - xs = [(0, 0) for _ in x.shape] - xs[2] = (0, to_add) - x = x.movement_op(MovementOps.PAD, xs) - C = C._replace(cin = C.cin + to_add) - x = x.movement_op(MovementOps.RESHAPE, (C.bs, C.groups*C.cin, C.iy, C.ix)) - - # hack for non multiples of 4 on C.rcout - if C.rcout % 4 != 0 and not (C.rcout == 1 and C.groups%4 == 0): - added_output_channels = 4 - (C.rcout % 4) - ws = [(0, 0) for _ in w.shape] - ws[1] = (0, added_output_channels) - w = w.movement_op(MovementOps.PAD, ws) - C = C._replace(rcout = C.rcout + added_output_channels, cout = C.groups * (C.rcout + added_output_channels)) - - # packed - assert (C.groups*C.cin) % 4 == 0 - #print(x.shape) - x = x.movement_op(MovementOps.PERMUTE, (0,2,3,1)) - x = x.movement_op(MovementOps.RESHAPE, (C.bs*C.iy, C.ix*C.groups*C.cin//4, 4)) - - assert C.cout % 4 == 0 - if C.cin == 1: - # depthwise - w = w.movement_op(MovementOps.RESHAPE, (C.cout//4,4,C.H*C.W)) - w = w.movement_op(MovementOps.PERMUTE, (0,2,1)) - else: - w = w.movement_op(MovementOps.RESHAPE, (C.cout//4,4,C.cin//4,4,C.H,C.W)) - w = w.movement_op(MovementOps.PERMUTE, (0,4,2,5,1,3)) - w = w.movement_op(MovementOps.RESHAPE, (C.cout//4, C.H * C.cin//4 * C.W * 4, 4)) - - C = C._replace(out_shape = (C.bs*C.oy, C.ox*C.cout//4, 4)) - #x = contiguous(ctx, x, x.shapetracker) if not x.shapetracker.contiguous else x - #w = contiguous(ctx, w, w.shapetracker) if not w.shapetracker.contiguous else w - - # contiguous before image, always - x = x.contiguous() - w = w.contiguous() - - # early realize on the weights - bw = w - while getattr(bw, 'op', None) and len(bw.op.src) == 1: - bw = bw.op.src[0] - if bw.realized: - # weights are static - wr = w.realize() #.image - if make_image: - wr.image - return x,w,C - -def postprocessing_op(ret, C, C_initial): - added_output_channels = C.rcout - C_initial.rcout - - # undo hack for non multiples of 4 on C.rcout - if added_output_channels != 0: - ret = ret.movement_op(MovementOps.RESHAPE, (C.bs, C.oy, C.ox, C.groups, C.rcout)) - xs = [(0, s) for s in ret.shape] - xs[4] = (0, ret.shape[4]-added_output_channels) - ret = ret.movement_op(MovementOps.SHRINK, xs) - C = C._replace(rcout = C.rcout - added_output_channels, cout = C.groups * (C.rcout - added_output_channels)) - - ret = ret.movement_op(MovementOps.RESHAPE, (C.bs, C.oy, C.ox, C.cout)) - ret = ret.movement_op(MovementOps.PERMUTE, (0,3,1,2)) - return ret diff --git a/tinygrad_repo/extra/onnx.py b/tinygrad_repo/extra/onnx.py index a2eb403a0..c64f1bb7d 100644 --- a/tinygrad_repo/extra/onnx.py +++ b/tinygrad_repo/extra/onnx.py @@ -1,24 +1,52 @@ -import os +import functools +import importlib import numpy as np from tinygrad.tensor import Tensor from tinygrad.helpers import prod -from tinygrad.ops import DEBUG -from onnx.mapping import TENSOR_TYPE_TO_NP_TYPE +from tinygrad.helpers import getenv, DEBUG +try: + from onnx.helper import tensor_dtype_to_np_dtype +except ImportError: + # for onnx < 1.13 + from onnx.mapping import TENSOR_TYPE_TO_NP_TYPE + tensor_dtype_to_np_dtype = lambda x: TENSOR_TYPE_TO_NP_TYPE[x] -ONNXLIMIT = int(os.getenv("ONNXLIMIT", "-1")) +# global numpy cache for parameters +numpy_cache = {} +def safe_numpy(t): + if not isinstance(t, Tensor): return t + global numpy_cache + if t not in numpy_cache: + if DEBUG >= 1: + print("numpy cache miss", t) + numpy_cache[t] = t.numpy() + return numpy_cache[t] + +onnx_ops = importlib.import_module('extra.onnx_ops') + +ONNXLIMIT = getenv("ONNXLIMIT", -1) def get_run_onnx(onnx_model): def shape_to_tuple(s): return tuple(x.dim_value for x in s.dim) def buffer_parse(inp): - if inp.data_type in (1,10,7): - ret = Tensor(np.frombuffer(inp.raw_data, dtype=TENSOR_TYPE_TO_NP_TYPE[inp.data_type]).reshape(inp.dims).astype(np.float32).copy(), requires_grad=False) + if inp.data_type in (1,10,6,7): + # TODO: this is shared with below + if len(inp.float_data) > 0: + ret = Tensor(np.array(inp.float_data, dtype=np.float32).reshape(inp.dims), requires_grad=False) + elif len(inp.int64_data) > 0: + ret = Tensor(np.array(inp.int64_data, dtype=np.float32).reshape(inp.dims), requires_grad=False) + elif len(inp.int32_data) > 0: + ret = Tensor(np.array(inp.int32_data, dtype=np.float32).reshape(inp.dims), requires_grad=False) + else: + ret = Tensor(np.frombuffer(inp.raw_data, dtype=tensor_dtype_to_np_dtype(inp.data_type)).reshape(inp.dims).astype(np.float32).copy(), requires_grad=False) else: raise Exception(f"bad data type {inp.name} {inp.dims} {inp.data_type}") return ret def attribute_parse(a): - if a.type == 7: return tuple([int(x) for x in a.ints]) + if a.type in [6,7]: return tuple([int(x) for x in a.ints]) elif a.type == 4: return buffer_parse(a.t) # TENSOR + elif a.type == 3: return str(a.s) elif a.type == 2: return int(a.i) elif a.type == 1: return float(a.f) else: raise Exception(f"can't parse {a.type} {a}") @@ -46,18 +74,11 @@ def get_run_onnx(onnx_model): attribute_dict = {} for num,n in enumerate(onnx_model.graph.node): attribute_dict[num] = attribute_to_dict(n.attribute) - - # and cache them - numpy_cache = {} - def safe_numpy(t): - nonlocal numpy_cache - if t not in numpy_cache: - if DEBUG >= 1: - print("numpy cache miss", t) - numpy_cache[t] = t.numpy() - return numpy_cache[t] + + onnx_version = onnx_model.opset_import[0].version def run_onnx(inputs={}, debug=False): + if getenv("DEBUGONNX"): debug = True input_tensors = {} intermediate_tensors = {} output_tensor_names = [x.name for x in onnx_model.graph.output] @@ -66,9 +87,10 @@ def get_run_onnx(onnx_model): for inp in onnx_model.graph.input: if inp.name in tensors: continue shape = shape_to_tuple(inp.type.tensor_type.shape) - if shape[0] == 0: shape = tuple([1]+list(shape[1:])) # 1 batch size + if len(shape) >= 1 and shape[0] == 0: shape = tuple([1]+list(shape[1:])) # 1 batch size if inp.name in inputs: input_shape = inputs[inp.name].shape + if input_shape == (0,): raise NotImplementedError("empty tensors aren't supported in tinygrad") assert input_shape == shape, f"wrong shape for input {inp.name}, {input_shape} isn't {shape}" if isinstance(inputs[inp.name], Tensor): input_tensors[inp.name] = inputs[inp.name] @@ -79,32 +101,32 @@ def get_run_onnx(onnx_model): raise Exception(f"no data for {inp.name} with shape {shape}") for num,n in enumerate(onnx_model.graph.node): - if debug: print(f"{num}: op {n.op_type}") - inp = [tensors[x] if x in tensors else (intermediate_tensors[x] if x in intermediate_tensors else input_tensors[x]) for x in n.input] + inp = [tensors[x] if x in tensors else (intermediate_tensors[x] if x in intermediate_tensors else (input_tensors[x] if x != str() else None)) for x in n.input] opt = attribute_dict[num] + if debug: print(f"{num}: op {n.op_type} shape {[x.shape if isinstance(x, Tensor) else x for x in inp]} opt {opt}") # free ones if n.op_type == "Relu": ret = inp[0].relu() elif n.op_type == "Sigmoid": ret = inp[0].sigmoid() elif n.op_type == "Tanh": ret = inp[0].tanh() - elif n.op_type == "Softmax": ret = inp[0].softmax() elif n.op_type == "MatMul": ret = inp[0].matmul(inp[1]) # one liners - elif n.op_type == "Elu": ret = inp[0].elu(alpha=opt['alpha']) - elif n.op_type == "Clip": ret = inp[0].clip(*(inp[1:] if len(inp) > 1 else (opt.get('min', -3.4e38), opt.get('max', 3.4e38)))) + elif n.op_type == "Elu": ret = inp[0].elu(alpha=opt.get('alpha', 1.0)) elif n.op_type == "Concat": ret = inp[0].cat(*inp[1:], dim=opt['axis']) - elif n.op_type == "Flatten": ret = inp[0].flatten(opt['axis'] if 'axis' in opt else 0) - elif n.op_type == "Transpose": ret = inp[0].permute(order=opt['perm']) + elif n.op_type == "Transpose": ret = inp[0].permute(order=opt.get('perm', list(range(len(inp[0].shape))[::-1]))) elif n.op_type == "Squeeze": ret = inp[0].reshape([s for i,s in enumerate(inp[0].shape) if i not in opt['axes']]) - elif n.op_type == "Unsqueeze": ret = inp[0].reshape(np.insert(inp[0].shape, opt['axes'][0], 1).tolist()) - elif n.op_type == "ReduceL2": ret = inp[0].pow(2).sum(axis=opt['axes'], keepdim=opt['keepdims']).sqrt() - elif n.op_type == "ReduceSum": ret = inp[0].sum(axis=opt['axes'], keepdim=opt['keepdims']) - elif n.op_type == "GlobalAveragePool": ret = inp[0].mean(axis=tuple(range(2, len(inp[0].shape))), keepdim=True) - elif n.op_type == "Shape": ret = inp[0].shape - elif n.op_type == "Expand": ret = inp[0].reshape([1]*(max(len(inp[0].shape), len(inp[1]))-len(inp[0].shape)) + list(inp[0].shape)) # just broadcast - elif n.op_type == "Div": ret = inp[0].div(inp[1]) - elif n.op_type == "Constant": ret = opt['value'] - elif n.op_type == "Reshape": ret = inp[0].reshape([int(x) for x in safe_numpy(inp[1])]) + elif n.op_type == "Div": + # in openpilot, due to SHUFFLE_PAD_OPS issues, we are spending an extra kernel + ret = inp[0].div(inp[1]) + elif n.op_type == "Constant": ret = opt['value'] if 'value' in opt else opt['value_float'] + elif n.op_type == "Reshape": ret = inp[0].reshape([int(x) if x != 0 else inp[0].shape[i] for i,x in enumerate(safe_numpy(inp[1]))]) + elif n.op_type == "Resize": + # TODO: this is handcoded for YOLOv8 + scales = safe_numpy(inp[2]) + assert all([int(x) == x and x >= 1 for x in scales]) + ret = inp[0].reshape([val for pair in zip(inp[0].shape, [1] * len(scales)) for val in pair]) + ret = ret.expand([val for pair in zip(inp[0].shape, [int(x) for x in scales]) for val in pair]) + ret = ret.reshape([x*y for x,y in zip(inp[0].shape, [int(x) for x in scales])]) elif n.op_type == "Gather": # TODO: is this correct? seems to work for simple gather ops axis = opt['axis'] @@ -113,21 +135,7 @@ def get_run_onnx(onnx_model): args = [[(0,x) if j != axis else (i,i+1) for j, x in enumerate(shape)] for i in indices] ret = inp[0].slice(arg=args[0]).cat(*[inp[0].slice(arg=arg) for arg in args[1:]], dim=axis) ret = ret.reshape([s for i,s in enumerate(shape) if i != axis]) if len(indices) == 1 else ret # squeeze if needed - elif n.op_type == "BatchNormalization": - invstd = inp[4].add(opt.get('epsilon', 1e-5))**-0.5 - ret = inp[0].batchnorm(inp[1], inp[2], inp[3], invstd) - elif n.op_type == "Gemm": ret = inp[0].linear(inp[1].transpose() if opt.get('transB', 0) == 1 else inp[1], inp[2]) - elif n.op_type == "Conv": - x,w,b = inp if len(inp) == 3 else (inp[0], inp[1], None) - assert 'dilations' not in opt or opt['dilations'] == (1,1) - if opt['pads'][0] == opt['pads'][2] and opt['pads'][1] == opt['pads'][3]: - # symmetric padding - # TODO: is this backward? - ret = x.conv2d(w, b, stride=opt['strides'], groups=opt.get('group', 1), padding=opt['pads'][0:2]) - else: - x = x.pad2d((opt['pads'][0], opt['pads'][2], opt['pads'][1], opt['pads'][3])) - ret = x.conv2d(w, b, stride=opt['strides'], groups=opt.get('group', 1)) - elif n.op_type in ["Add", "Sub", "Mul"]: + elif n.op_type in ["Add", "Sub", "Mul", "Pow"]: # TODO: add this to tinygrad? i don't think it's in torch if len(inp[0].shape) != len(inp[1].shape) and prod(inp[0].shape) == prod(inp[1].shape): inp[1] = inp[1].reshape(inp[0].shape) @@ -136,7 +144,9 @@ def get_run_onnx(onnx_model): if n.op_type == "Add": ret = inp[0] + inp[1] if n.op_type == "Sub": ret = inp[0] - inp[1] if n.op_type == "Mul": ret = inp[0] * inp[1] + if n.op_type == "Pow": ret = inp[0] ** inp[1] elif n.op_type == "Split": + if 'split' not in opt: opt['split'] = [int(x) for x in safe_numpy(inp[1])] # split can be a tensor i = 0 arg = [(0,x) for x in inp[0].shape] for o,s in zip(n.output, opt['split']): @@ -144,21 +154,8 @@ def get_run_onnx(onnx_model): intermediate_tensors[o] = inp[0].slice(arg=arg) i = i+s continue - elif n.op_type == "AveragePool": - assert opt['kernel_shape'] == opt['strides'] or opt['strides'] == (1,1) - ret = inp[0].avg_pool2d(opt['kernel_shape']) - elif n.op_type == "MaxPool": - assert opt['kernel_shape'] == opt['strides'] - #opt['kernel_shape'] = opt['strides'] - # TODO: this is untested and probably wrong - ret = inp[0].pad2d(opt['pads']) - ret = ret.max_pool2d(opt['kernel_shape']) - # strides aren't supported in max_pool - #chan = ret.shape[1] - #w = Tensor.eye(chan).reshape((chan, chan, 1, 1)) - #ret = ret.conv2d(w, stride=opt['strides']) elif n.op_type == "Slice": - assert onnx_model.opset_import[0].version == 10 + assert onnx_version == 10 arg = [(0,x) for x in inp[0].shape] starts, ends, axes = inp[1:4] assert axes.shape == (1,) @@ -167,13 +164,23 @@ def get_run_onnx(onnx_model): starts = starts + inp[0].shape[axis] if starts < 0 else starts arg[axis] = (starts, ends) ret = inp[0].slice(arg=arg) + elif hasattr(onnx_ops, n.op_type): + fxn = getattr(onnx_ops, n.op_type) + if isinstance(fxn, dict): + for k in sorted(fxn.keys()): + if k < onnx_version: + real_fxn = fxn[k] + else: + real_fxn = fxn + ret = real_fxn(*inp, **opt) else: print("UNSUPPORTED", n.op_type, n.input, n.output) raise Exception(f"op_type {n.op_type} not supported") - assert len(n.output) == 1 - if debug: print(ret.shape) - intermediate_tensors[n.output[0]] = ret - #print(ret.numpy().mean()) + if not isinstance(ret, tuple): ret = (ret, ) + assert len(n.output) <= len(ret), f"expected output size must be less than {len(ret)}, it's {n.output}" + if debug: print([x.shape if isinstance(x, Tensor) else None for x in ret]) + for i in range(len(n.output)): intermediate_tensors[n.output[i]] = ret[i] + #print(ret[0].numpy().mean()) if num == ONNXLIMIT: output_tensor_names = n.output break diff --git a/tinygrad_repo/extra/onnx_ops.py b/tinygrad_repo/extra/onnx_ops.py new file mode 100644 index 000000000..35c1d92eb --- /dev/null +++ b/tinygrad_repo/extra/onnx_ops.py @@ -0,0 +1,178 @@ +from tinygrad.tensor import Tensor +from tinygrad.helpers import prod +from extra.onnx import safe_numpy +import numpy as np +import functools + +def Unsqueeze(data, axes): + axes = [len(data.shape) + int(x) if x < 0 else int(x) for x in safe_numpy(axes)] + ptr = 0 + new_shape = [] + for i in range(len(data.shape) + len(axes)): + if i in axes: new_shape.append(1) + else: + new_shape.append(data.shape[ptr]) + ptr += 1 + return data.reshape(new_shape) + +def Gemm(A, B, C=None, alpha=1.0, beta=1.0, transA=0, transB=0): + ret = alpha * ((A.transpose() if transA == 1 else A) @ (B.transpose() if transB == 1 else B)) + if C is not None: ret += beta * C + return ret + +# TODO: this is copied from tinygrad/nn/__init__.py +# spatial is from opset 7 and has since been removed +def BatchNormalization(X, scale, B, input_mean, input_var, epsilon=1e-05, momentum=0.9, training_mode=0, spatial=1): + if training_mode: + x_detached = X.detach() + current_mean = x_detached.mean(axis=(0,2,3)) + y = (x_detached - current_mean.reshape(shape=[1, -1, 1, 1])) + current_var = (y*y).mean(axis=(0,2,3)) + current_invstd = current_var.add(epsilon).pow(-0.5) + + running_mean = input_mean * momentum + current_mean * (1 - momentum) + running_var = input_var * momentum + current_var * (1 - momentum) + + return X.batchnorm(scale, B, current_mean, current_invstd), running_mean, running_var + else: + invstd = (input_var + epsilon)**-0.5 + return X.batchnorm(scale, B, input_mean, invstd) + +def LayerNormalization(x: Tensor, scale, bias, axis=-1, epsilon=1e-05, stash_type=1): + assert stash_type == 1, "only float32 is supported" + axis = tuple(i for i in range(axis if axis >= 0 else len(x.shape) + axis, len(x.shape))) + mean = x.mean(axis=axis, keepdim=True) + return x.layernorm(axis, epsilon).mul(scale).add(bias), mean, (x.sub(mean)).pow(2).mean(axis=axis, keepdim=True).add(epsilon).sqrt().reciprocal() + +def GroupNormalization(x: Tensor, scale: Tensor, bias: Tensor, num_groups, epsilon=1e-05): + return x.reshape(x.shape[0], num_groups, -1).layernorm(axis=-1, eps=epsilon).mul(scale.unsqueeze(-1)).add(bias.unsqueeze(-1)).reshape(x.shape) + +# TODO: expand to N-D +def _padding(X, pads=None, auto_pad="NOTSET"): + assert auto_pad == "NOTSET" # TODO: write this + if pads is not None: + return X.pad2d((pads[1], pads[3], pads[0], pads[2])) + else: + return X + +def AveragePool(X, kernel_shape, auto_pad="NOTSET", ceil_mode=0, count_include_pad=0, dilations=1, pads=None, strides=1): + # TODO: the padding shouldn't be counted in the average! this is causing a test failure + assert ceil_mode == 0 and dilations == 1 + padding_included = _padding(X, pads, auto_pad).avg_pool2d(kernel_shape, stride=strides) + if count_include_pad: + return padding_included + else: + div = _padding(Tensor.ones(*X.shape), pads, auto_pad).avg_pool2d(kernel_shape, stride=strides) + return padding_included / div + +def MaxPool(X, kernel_shape, auto_pad="NOTSET", ceil_mode=0, dilations=1, pads=None, storage_order=0, strides=1): + # TODO: the padding should be infinity, not 0! + assert ceil_mode == 0 and storage_order == 0 and dilations == 1 + return _padding(X, pads, auto_pad).max_pool2d(kernel_shape, stride=strides) + +def Conv(X, W, B=None, auto_pad="NOTSET", dilations=1, group=1, kernel_shape=None, pads=None, strides=1): + return X.conv2d(W, B, stride=strides, groups=group, dilation=dilations, padding=(pads[1], pads[3], pads[0], pads[2]) if pads is not None else 0) + +# TODO: copied from tensor.py +def Dropout(data, ratio=0.5, training_mode=False, seed=None): + # TODO: mask should be a boolean tensor + if not training_mode: return data, Tensor.ones(*data.shape) # if mask is requested as output it will contain all ones. + if seed is not None: Tensor.manual_seed(seed) + _mask : np.ndarray = np.asarray(Tensor._rng.binomial(1, 1.0-ratio, size=data.shape), dtype=data.dtype) + mask = Tensor(_mask, requires_grad=False, device=data.device) + return data * mask * (1/(1.0 - ratio)), mask + +def Shape(data, end=None, start=0): return list(data.shape)[start:end] +def Size(data): return prod(data.shape) + +# TODO: this doesn't match Tensor.flatten behavior +def Flatten(input, axis=1): + new_shape = (1, -1) if axis == 0 else (prod(input.shape[0:axis]), -1) + return input.reshape(new_shape) + +# TODO: abstract out the broadcast logic in tensor +def Expand(input, shape): + x_shape, y_shape = input.shape, [int(x) for x in safe_numpy(shape)] + # copied from _broadcasted + x_shape, y_shape = [([1]*(max(len(x_shape), len(y_shape))-len(t_shape)) + list(t_shape)) for t_shape in [x_shape, y_shape]] + shape_ret = tuple(max(sx, sy) for sx,sy in zip(x_shape, y_shape)) + return input.reshape(x_shape).expand(shape_ret) + +def LRN(input, size, alpha=1e-4, beta=0.75, bias=1.0): + bs, c, iy, ix = input.shape + return input / input.mul(input).reshape(bs,1,c,iy*ix).pad2d((0,0,(size-1)//2, size//2)).avg_pool2d((size, 1), 1).reshape(bs,c,iy,ix).mul(alpha).add(bias).pow(beta) + +def Identity(input): return input +def Neg(input): return -input +def Reciprocal(input): return input.reciprocal() +def Sqrt(input): return input.sqrt() +def Sign(input): return input.sign() +def Softsign(input): return input / (1+input.abs()) +def Abs(input): return input.abs() +def Exp(input): return input.exp() +def Log(input): return input.log() +def Mish(input): return input.mish() +def HardSigmoid(input, alpha=0.2, beta=0.5): return (alpha*input + beta).clip(0, 1) +def HardSwish(input): return input * HardSigmoid(input, 1/6, 0.5) +def Celu(X, alpha=1.0): return X.relu() - (-alpha*(X/alpha).exp()+1).relu() +def Selu(X, alpha=1.67326319217681884765625, gamma=1.05070102214813232421875): return gamma * (X.relu() - (-alpha*X.exp()+alpha).relu()) +def Softplus(X): return X.softplus() +def PRelu(X, slope): return X.leakyrelu(slope) +def LeakyRelu(X, alpha=0.01): return X.leakyrelu(alpha) +def ThresholdedRelu(X, alpha=1.0): return (X-alpha).relu() + (X-alpha).relu().sign() * alpha +def Softmax_1(input, axis=1): return input.softmax(axis) +def Softmax_13(input, axis=-1): return input.softmax(axis) +Softmax = {1: Softmax_1, 13: Softmax_13} # Softmax default axis changed +def LogSoftmax(input, axis=-1): return input.log_softmax(axis) +def Clip(input, min=-3.4e38, max=3.4e38): return input.clip(min, max) + +def Less(x, y): return (xy).numpy().astype(bool) +def GreaterOrEqual(x, y): return (x>=y).numpy().astype(bool) +def Equal(x, y): return (x.eq(y)).numpy().astype(bool) + +def Max(*data_0): return functools.reduce(Tensor.maximum, data_0) +def Min(*data_0): return -functools.reduce(Tensor.maximum, [-x for x in data_0]) +def Sum(*data_0): return functools.reduce(Tensor.__add__, data_0) +def Mean(*data_0): return functools.reduce(Tensor.__add__, data_0) / len(data_0) + +def _axes(axes, noop_with_empty_axes): return [int(x) for x in safe_numpy(axes)] if axes is not None else ([] if noop_with_empty_axes else None) + +# ReduceProd would require a new llop +def ReduceMax(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.max(_axes(axes, noop_with_empty_axes), keepdim=keepdims) +def ReduceMin(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.min(_axes(axes, noop_with_empty_axes), keepdim=keepdims) +def ReduceSum(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims) +def ReduceMean(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.mean(_axes(axes, noop_with_empty_axes), keepdim=keepdims) +def ReduceSumSquare(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.square().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims) +def ReduceL1(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.abs().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims) +def ReduceL2(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.square().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).sqrt() +def ReduceLogSum(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).log() +def ReduceLogSumExp(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.exp().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).log() + +def GlobalAveragePool(X): return X.mean(axis=tuple(range(2, len(X.shape))), keepdim=True) +def GlobalMaxPool(X): return X.max(axis=tuple(range(2, len(X.shape))), keepdim=True) + +def Tile(input, repeats): + repeats_ = [int(x) for x in safe_numpy(repeats)] + new_shape = [x for i in range(len(input.shape)) for x in [1,input.shape[i]]] + expand_shape = [x for r,s in zip(repeats_, input.shape) for x in [r,s]] + final_shape = [r*s for r,s in zip(repeats_, input.shape)] + return input.reshape(new_shape).expand(expand_shape).reshape(final_shape) + +def Range(start, limit, delta): return Tensor.arange(safe_numpy(limit)[0], safe_numpy(start)[0], safe_numpy(delta)[0]) +def Where(condition, X, Y): return condition*X + (1-condition)*Y + +def ConstantOfShape(input, value=0.0): + shape = [int(x) for x in safe_numpy(input)] + return Tensor.ones(*shape) * value + +# this is obviously wrong, but since we don't have types, it's better than nothing +def Cast(input, to): + print(f"WARNING: attempting to cast to {to}") + return input + +# NOTE: since we only have one type, this is valid! +def CastLike(input, target_type): + assert isinstance(target_type, Tensor), "can only CastLike Tensor" + return input diff --git a/tinygrad_repo/extra/thneed.py b/tinygrad_repo/extra/thneed.py index d398eafe4..b9b896ca6 100644 --- a/tinygrad_repo/extra/thneed.py +++ b/tinygrad_repo/extra/thneed.py @@ -1,17 +1,17 @@ # this can be constructed from a cl_cache or loaded from a thneed file -import os import time import struct import json import traceback import numpy as np -from tinygrad.llops.ops_gpu import CL, CLProgram -from tinygrad.helpers import prod +from tinygrad.runtime.ops_gpu import CLProgram +from tinygrad.helpers import prod, getenv from collections import defaultdict import pyopencl as cl +from tinygrad.runtime.ops_gpu import CL, OSX_TIMING_RATIO -DEBUGCL = int(os.getenv("DEBUGCL", 0)) -FLOAT16 = int(os.getenv("FLOAT16", 0)) +DEBUGCL = getenv("DEBUGCL", 0) +FLOAT16 = getenv("FLOAT16", 0) class Thneed: def __init__(self, cl_cache=[], inputs={}): @@ -74,29 +74,29 @@ class Thneed: if o['arg_type'] == "image2d_t": if 'buffer_id' in o and o['height'] == 1 and not bufs_loaded[o['buffer_id']]: # hack: use a image1d since we can back that with a buffer - buf = cl.Image(CL().cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']]) + buf = cl.Image(CL.cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']]) else: # buffer isn't supported in image2d, copy buffer into image if 'buffer_id' in o and bufs_loaded[o['buffer_id']]: arr = np.zeros(bufs[o['buffer_id']].size // 2, dtype=np.float16) cl.enqueue_copy(q, arr, bufs[o['buffer_id']]) - buf = cl.Image(CL().cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt, + buf = cl.Image(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt, shape=(o['width'], o['height']), pitches=(o['row_pitch'],), hostbuf=arr) elif o['needs_load']: - buf = cl.Image(CL().cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt, + buf = cl.Image(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt, shape=(o['width'], o['height']), pitches=(o['row_pitch'],), hostbuf=o['data']) else: - buf = cl.Image(CL().cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'], o['height'])) + buf = cl.Image(CL.cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'], o['height'])) if o['arg_type'] == "image1d_t": assert not o['needs_load'] assert not bufs_loaded[o['buffer_id']] - buf = cl.Image(CL().cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']]) + buf = cl.Image(CL.cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']]) else: if 'data' in o: - buf = cl.Buffer(CL().cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=o['data']) + buf = cl.Buffer(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=o['data']) else: # zero out buffers - buf = cl.Buffer(CL().cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=b'\x00'*o['size']) + buf = cl.Buffer(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=b'\x00'*o['size']) bufs[o['id']] = buf bufs_loaded[o['id']] = 'data' in o @@ -118,7 +118,7 @@ class Thneed: # load binaries for o in jdat['binaries']: nptr = ptr + o['length'] - prgs[o['name']] = CLProgram(o['name'], weights[ptr:nptr], rename=False, binary=True) + prgs[o['name']] = CLProgram(o['name'], weights[ptr:nptr], binary=True) ptr = nptr # populate the cl_cache @@ -193,17 +193,17 @@ class Thneed: }) if needs_load: data = np.empty(a.size//4, dtype=np.float32) - cl.enqueue_copy(CL().cl_queue, data, a, is_blocking=True) + cl.enqueue_copy(CL.cl_queue, data, a, is_blocking=True) weights.append(data.tobytes()) elif isinstance(a, cl.Image): needs_load = a in self.buffers_to_save row_pitch = (a.shape[0]*4*(2 if FLOAT16 else 4) + 63)//64 * 64 size = row_pitch * a.shape[1] # this is *2 if float16 and *4 if float32 - buf = cl.Buffer(CL().cl_ctx, cl.mem_flags.READ_WRITE, size=size * (2 if FLOAT16 else 1)) + buf = cl.Buffer(CL.cl_ctx, cl.mem_flags.READ_WRITE, size=size * (2 if FLOAT16 else 1)) # zero out the buffer - cl.enqueue_copy(CL().cl_queue, buf, b'\x00'*buf.size, is_blocking=True) + cl.enqueue_copy(CL.cl_queue, buf, b'\x00'*buf.size, is_blocking=True) CLProgram("from_image_strided", """ __kernel void from_image_strided(read_only image2d_t in, __global float4 *out, int row_pitch) { @@ -223,7 +223,7 @@ class Thneed: if needs_load: data = np.empty(size//(2 if FLOAT16 else 4), dtype=np.float32) - cl.enqueue_copy(CL().cl_queue, data, buf, is_blocking=True) + cl.enqueue_copy(CL.cl_queue, data, buf, is_blocking=True) if FLOAT16: data = data.astype(np.float16) weights.append(data.tobytes()) else: @@ -270,75 +270,23 @@ class Thneed: events = [] st = time.monotonic() for prg, args in self.cl_cache: - events.append(prg.clprg(CL().cl_queue, *args)) + events.append(prg.clprg(CL.cl_queue, *args)) mt = time.monotonic() - CL().cl_queue.finish() + CL.cl_queue.finish() et = time.monotonic() - st print(f"submit in {(mt-st)*1000.0:.2f} ms, total runtime is {et*1000.0:.2f} ms") + if DEBUGCL >= 2: + for i, ((prg, args), e) in enumerate(zip(self.cl_cache, events)): + print(f"{i:3d} {prg.name:20s} " + "queued @ %5.2f ms, submit @ %5.2fms, start @ %5.2f ms, end @ %5.2f ms" % tuple((x*OSX_TIMING_RATIO - st*1e9)/1e6 for x in [e.profile.queued, e.profile.submit, e.profile.start, e.profile.end])) if DEBUGCL >= 1: - scale_total_runtime = sum([(e.profile.end - e.profile.start) for e in events])/1e9 - # TODO: Mac OS has a scaling issue, this hack fixes it - scale = 1 if (et/scale_total_runtime) < 10 else (et/scale_total_runtime) total_runtime = 0 for i, ((prg, args), e) in enumerate(zip(self.cl_cache, events)): - runtime = (e.profile.end - e.profile.start)*scale - print(f"{i:3d} time {total_runtime/1e6:5.2f} ms running {prg.name:20s} with {str(args[0]):15s} {str(args[1]):15s} count {len(args)-2:2d} runtime {runtime/1e3:7.2f} us {(prg.op_estimate)/runtime:9.2f} GFLOPS {prg.options} -> {args[2].shape if hasattr(args[2], 'shape') else args[2].size}") - if (DEBUGCL >= 2 and int(os.getenv("PRINT_KERNEL", "-1")) == i) or DEBUGCL >= 3: + runtime = (e.profile.end - e.profile.start) * OSX_TIMING_RATIO + print(f"{i:3d} time {total_runtime/1e6:5.2f} ms running {prg.name:20s} with {str(args[0]):15s} {str(args[1]):15s} count {len(args)-2:2d} runtime {runtime/1e3:7.2f} us {(getattr(prg, 'op_estimate', float('nan')))/runtime:9.2f} GFLOPS -> {args[2].shape if hasattr(args[2], 'shape') else args[2].size}") + if hasattr(prg, 'prg') and ((DEBUGCL >= 2 and getenv("PRINT_KERNEL", -1) == i) or DEBUGCL >= 3): print(prg.prg) total_runtime += runtime print(f"total runtime: {total_runtime/1e6:.2f} ms wall time: {et*1000.0:.2f} ms") + return total_runtime/1e9 return et - - def optimize_local_workgroup(self): - MAX_WORKGROUP = CL.cl_ctx.devices[0].max_work_group_size - local_cl_cache = [] - for prg, args in self.cl_cache: - potential_locals = [tuple(args[1])] if args[1] is not None else [] - runtimes = [] - args = list(args) - - if args[1] is None and len(args[0]) == 1: - for l1 in [args[0][0], 1, 4, 16, MAX_WORKGROUP//4, MAX_WORKGROUP]: - potential_locals.append((l1,)) - - if args[1] is None and len(args[0]) == 2: - for l2 in [1, 4, 16, MAX_WORKGROUP//4, MAX_WORKGROUP]: - potential_locals.append((min(MAX_WORKGROUP, args[0][0]), l2)) - - if args[1] is None and len(args[0]) == 3: - for l2 in [16,args[0][1],MAX_WORKGROUP]: - for l3 in [4,16,args[0][2],MAX_WORKGROUP]: - for l1 in [max(1, MAX_WORKGROUP//(l2*l3)), args[0][0], 4, 16, MAX_WORKGROUP]: - if l1 > args[0][0] or l2 > args[0][1] or l3 > args[0][2]: continue - potential_locals.append((l1, l2, l3)) - - # TODO: why does this introduce a bug? - #if args[1] is not None and len(args[0]) == 3: - # for l2 in [args[1][1], args[0][1]]: # might be required, whole thing is always safe - # for l3 in [4,16,args[0][2],MAX_WORKGROUP,args[1][2]]: - # potential_locals.append((args[1][0], l2, l3)) - - for local_args in potential_locals: - if prod(local_args) > MAX_WORKGROUP: continue - args[1] = local_args - # 3 runs just in case - for i in range(3): - try: - e = prg.clprg(CL().cl_queue, *args) - except (cl.LogicError, cl.RuntimeError): - # INVALID_WORK_GROUP_SIZE - continue - CL().cl_queue.finish() - runtime = e.profile.end - e.profile.start - #print(runtime, args[0], args[1]) - runtimes.append((runtime, local_args)) - - if len(runtimes) > 0: - args[1] = sorted(runtimes)[0][1] - else: - args[1] = None - print("couldn't optimize", args[0]) - - local_cl_cache.append((prg, args)) - self.cl_cache = local_cl_cache diff --git a/tinygrad_repo/extra/utils.py b/tinygrad_repo/extra/utils.py index 23f1e9030..dc124f344 100644 --- a/tinygrad_repo/extra/utils.py +++ b/tinygrad_repo/extra/utils.py @@ -1,29 +1,31 @@ -from tinygrad.tensor import Tensor import pickle import numpy as np -from tinygrad.helpers import prod +from tqdm import tqdm +import tempfile +from tinygrad.helpers import prod, getenv def fetch(url): if url.startswith("/"): with open(url, "rb") as f: - dat = f.read() - return dat - import requests, os, hashlib, tempfile + return f.read() + import os, hashlib, tempfile fp = os.path.join(tempfile.gettempdir(), hashlib.md5(url.encode('utf-8')).hexdigest()) - if os.path.isfile(fp) and os.stat(fp).st_size > 0 and os.getenv("NOCACHE", None) is None: - with open(fp, "rb") as f: - dat = f.read() - else: - print("fetching %s" % url) - r = requests.get(url) - assert r.status_code == 200 - dat = r.content - with open(fp+".tmp", "wb") as f: - f.write(dat) - os.rename(fp+".tmp", fp) - return dat + download_file(url, fp, skip_if_exists=not getenv("NOCACHE")) + with open(fp, "rb") as f: + return f.read() -from tinygrad.nn.optim import get_parameters +def download_file(url, fp, skip_if_exists=False): + import requests, os + if skip_if_exists and os.path.isfile(fp) and os.stat(fp).st_size > 0: + return + r = requests.get(url, stream=True) + assert r.status_code == 200 + progress_bar = tqdm(total=int(r.headers.get('content-length', 0)), unit='B', unit_scale=True, desc=url) + with tempfile.NamedTemporaryFile(delete=False) as f: + for chunk in r.iter_content(chunk_size=16384): + progress_bar.update(f.write(chunk)) + f.close() + os.rename(f.name, fp) def my_unpickle(fb0): key_prelookup = {} diff --git a/tinygrad_repo/openpilot/compile.py b/tinygrad_repo/openpilot/compile.py index 065b1c550..e51f99457 100644 --- a/tinygrad_repo/openpilot/compile.py +++ b/tinygrad_repo/openpilot/compile.py @@ -1,14 +1,17 @@ #!/usr/bin/env python3 -import os, time, io, pathlib, sys +import os, time, io, pathlib, sys, traceback sys.path.insert(0, str(pathlib.Path(__file__).parent.parent)) if os.getenv("OPT", None) is None: os.environ['OPT'] = '99' if os.getenv("GPU", None) is None: - os.environ['OPENCL'] = '1' + os.environ['GPU'] = '1' +if os.getenv("IMAGE", None) is None: + os.environ['IMAGE'] = '2' -ALLOWED_KERNEL_COUNT = int(os.getenv("ALLOWED_KERNEL_COUNT", 0)) -DEBUGCL = int(os.getenv("DEBUGCL", 0)) +from tinygrad.helpers import getenv +ALLOWED_KERNEL_COUNT = getenv("ALLOWED_KERNEL_COUNT", 0) +DEBUGCL = getenv("DEBUGCL", 0) import onnx import numpy as np @@ -16,7 +19,8 @@ import numpy as np import tinygrad.graph as graph from tinygrad.ops import GlobalCounters -from tinygrad.llops.ops_gpu import CL +import pyopencl as cl +from tinygrad.runtime.ops_gpu import CL from extra.utils import fetch from extra.onnx import get_run_onnx from tinygrad.tensor import Tensor @@ -25,106 +29,73 @@ OPENPILOT_MODEL = "https://github.com/commaai/openpilot/raw/6c5693e965b9c63f8678 np.random.seed(1337) def get_random_input_tensors(input_shapes): - np_inputs = { - "input_imgs": np.random.randn(*(1, 12, 128, 256))*256, - "big_input_imgs": np.random.randn(*(1, 12, 128, 256))*256, - "desire": np.zeros((1,100, 8)), - "traffic_convention": np.array([[1., 0.]]), - #"features_buffer": np.random.randn(*(1, 99, 128)) - "features_buffer": np.random.randn(*input_shapes['features_buffer']) - #"initial_state": np.zeros((1, 768)) - } - if int(os.getenv("ZERO_OUT", "0")): - np_inputs = {k:v*0 for k,v in np_inputs.items()} - - for k,v in np_inputs.items(): - assert v.shape == input_shapes[k], f"{k} shape mismatch, {v.shape} {input_shapes[k]}" - - #import pickle - #frames, big_frames, last_state, frame_inputs, policy_outs = pickle.load(open("openpilot/test/frame_0.pkl", "rb")) - #np_inputs["input_imgs"] = frames - #np_inputs["big_input_imgs"] = big_frames - #np_inputs["initial_state"] = last_state[0] - - #for i,k in enumerate(np_inputs.keys()): - # dat = open("/home/batman/openpilot/xx/ml_tools/snpe/compile_test_data/dlc_input_%d" % i, "rb").read() - # np_inputs[k] = np.frombuffer(dat, np.float32).reshape(np_inputs[k].shape) - - np_inputs = {k:v.astype(np.float32) for k,v in np_inputs.items()} - inputs = {k:Tensor(v.astype(np.float32), requires_grad=False) for k,v in np_inputs.items()} - for _,v in inputs.items(): v.realize() + # this 16 is a random scale factor + inputs = {k:Tensor.randn(*shp, requires_grad=False)*8 for k,shp in input_shapes.items()} + np_inputs = {k:v.realize().numpy() for k,v in inputs.items()} return inputs, np_inputs +from tinygrad.jit import TinyJit + +@TinyJit +def model_exec(run_onnx, using_graph, **inputs): + ret = next(iter(run_onnx(inputs).values())) + GlobalCounters.reset() + GlobalCounters.cache = [] # don't cache pre-realize + if using_graph: graph.GRAPH = True + print("realizing") + return ret.realize() + def compile(dat, output_fn): + Tensor.manual_seed(1337) Tensor.no_grad = True using_graph = graph.GRAPH graph.GRAPH = False onnx_model = onnx.load(io.BytesIO(dat)) run_onnx = get_run_onnx(onnx_model) + input_shapes = {inp.name:tuple(x.dim_value for x in inp.type.tensor_type.shape.dim) for inp in onnx_model.graph.input} - input_shapes = {} - for inp in onnx_model.graph.input: - input_shapes[inp.name] = tuple(x.dim_value for x in inp.type.tensor_type.shape.dim) - - inputs, _ = get_random_input_tensors(input_shapes) - - # initial run(s) to load weights - for _ in range(2): - st = time.monotonic() - tinygrad_out = next(iter(run_onnx(inputs).values())) - mt = time.monotonic() - tinygrad_out.realize() - mt2 = time.monotonic() - tinygrad_out = tinygrad_out.numpy() - et = time.monotonic() - print(f"ran openpilot model in {(et-st)*1000.0:.2f} ms, waited {(mt2-mt)*1000.0:.2f} ms for realize, {(et-mt2)*1000.0:.2f} ms for GPU queue") - - # realize all non GCed tensors (fix for batchnorm folding) - import gc - gc.collect() - for x in [x for x in gc.get_objects() if isinstance(x, Tensor)]: - x.realize() - - # real run inputs, np_inputs = get_random_input_tensors(input_shapes) - print("***** REAL RUN *****") - tinygrad_out = next(iter(run_onnx(inputs).values())) - - # note, since CL.CACHE is enabled, it doesn't actually run the kernels - start_ops = GlobalCounters.global_ops - CL.CACHE = [] - if using_graph: graph.GRAPH = True - CL.kernel_count = -1 - tinygrad_out.realize() + # run twice to trigger the JIT + for i in range(2): tinygrad_out = model_exec(run_onnx, i == 1 and using_graph, **inputs) graph.GRAPH = False - print("kernel count:", len(CL.CACHE)) - assert len(CL.CACHE) <= ALLOWED_KERNEL_COUNT or ALLOWED_KERNEL_COUNT == 0, "too many kernels!" - used_ops = GlobalCounters.global_ops - start_ops + print("kernel count:", len(model_exec.jit_cache)) + assert len(model_exec.jit_cache) <= ALLOWED_KERNEL_COUNT or ALLOWED_KERNEL_COUNT == 0, "too many kernels!" + + # pull out inputs and put them in the jit cache + input_rawbuffers = {k:inputs[k].lazydata.realized.raw() for k in inputs.keys()} + for (j,i),idx in model_exec.input_replace.items(): model_exec.jit_cache[j][1][i] = input_rawbuffers[idx] + + # transform to CL.CACHE + used_ops = 0 + cl_cache = [] + for prg,args in model_exec.jit_cache: + # pass these to thneed + setattr(prg.clprg, 'op_estimate', prg.op_estimate) + setattr(prg.clprg, 'prg', prg.prg) + cl_cache.append((prg.clprg, [prg.global_size, prg.local_size, *[x._cl for x in args]])) + used_ops += prg.op_estimate from extra.thneed import Thneed - t = Thneed(CL.CACHE, {k:inputs[k].lazydata.realized.cl for k in inputs.keys()}) - CL.CACHE = None - if int(os.getenv("OPTWG", "0")): - t.optimize_local_workgroup() + t = Thneed(cl_cache, {k:v._cl for k,v in input_rawbuffers.items()}) # save thneed (before run) t.save(output_fn) - print(f"buffers to save: {len(t.buffers_to_save)}, outputs: {t.outputs}") + print(f"buffers to save: {len(t.buffers_to_save)}, inputs: {list(t.inputs.keys())}, outputs: {t.outputs}") runtime = t.run() print(f"network using {used_ops/1e9:.2f} GOPS with runtime {runtime*1e3:.2f} ms that's {used_ops/runtime*1e-9:.2f} GFLOPS") # confirm thneed found the right output thneed_out = np.empty((t.outputs[0].size//4,), dtype=np.float32).reshape(tinygrad_out.shape) - CL.enqueue_copy(thneed_out, t.outputs[0], is_blocking=True) + cl.enqueue_copy(CL.cl_queue, thneed_out, t.outputs[0], is_blocking=True) np.testing.assert_allclose(thneed_out, tinygrad_out.numpy()) - # float32 only (fix this) - FLOAT16 = int(os.getenv("FLOAT16", 0)) + # testing is float32 only (fix this) + FLOAT16 = getenv("FLOAT16", 0) if FLOAT16 == 0: try: - from test.test_onnx import run_onnx_torch + from test.models.test_onnx import run_onnx_torch torch_out = run_onnx_torch(onnx_model, np_inputs).numpy() print(thneed_out, torch_out, "mse", np.sum((thneed_out-torch_out)**2), "max err", np.max(np.abs((thneed_out-torch_out)))) np.testing.assert_allclose(torch_out, thneed_out, atol=1e-4, rtol=1e-2) @@ -135,11 +106,11 @@ def compile(dat, output_fn): # try old thneed with a different input for k,v in t.inputs.items(): - CL.enqueue_copy(v, new_np_inputs[k], is_blocking=True) + cl.enqueue_copy(CL.cl_queue, v, new_np_inputs[k], is_blocking=True) t.run() old_thneed_out = np.empty((t.outputs[0].size//4,), dtype=np.float32).reshape(tinygrad_out.shape) - CL.enqueue_copy(old_thneed_out, t.outputs[0], is_blocking=True) + cl.enqueue_copy(CL.cl_queue, old_thneed_out, t.outputs[0], is_blocking=True) # compare thneed (rerun) with torch np.testing.assert_allclose(new_torch_out, old_thneed_out, atol=1e-4, rtol=1e-2) @@ -152,18 +123,17 @@ def compile(dat, output_fn): # inputs for k,v in nt.inputs.items(): - CL.enqueue_copy(v, new_np_inputs[k], is_blocking=True) + cl.enqueue_copy(CL.cl_queue, v, new_np_inputs[k], is_blocking=True) nt.run() new_thneed_out = np.empty((nt.outputs[0].size//4,), dtype=np.float32).reshape(tinygrad_out.shape) - CL.enqueue_copy(new_thneed_out, nt.outputs[0], is_blocking=True) + cl.enqueue_copy(CL.cl_queue, new_thneed_out, nt.outputs[0], is_blocking=True) # compare torch to thneed np.testing.assert_allclose(new_torch_out, new_thneed_out, atol=1e-4, rtol=1e-2) print("thneed self-test passed!") - except ModuleNotFoundError: - pass - + except ModuleNotFoundError as e: + print(f"TEST NOT HAPPENING {e}") # UNSAFE_FLOAT4=1 DEBUGCL=1 FLOAT16=1 python3 openpilot/compile.py diff --git a/tinygrad_repo/tinygrad/__init__.py b/tinygrad_repo/tinygrad/__init__.py deleted file mode 100644 index e49d8ccd5..000000000 --- a/tinygrad_repo/tinygrad/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from tinygrad import tensor, nn # noqa: F401 diff --git a/tinygrad_repo/tinygrad/ast.py b/tinygrad_repo/tinygrad/ast.py deleted file mode 100644 index 1927ad4d9..000000000 --- a/tinygrad_repo/tinygrad/ast.py +++ /dev/null @@ -1,164 +0,0 @@ -from enum import Enum -import itertools -from typing import List, Tuple -from tinygrad.helpers import prod, dedup, all_same -from tinygrad.ops import LazyOp, MovementOps, get_lazyop_info, get_buffers, ReduceOps, get_lazyops -from tinygrad.shape import ShapeTracker - -def get_first_reduce(shapes): - for i in range(len(shapes[0])): - if not all_same([x[i] for x in shapes]): - return i - return len(shapes[0]) # off the end - -Types = Enum("Types", ["FLOAT", "FLOAT4"]) -class Token: - def __init__(self, tok:str, typ:Types, ptr:bool=False): - assert isinstance(tok, str) - self.tok, self.typ, self.ptr = tok, typ, ptr - self.axis : List[Tuple[int, int]] = [] - def array(self, length, stride): self.axis.append((length, stride)) - def size(self): return prod(x[0] for x in self.axis) - def offsets(self): return [sum(t) for t in itertools.product(*[[y*x[1] for y in range(x[0])] for x in self.axis[::-1]])] if len(self.axis) else [0] - def decltype(self): return ('float' if self.typ == Types.FLOAT else 'float4') + ('*' if self.ptr else '') - def __repr__(self): return f"<{self.typ}{'*' if self.ptr else ''} {self.tok}{f'[{self.axis}]' if len(self.axis) else ''}>" - -# ast kernel can contain one ReduceOp with arbitrary Binary/Unary ops -class ASTKernel: - def __init__(self, ast:LazyOp): - # key for lookup in cache (can change, str might not be right) - self.input_ast = ast - self.key = str(ast) - - # if the AST ends with a RESHAPE, we remove it and create the buffer accordingly - if ast.op == MovementOps.RESHAPE: - output_shape = ast.arg - ast = ast.src[0] - else: - output_shape = None - - self.info = get_lazyop_info(ast) - self.bufs = dedup(get_buffers(ast)) - reduceops = [x for x in get_lazyops(ast) if x.op in ReduceOps] - assert len(dedup(reduceops)) <= 1, "max one reduce op in an ast" - self.reduceop = reduceops[0] if reduceops else None - self.earlybufs = dedup(get_buffers(self.reduceop)) if self.reduceop else [] - self.ast = ast - - # create the buffer we are returning (as the same type as the input buffers) and add it as the first buffer - self.ret = type(self.bufs[0])(output_shape if output_shape else self.info.shape) - if hasattr(self.ret, "cl"): self.ret.cl # does the allocation of unbacked buffer, pylint: disable=W0104 - self.bufs = [type(self.ret)(self.info.shape, hostbuf=self.ret)] + self.bufs - self.buftokens = [Token(f"data{i}", Types.FLOAT, ptr=True) for i in range(len(self.bufs))] - - # check valid AST kernel - assert all_same([x.shape for x in self.earlybufs]), "all earlybufs must have the same shape" - assert all_same([x.shape for x in self.bufs if x not in self.earlybufs]), "all latebufs must have the same shape" - assert all_same([len(x.shape) for x in self.bufs]), "all bufs must have the same shape size" - - def print(self): - buf_count = -1 - op_count = -1 - cache = {} - def print_ast(x, name=None): - nonlocal buf_count, op_count - if x not in cache: - if not isinstance(x, LazyOp): - if name is None: - buf_count += 1 - name = f"buf{buf_count}" - print(f"buf{buf_count} = {x}") - cache[x] = name - else: - srcs = [print_ast(y) for y in x.src] - if name is None: - op_count += 1 - name = f"op{op_count}" - print(f"{name} = LazyOp({str(x.op)}, ({','.join(srcs)},), {x.arg})") - cache[x] = name - return cache[x] - print_ast(self.input_ast, "ast") - - - def process(self): - # get shape, strides, and offset - # if it's a multiview buffer we take the final view - self.shapes = [x.shape for x in self.bufs] - self.strides = [x.st.views[-1].strides for x in self.bufs] - self.offsets = [x.st.views[-1].offset for x in self.bufs] # include the offsets (as is) - self.simplify_ones() - self.simplify_merge_adjacent() - - def simplify_ones(self): - # remove places where the shape is all ones - # TODO: this should be factored in to multi shape stride - all_ones = [all(s[i]==1 for s in self.shapes) for i in range(len(self.shapes[0]))] - # keep at least 1 one - if all(all_ones): - all_ones[-1] = False - self.shapes = [[s[i] for i in range(len(s)) if not all_ones[i]] for s in self.shapes] - self.strides = [[s[i] for i in range(len(s)) if not all_ones[i]] for s in self.strides] - # find first mismatch, don't reduce this - self.first_reduce = get_first_reduce(self.shapes) - - def simplify_merge_adjacent(self): - shapes, strides = self.shapes, self.strides - - # merge dimensions if we can, multi get_shape_strides - # TODO: does this always preserve the reduce dimension, NO - # TODO: move this into shapetracker, with tests! - rets = [[(shapes[j][0], strides[j][0])] for j in range(len(shapes))] - for i in range(1, len(shapes[0])): - can_merge = [] - for j in range(len(shapes)): - # TODO: added the always mergability of 1s, is this right? if so, add to shapetracker in the 1 case - can_merge.append((strides[j][i] != 0 and rets[j][-1][1] == shapes[j][i]*strides[j][i]) or (strides[j][i] == 0 and rets[j][-1][1] == 0)) - # more can merge than this - can_merge = all(can_merge) and i != self.first_reduce - for j in range(len(shapes)): - if can_merge: - rets[j][-1] = (rets[j][-1][0] * shapes[j][i], strides[j][i]) - else: - rets[j].append((shapes[j][i], strides[j][i])) - self.shapes, self.strides = [[y[0] for y in x] for x in rets], [[y[1] for y in x] for x in rets] - self.first_reduce = get_first_reduce(self.shapes) - - @property - def shape_len(self): return len(self.shapes[0]) - - # this should be aware of the three parts to the shape - # * the input/output dimensions - # * the reduce dimensions - # * the size outputted by each kernel - def reshape_and_permute(self, new_shape_fxn, axis): - new_shapes, new_strides = [], [] - for shape, stride in zip(self.shapes, self.strides): - st = ShapeTracker(tuple(shape)) - st.strided(*zip(shape, stride)) - # TODO: handle reduced shape here - if new_shape_fxn is not None: st.reshape(*new_shape_fxn(shape)) - if axis is not None: st.permute(*axis) - assert len(st.views) == 1 - new_shapes.append(st.shape) - new_strides.append(st.strides) - self.shapes, self.strides = new_shapes, new_strides - - # drops the final dimension - def upcast(self): - upcasted = [x[-1] for x in self.shapes if x[-1] != 1] - assert len(upcasted) >= 1 and all_same(upcasted), f"can't upcast mismatch {upcasted}" - for i in range(len(self.bufs)): - if self.shapes[i][-1] == upcasted[0]: - # multiview shapetrackers can slice through a float4, so don't allow them - can_merge = (not self.bufs[i].st.needs_valid() and len(self.bufs[i].st.views) == 1) or "Image" in str(type(self.bufs[i]._buf)) # TODO: terrible hack - if self.shapes[i][-1] == 4 and self.buftokens[i].typ == Types.FLOAT and self.strides[i][-1] == 1 and can_merge: - # this is an upcast to FLOAT4 - self.buftokens[i].typ = Types.FLOAT4 - assert all(x%upcasted[0] == 0 for x in self.strides[i][0:-1]) - assert self.offsets[i]%upcasted[0] == 0 - else: - self.buftokens[i].array(upcasted[0], self.strides[i][-1]) - - # remove the last dimension - self.shapes = [x[:-1] for x in self.shapes] - self.strides = [x[:-1] for x in self.strides] diff --git a/tinygrad_repo/tinygrad/codegen/ast.py b/tinygrad_repo/tinygrad/codegen/ast.py new file mode 100644 index 000000000..270bc9791 --- /dev/null +++ b/tinygrad_repo/tinygrad/codegen/ast.py @@ -0,0 +1,192 @@ +import itertools +from enum import Enum, auto +from typing import List, Tuple +from tinygrad.helpers import prod, dedup, all_same, colored +from tinygrad.ops import LazyOp, MovementOps, get_lazyop_info, get_buffers, ReduceOps, get_lazyops, map_buffers +from tinygrad.shape import ShapeTracker, View, strides_for_shape + +def get_first_reduce(shapes): + for i in range(len(shapes[0])): + if not all_same([x[i] for x in shapes]): return i + return len(shapes[0]) # off the end + +# this will be removed soon anyway +class Types(Enum): FLOAT = auto(); FLOAT4 = auto() # noqa: E702 +class Token: + def __init__(self, tok:str, typ:Types, ptr:bool=False): + assert isinstance(tok, str) + self.tok, self.typ, self.ptr = tok, typ, ptr + self.axis : List[Tuple[int, int, bool]] = [] + def array(self, length, stride, reduce): self.axis.append((length, stride, reduce)) + def size(self): return prod([x[0] for x in self.axis]) + def offsets(self): return [sum(t) for t in itertools.product(*[[y*x[1] for y in range(x[0])] for x in self.axis[::-1]])] if len(self.axis) else [0] + def can_float4(self): return any(a[0:2] == (4,1) for a in self.axis) + # TODO: this is sort of a hack, it gets the accumulator indices + def acc_offsets(self): + if len(self.axis) == 0: return [0] + acc_strides = [x*(1-self.axis[::-1][i][2]) for i,x in enumerate(strides_for_shape(tuple(1 if r else s for s,_,r in self.axis[::-1])))] + return [sum(t) for t in itertools.product(*[[y*acc_strides[i] for y in range(x[0])] for i,x in enumerate(self.axis[::-1])])] + def decltype(self): return ('float' if self.typ == Types.FLOAT else 'float4') + ('*' if self.ptr else str()) + def __repr__(self): return f"<{self.typ}{'*' if self.ptr else str()} {self.tok}{f'[{self.axis}]' if len(self.axis) else str()}>" + +# ast kernel can contain one ReduceOp with arbitrary Binary/Unary ops +class ASTKernel: + def __init__(self, ast:LazyOp, output_buffer=None): + self.input_ast = ast + + # if the AST ends with a RESHAPE, we remove it and create the buffer accordingly + if ast.op == MovementOps.RESHAPE: + output_shape = ast.arg + ast = ast.src[0] + else: + output_shape = None + + self.info = get_lazyop_info(ast) + self.bufs = dedup(get_buffers(ast)) + for b in self.bufs: b.st.simplify() + self.ast = ast + + # check if the output buffer is allowed to be used + # if it's aliased, don't use it + if output_buffer is not None: + for a in self.bufs: + if a._buf == output_buffer._buf and not a.st.contiguous: + output_buffer = None + break + + # create the buffer we are returning (as the same type as the input buffers) and add it as the first buffer + self.ret = output_buffer if output_buffer else type(self.bufs[0])(output_shape if output_shape else self.info.shape, force_create=True) + self.bufs = ([type(self.ret)(self.info.shape, hostbuf=self.ret)] if output_shape else [self.ret]) + self.bufs + + # key for lookup in cache (can change, str might not be right) + # bufs are needed because kernels like f(x) = x + x and f(x, y) = x + y have the same str(ast), but are different kernels. + # mapping the buffers to integers is required because a-b != b-a (and how would you tell a and b apart?) + self.key = f"ASTKernelKey ast={str(map_buffers({x:i for i,x in enumerate(self.bufs)}, ast))} bufs={self.bufs}" + + def process(self) -> None: + if hasattr(self, "sts"): return # already processed + + reduceops = [x for x in get_lazyops(self.ast) if x.op in ReduceOps] + assert len(dedup(reduceops)) <= 1, "max one reduce op in an ast" + self.reduceop = reduceops[0] if reduceops else None + self.earlybufs = dedup(get_buffers(self.reduceop)) if self.reduceop else [] + + self.buftokens = [Token(f"data{i}", Types.FLOAT, ptr=True) for i in range(len(self.bufs))] + self.group_for_reduce : List[int] = [] + + # check valid AST kernel + assert all_same([x.shape for x in self.earlybufs]), "all earlybufs must have the same shape" + assert all_same([x.shape for x in self.bufs if x not in self.earlybufs]), "all latebufs must have the same shape" + assert all_same([len(x.shape) for x in self.bufs]), "all bufs must have the same shape size" + + # process + self.sts : List[ShapeTracker] = [x.st.copy() for x in self.bufs] # create new shapetrackers inside this kernel + self.simplify_ones() + self.simplify_merge_adjacent() + + # get full shape buf index (earlybufs if there are any, otherwise output) + self.full_buf_index : int = self.bufs.index(self.earlybufs[0]) if len(self.earlybufs) > 0 else 0 + + def print(self): + buf_count, op_count, cache = -1, -1, {} + def print_ast(x, name=None): + nonlocal buf_count, op_count + if x not in cache: + if not isinstance(x, LazyOp): + if name is None: + buf_count += 1 + name = f"buf{buf_count}" + print(f"buf{buf_count} = {x}") + cache[x] = name + else: + srcs = [print_ast(y) for y in x.src] + if name is None: + op_count += 1 + name = f"op{op_count}" + print(f"{name} = LazyOp({str(x.op)}, ({','.join(srcs)},), {x.arg})") + cache[x] = name + return cache[x] + print_ast(self.input_ast, "ast") + + def printbufs(self, prefix="", print_shapetrackers=False): + print(f"first_reduce: {self.first_reduce} shape_len: {self.shape_len} group_for_reduce: {self.group_for_reduce}") + if print_shapetrackers: + for st in self.sts: print(st) + for i in range(len(self.sts)): + print(prefix, self.buftokens[i], f"early:{'T' if i < len(self.bufs) and self.bufs[i] in self.earlybufs else 'F'}", self.sts[i].shape, self.sts[i].views[-1].strides, len(self.sts[i].views), type(self.bufs[i]._buf) if self.bufs[i] is not None else "FAKE") + + @property + def shape_len(self) -> int: return len(self.sts[0].shape) + + @property + def full_shape(self) -> Tuple[int, ...]: return self.sts[self.full_buf_index].shape + + @property + def upcast_in_mid_reduce_axes(self): return [j for j in range(self.first_reduce, self.first_reduce+len(self.group_for_reduce)) if self.full_shape[j] == self.sts[0].shape[j]] + + def colorshape(self, pad=50) -> str: + axis = [(f"{rs:4d}", (("green" if i in self.upcast_in_mid_reduce_axes else "cyan") if i < self.first_reduce + len(self.group_for_reduce) else "red") if i >= self.first_reduce else "blue") for i, rs in enumerate(self.full_shape)] + axis += [(f"{s:4d}", 'magenta' if reduce else 'yellow') for s, _, reduce in self.buftokens[self.full_buf_index].axis[::-1]] + return ' '.join([colored(*x) for x in axis])+(" "*(pad-len(' '.join([x[0] for x in axis])))) + + def simplify_ones(self): + # remove places where the shape is all ones + # TODO: this should be factored in to multi shape stride + all_ones = [all(st.shape[i]==1 for st in self.sts) for i in range(self.shape_len)] + # keep at least 1 one + if all(all_ones): all_ones[-1] = False + self.reshape_and_permute(lambda shape: [x for i,x in enumerate(shape) if not all_ones[i]], None) + # find first mismatch, don't reduce this + self.first_reduce = get_first_reduce([x.shape for x in self.sts]) + + def simplify_merge_adjacent(self): + shapes, strides = [x.shape for x in self.sts], [x.views[-1].strides for x in self.sts] + + # merge dimensions if we can, multi get_shape_strides + # TODO: does this always preserve the reduce dimension, NO + # TODO: move this into shapetracker, with tests! + rets = [[(shapes[j][0], strides[j][0])] for j in range(len(shapes))] + for i in range(1, len(shapes[0])): + can_merge = [] + for j in range(len(shapes)): + # TODO: added the always mergeability of 1s, is this right? if so, add to shapetracker in the 1 case + can_merge.append((strides[j][i] != 0 and rets[j][-1][1] == shapes[j][i]*strides[j][i]) or (strides[j][i] == 0 and rets[j][-1][1] == 0)) + # more can merge than this + mergeable = all(can_merge) and i != self.first_reduce + for j in range(len(shapes)): + if mergeable: rets[j][-1] = (rets[j][-1][0] * shapes[j][i], strides[j][i]) + else: rets[j].append((shapes[j][i], strides[j][i])) + + for i,x in enumerate(rets): self.sts[i].reshape(tuple(y[0] for y in x)) + self.first_reduce = get_first_reduce([x.shape for x in self.sts]) + + # this should be aware of the three parts to the shape + # * the input/output dimensions + # * the reduce dimensions + # * the size outputted by each kernel + def reshape_and_permute(self, new_shape_fxn, axis): + for st in self.sts: + if new_shape_fxn is not None: st.reshape(tuple(new_shape_fxn(st.shape))) + if axis is not None: st.permute(tuple(axis)) + + # axis : the axis to pull from + # amount : the amount to take + # top : if you want to pull that amount from the top + # insert_before : place to insert the new stuff + def shift_to(self, axis, amount, top=False, insert_before=None): + if insert_before is None: insert_before = self.shape_len + move_axis = axis if top else axis+1 + if move_axis < insert_before: insert_before += 1 + self.reshape_and_permute( + lambda x: list(x[0:axis]) + (([amount, x[axis]//amount] if top else [x[axis]//amount, amount]) if x[axis] > 1 else [1,1]) + list(x[axis+1:]), + [i for i in range(insert_before) if i != move_axis] + [move_axis] + [i for i in range(insert_before, self.shape_len+1) if i != move_axis]) + + # drops the final dimension + def upcast(self): + upcasted = [x.shape[-1] for x in self.sts if x.shape[-1] != 1] + assert len(upcasted) >= 1 and all_same(upcasted), f"can't upcast mismatch {upcasted}" + for st,buftoken in zip(self.sts, self.buftokens): + # add last axis to the buftoken (if it's not a 1) + if st.shape[-1] == upcasted[0]: buftoken.array(st.shape[-1], st.views[-1].strides[-1], len(upcasted) != len(self.sts)) + # remove the last axis (unless it's the only dimension, then make it a 1) + st.views[-1] = View(st.shape[0:-1], st.views[-1].strides[0:-1], st.views[-1].offset) if len(st.shape) > 1 else View((1,), (0,), st.views[-1].offset) diff --git a/tinygrad_repo/tinygrad/codegen/gpu.py b/tinygrad_repo/tinygrad/codegen/gpu.py new file mode 100644 index 000000000..38b33a241 --- /dev/null +++ b/tinygrad_repo/tinygrad/codegen/gpu.py @@ -0,0 +1,340 @@ +import math, itertools +from collections import defaultdict +from typing import Optional, List, Tuple, Dict, Set, Final, NamedTuple +from tinygrad.ops import UnaryOps, BinaryOps, ReduceOps, LazyOp, Op, ASTRunner +from tinygrad.codegen.ast import ASTKernel, Token, Types +from tinygrad.shape.symbolic import Node, MulNode, DivNode, SumNode, Variable, render_python +from tinygrad.shape import ShapeTracker, View +from tinygrad.helpers import getenv, DEBUG, prod, partition, mnum, all_same, dedup + +# div is different in cl than python +render_cl = render_python.copy() +render_cl[DivNode] = lambda self,ops,ctx: f"({self.a.render(ops)}/{self.b})" + +VALIDHACKS = getenv("VALIDHACKS", 0) # TODO: remove the need for this +NATIVE_EXPLOG = getenv("NATIVE_EXPLOG", 0) # this is needed as a switch for the tests to pass + +class GPULanguage(NamedTuple): + kernel_prefix : str = "" + buffer_prefix : str = "" + buffer_suffix : str = "" + smem_prefix : str = "" + barrier : str = "" + gid : List[str] = [] + lid : List[str] = [] + extra_args : List[str] = [] + float4 : Optional[str] = None + +def to_image_idx(base_shape:Tuple[int, ...], idxy:Node, valid:Node, validhacks=False) -> Tuple[Node, Node]: + idy = (idxy//(4*base_shape[1])) + if validhacks and valid.min == 0: + idx = (idxy//4) + (idy*-base_shape[1]) + # find the ones in idx that didn't factorize and remove them (TODO: this is not universal) + if isinstance(idx, SumNode): + unfactored, idx_nodes = partition(idx.nodes, lambda x: isinstance(x, MulNode) and x.b == -base_shape[1]) + assert len(unfactored) <= 1 + idx = Variable.sum(idx_nodes) + unfactored = (Variable.sum(unfactored) // base_shape[1]) + idy += unfactored + # ugh really...handtuned garbage + if idx.min >= (base_shape[1]*3)//4: + idx -= base_shape[1] + idy += 1 + else: + idx = (idxy//4)%base_shape[1] + #print(base_shape, idx.min, idx.max, idy.min, idy.max, idx, idy) + return idx, idy + +class GPUCodegen(ASTKernel): + lang : GPULanguage = GPULanguage() + + # for renaming + kernel_cnt : Final[Dict[str, int]] = defaultdict(lambda: -1) + kernel_name_cache : Final[Dict[str, str]] = {} + + code_for_op : Final[Dict[Op, str]] = { + UnaryOps.NOOP: "(A)", UnaryOps.NEG: "(-(A))", UnaryOps.NOT: "(1.0f-A)", + UnaryOps.EXP: "native_exp(A)" if NATIVE_EXPLOG else "exp(A)", + UnaryOps.LOG: "native_log(A)" if NATIVE_EXPLOG else "log(A)", + BinaryOps.ADD: "(A+B)", BinaryOps.SUB: "(A-B)", BinaryOps.MUL: "(A*B)", + BinaryOps.DIV: "(A/B)", BinaryOps.POW: "pow(A,B)", BinaryOps.CMPEQ: "(A==B)", + BinaryOps.MAX: "max(A,B)", ReduceOps.SUM: "A+=B", ReduceOps.MAX: "A=max(A,B)" + } + start_for_op : Final[Dict[Op, str]] = {ReduceOps.SUM: "0.0f", ReduceOps.MAX: "-INFINITY"} + + def group_float4(self, grp:List[Token]) -> Token: + if all(g.tok.endswith(e) for g,e in zip(grp, [".x", ".y", ".z", ".w"])) and all_same([g.tok.split(".")[0] for g in grp]): return Token(grp[0].tok.split(".")[0], Types.FLOAT4) + else: return Token(f"{self.lang.float4}({','.join(g.tok for g in grp)})", Types.FLOAT4) + + def store(self, buf_index:int, value:List[Token]) -> None: + assert len(value) == self.buftokens[buf_index].size(), f"size mismatch {len(value)} != {self.buftokens[buf_index].size()}" + assert len(self.sts[buf_index].views) == 1, "store has more than one view" + + # all stores can merge, since they have one view and are valid + should_upcast = self.lang.float4 and self.buftokens[buf_index].can_float4() + + to_store = {o:v for o,v in zip(self.buftokens[buf_index].offsets(), value)} + did_store = set() + for o,v in to_store.items(): + if o in did_store: continue + idxy, valid = self.sts[buf_index].expr_idxs(o) + assert valid.min == 1, "store must always be valid" + if should_upcast: + for j in range(4): did_store.add(o+j) + v = self.group_float4([to_store[o+j] for j in range(4)]) + if self.bufs[buf_index] is not None and hasattr(self.bufs[buf_index]._buf, "IMAGE"): + assert v.typ == Types.FLOAT4, "Image requires upcasting to FLOAT4" + idx, idy = to_image_idx(self.bufs[buf_index]._base_shape, idxy, valid) + self.kernel.append(f"write_imagef({self.buftokens[buf_index].tok}, (int2)({idx.render(render_cl)}, {idy.render(render_cl)}), {v.tok}); /* {self.bufs[buf_index]._base_shape} */\n") + elif v.typ == Types.FLOAT4: + self.kernel.append(f"(({self.lang.buffer_prefix if self.bufs[buf_index] is not None else self.lang.smem_prefix}float4*){self.buftokens[buf_index].tok})[{(idxy//4).render(render_cl)}] = {v.tok};\n") + else: + self.kernel.append(f"{self.buftokens[buf_index].tok}[{(idxy//(4 if v.typ == Types.FLOAT4 else 1)).render(render_cl)}] = {v.tok};\n") + + def load(self, buf_index:int, idx_override:Optional[str]=None) -> List[Token]: + # constant folding + const = None + if self.bufs[buf_index] is not None and self.bufs[buf_index]._base_shape == (1,) and self.bufs[buf_index]._backing is not None: + if buf_index != 0: self.bufs_to_delete.add(buf_index) + val = self.bufs[buf_index]._backing[0] + assert not math.isnan(val) + const = Token(f"({val}f)", Types.FLOAT) + should_upcast = self.lang.float4 and const is None and self.buftokens[buf_index].can_float4() + tokens = [] + test_idy = [] + for o in self.buftokens[buf_index].offsets(): + key = f"val{mnum(buf_index)}_{mnum(o)}" + if (buf_index, o) not in self.loaded_keys: + idxy, valid = self.sts[buf_index].expr_idxs(o) if idx_override is None else self.sts[buf_index].expr_node(idx_override, o) + if should_upcast: + float4_index = Variable("FLOAT4_INDEX", 0, 3) + idxy_test, valid_test = self.sts[buf_index].expr_idxs(float4_index+o) if idx_override is None else self.sts[buf_index].expr_node(idx_override, float4_index+o) + can_merge = idxy_test == float4_index or (isinstance(idxy_test, SumNode) and any(x == float4_index for x in idxy_test.nodes)) # float4_index must be in there without a multiply + can_merge = can_merge and "FLOAT4_INDEX" not in (idxy_test//4).render() and "FLOAT4_INDEX" not in valid_test.render() # float4_index must not be in after divide or in valid (TODO: don't check render) + if const is not None: + ldr = const + elif self.bufs[buf_index] is not None and hasattr(self.bufs[buf_index]._buf, "IMAGE"): + assert should_upcast and can_merge, f"Image requires upcasting to FLOAT4 {self.buftokens[buf_index]}" + idx, idy = to_image_idx(self.bufs[buf_index]._base_shape, idxy, valid, VALIDHACKS) + ldr = Token(f"read_imagef({self.buftokens[buf_index].tok}, smp, (int2)({idx.render(render_cl)}, {idy.render(render_cl)})) /* {self.bufs[buf_index]._base_shape} */", Types.FLOAT4) + test_idy.append(idy.render(render_cl)) + elif should_upcast and can_merge: + ldr = Token(f"(({self.lang.buffer_prefix if self.bufs[buf_index] is not None else self.lang.smem_prefix}float4*){self.buftokens[buf_index].tok})[{(idxy//4).render(render_cl)}]", Types.FLOAT4) + else: + ldr = Token(f"{self.buftokens[buf_index].tok}[{idxy.render(render_cl)}]", Types.FLOAT) + invalid = self.group_float4([Token("0.0f", Types.FLOAT)]*4) if ldr.typ == Types.FLOAT4 else Token("0.0f", Types.FLOAT) + ldr = ldr if valid.min == 1 or (VALIDHACKS and hasattr(self.bufs[buf_index]._buf, "IMAGE")) else (Token(f"({valid.render(render_cl)} ? {ldr.tok} : {invalid.tok})", ldr.typ) if valid.max == 1 else invalid) + if const is not None: + self.loaded_keys[(buf_index,o)] = ldr + else: + self.kernel.append(f"{ldr.decltype()} {key} = {ldr.tok};\n") + if should_upcast and can_merge: + for j in range(4): + self.loaded_keys[(buf_index,o+j)] = Token(key+f'.{"xyzw"[j]}', Types.FLOAT) + else: + self.loaded_keys[(buf_index,o)] = Token(key, Types.FLOAT) + tokens.append(self.loaded_keys[(buf_index,o)]) + assert not VALIDHACKS or all_same(test_idy), f"idy changed! {test_idy}" + return tokens + + def ast_parse(self, x, acc:List[Token], do_reduce=False) -> List[Token]: + if not isinstance(x, LazyOp): return self.load(self.bufs.index(x), "mid" if x is None else None) # hack for local + if isinstance(x.op, ReduceOps) and not do_reduce: return acc + values : List[List[Token]] = ([acc] if isinstance(x.op, ReduceOps) else []) + [self.ast_parse(v, acc, do_reduce) for v in x.src] + code = GPUCodegen.code_for_op[x.op] # TODO: replace this with a function + if len(values) == 2: + assert len(values[0]) == len(values[1]) and values[0][0].typ == values[1][0].typ, f"values mismatch {values}" + return [Token(code.replace("A", a.tok).replace("B", b.tok), a.typ) for a,b in zip(values[0], values[1])] + else: + return [Token(code.replace("A", a.tok), a.typ) for a in values[0]] + + def required_optimizations(self, early_only=False): + for buf_index,buf in enumerate(self.bufs): + upcast_strides = [self.sts[buf_index].strides[i] for i in self.upcast_in_mid_reduce_axes] + if (not early_only or buf in self.earlybufs) and hasattr(buf._buf, "IMAGE") and not (self.buftokens[buf_index].can_float4() or (buf not in self.earlybufs and (1 in upcast_strides))): + axes = [i for i,x in enumerate(self.sts[buf_index].strides) if x == 1] + assert len(axes) == 1, f"wrong number of stride 1 axis : {axes}" + self.shift_to(axes[0], 4) + self.upcast() + assert self.buftokens[buf_index].can_float4() + + def hand_coded_optimizations(self): + # if there's images in the earlybufs, we have to make an axis the 4 loading one + self.required_optimizations(early_only=True) + + # simplify (sets first_reduce) + self.simplify_ones() + + # are we grouping? (requires local shape support) + if len(self.lang.lid) and not self.buftokens[0].can_float4() and self.first_reduce <= 2 and self.first_reduce + 1 <= self.shape_len and prod(self.sts[0].shape[:self.first_reduce]) <= 2048: + # TODO: use 1024 if it's allowed in a smarter way + for sz in (([256, 16]) if prod(self.sts[0].shape[:self.first_reduce]) <= 32 else [16]): + if all([st.shape[self.first_reduce] % sz == 0 or st.shape[self.first_reduce] == 1 for st in self.sts]): + self.shift_to(self.first_reduce, sz, top=True, insert_before=self.first_reduce) + self.group_for_reduce.append(sz) + break + + # are we upcasting in mid reduce? + if hasattr(self.bufs[0]._buf, "IMAGE") and not self.buftokens[0].can_float4() and self.group_for_reduce and self.first_reduce <= 2: + axes = [i for i,x in enumerate(self.sts[0].strides) if x == 1] + assert len(axes) == 1, f"wrong number of stride 1 axis : {axes}" + self.shift_to(axes[0], 4, insert_before=self.first_reduce + len(self.group_for_reduce)) # insert at the end of the grouped axis + self.group_for_reduce.append(4) + + # now do everything required + self.required_optimizations() + + # simplify (sets first_reduce) + self.simplify_ones() + + # use more opencl indexing if the output buffer is an image and we have room + if hasattr(self.bufs[0]._buf, "IMAGE") and self.first_reduce+len(self.group_for_reduce) < 3: + base_shape = self.bufs[0]._base_shape + if (base_shape[0]*base_shape[1]) % self.sts[0].shape[0] == 0 and self.sts[0].shape[0]//base_shape[0] != 0: + if DEBUG >= 4: print("split opencl", base_shape, self.sts[0].shape) + self.reshape_and_permute(lambda x: [base_shape[0], x[0]//base_shape[0]]+list(x[1:]), None) + self.simplify_ones() + + # no more opt if we are grouping + if self.group_for_reduce: return + + # **** below this line need to be optional and benchmarked **** + + # potentially do more upcasts of non reduce axes based on a heuristic + while prod(self.sts[0].shape[:self.first_reduce]) >= 1024: + xb_choices = [] + for axis, upcast_amount in itertools.product(range(self.first_reduce), [3,4]): # consider all the non reduce axes, and a 3 or 4 reduce + # if it mods, and some buffer has stride 0 on axis while having no stride 0 in the buftoken + if self.full_shape[axis]%upcast_amount == 0 and any(self.sts[buf_index].strides[axis] == 0 and not any(x[1] == 0 for x in self.buftokens[buf_index].axis) for buf_index in range(len(self.sts))): + xb_choices.append((sum(st.strides[axis]>0 for st in self.sts), sum(st.strides[axis] for st in self.sts), axis, upcast_amount)) + if len(xb_choices): + xb_choices = sorted(xb_choices) + if DEBUG >= 4: print(f"float4 merging axis : {xb_choices}") + self.shift_to(xb_choices[0][2], amount=xb_choices[0][3]) + self.upcast() + self.simplify_ones() + else: + break + + # if last dim <= 5 and it's a reduce dim, upcast the reduce (loop unrolling). no simplify needed since it's just an upcast. NOTE: careful, this has broken VALIDHACKS + if self.first_reduce < self.shape_len and self.full_shape[-1] <= 5 and (max([x.size() for i,x in enumerate(self.buftokens) if self.bufs[i] in self.earlybufs]) <= 4 or not any(r for _,_,r in self.buftokens[self.full_buf_index].axis)): + self.upcast() + + def get_accumulators(self, name="acc") -> List[Token]: + assert self.reduceop is not None, "no accumulators if you aren't reducing" + should_upcast = self.lang.float4 and self.buftokens[0].can_float4() + accumulators = [Token(f"{name}{i//4}.{'xyzw'[i%4]}" if should_upcast else f"{name}{i}", self.buftokens[0].typ) for i in self.buftokens[0].offsets()] + if should_upcast: + self.kernel += [f"float4 {tok} = {self.group_float4([Token(GPUCodegen.start_for_op[self.reduceop.op], Types.FLOAT)]*4).tok};\n" for tok in dedup([x.tok.split('.')[0] for x in accumulators])] + else: + self.kernel += [f"float {x.tok} = {GPUCodegen.start_for_op[self.reduceop.op]};\n" for x in accumulators] + return accumulators + + # STOP WASTING TIME WITH DOING THE RESHAPES AND PERMUTES BY HAND. KERNEL SEARCH IS THE ONLY WAY IT WILL EVER BE GOOD + # group_for_reduce will have to be better first + def codegen(self) -> ASTRunner: + self.process() + if DEBUG >= 4: self.printbufs("old:", DEBUG>=5) + + self.hand_coded_optimizations() + + # fancy colored shape printer + if DEBUG >= 3: print(self.colorshape(), end="") + + # add a local buffer for multistage reduce + if len(self.group_for_reduce): + self.bufs.append(None) + # TODO: the strides of this can be controlled + st = ShapeTracker(tuple([1] * self.first_reduce + self.group_for_reduce + [1] * (self.shape_len - len(self.group_for_reduce) - self.first_reduce) + [x[0] for x in self.buftokens[0].axis])) + buftoken = Token("temp", Types.FLOAT, ptr=True) + # manual upcast of the local + for _,_,r in self.buftokens[0].axis[::-1]: + buftoken.array(st.shape[-1], st.views[-1].strides[-1], r) + st.views[-1] = View(st.shape[0:-1], st.views[-1].strides[0:-1], st.views[-1].offset) + self.sts.append(st) + self.buftokens.append(buftoken) + + self.output_shape : Tuple[int, ...] = self.sts[0].shape[:self.first_reduce] + tuple(self.group_for_reduce) + assert self.full_shape[:len(self.output_shape)] == self.output_shape, f"output shape mismatch : {self.full_shape[:len(self.output_shape)]} != {self.output_shape}" + if DEBUG >= 4: + print("output shape", self.output_shape) + self.printbufs("new:", DEBUG>=5) + + self.bufs_to_delete : Set[int] = set() + self.loaded_keys : Dict[Tuple[int,int], Token] = {} + self.prekernel : Set[str] = set() + self.kernel : List[str] = ["const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;\n"] if any(hasattr(buf._buf, "IMAGE") for buf in self.bufs if buf is not None) else [] + + if len(self.lang.gid) == 0: + self.kernel += [f"for (int idx{i} = 0; idx{i} < {self.output_shape[i]}; idx{i}++) {{\n" for i in range(0, len(self.output_shape))] + else: + # output_shape[-1] is get_global_id(0) + self.kernel += [f"int idx{len(self.output_shape)-1-i} = {self.lang.gid[i]}; /* {self.output_shape[-1-i]} */\n" for i in range(min(len(self.lang.gid), len(self.output_shape))) if self.output_shape[-1-i] != 1] + if len(self.output_shape) > len(self.lang.gid): + # sometimes, there's more dimensions. compact all the dimensions into the first one + # TODO: these compactions should be searchable (they sort of are with reshapes and permutes) + final_dimension = len(self.output_shape)-len(self.lang.gid) + for i in range(final_dimension-1, -1, -1): + self.kernel += [f"int idx{i} = idx{final_dimension} % {self.output_shape[i]};", f"idx{final_dimension} = idx{final_dimension} / {self.output_shape[i]};\n"] + self.output_shape = (prod(self.output_shape[0:final_dimension+1]), ) + self.output_shape[final_dimension+1:] + if DEBUG >= 4: print(f"replaced output shape with {self.output_shape}") + + # early ast + accumulators : List[Token] = [] + if self.reduceop is not None: + accumulators = self.get_accumulators() + self.kernel += [f"for (int idx{i} = 0; idx{i} < {self.full_shape[i]}; idx{i}++) {{\n" for i in range(self.first_reduce+len(self.group_for_reduce), self.shape_len)] + self.kernel += [f"{x.tok};\n" for x in self.ast_parse(self.reduceop, [accumulators[off] for off in self.buftokens[self.full_buf_index].acc_offsets()], do_reduce=True)] + self.kernel += ["}\n"] * (self.shape_len - (self.first_reduce + len(self.group_for_reduce))) + + # second stage reduce + if self.group_for_reduce: + self.kernel.append(self.lang.smem_prefix + f"float {self.buftokens[-1].tok}[{self.sts[-1].size()*self.buftokens[-1].size()}];\n") + self.store(-1, accumulators) # TODO: this is assuming the local size = global size. should use lidxs + self.kernel.append(self.lang.barrier+"\n") + + # this is used to identify the thread doing the reducing (lidx == 0) and is repeated from store + # must happen before the upcast + lidx, lvalid = self.sts[-1].expr_idxs() + assert lvalid.min == 1, "local buffer must always be valid" + + # if any group_for_reduce items aren't reduces, upcast them here + for j in self.upcast_in_mid_reduce_axes: + self.reshape_and_permute(None, [i for i in range(self.shape_len) if i != j] + [j]) + self.upcast() + if DEBUG >= 4: print("upcast", self.colorshape()) # NOTE: colorshape is wrong here + + self.kernel.append(f"if ({lidx.render(render_cl)} == 0) {{\n") # lidx.max works here too + + # second stage reduce with a new set of accumulators. TODO: do we need acc_offsets here? + accumulators = self.get_accumulators("output") + self.kernel.append(f"for (int mid = 0; mid < {self.sts[-1].size()}; mid++) {{\n") + self.kernel += [f"{x.tok};\n" for x in self.ast_parse(LazyOp(self.reduceop.op, (None,), self.sts[0].shape), accumulators, do_reduce=True)] + self.kernel.append("}\n") + + # late ast + self.store(0, self.ast_parse(self.ast, accumulators)) + if self.group_for_reduce: self.kernel.append("}") + if len(self.lang.gid) == 0: self.kernel += ["}"] * len(self.output_shape) + self.kernel.append("\n}") + + # concat kernel into prg + buftypes = [f"{'read_only' if i > 0 else 'write_only'} image2d_t" if hasattr(x._buf, "IMAGE") else self.lang.buffer_prefix+self.buftokens[i].decltype()+self.lang.buffer_suffix for i,x in enumerate(self.bufs) if x is not None] + prg = ' '.join(list(self.prekernel) + [f"{self.lang.kernel_prefix} void KERNEL_NAME_PLACEHOLDER(",] + + [', '.join([f'{t} data{i}' for i,t in enumerate(buftypes) if i not in self.bufs_to_delete] + self.lang.extra_args)] + + [") {\n"] + self.kernel) + + # kernel function definition + function_name = ("re_S" if self.reduceop else "ew_S") + '_'.join([str(x) for x in self.full_shape]) + + # painfully name the function + if prg in GPUCodegen.kernel_name_cache: function_name = GPUCodegen.kernel_name_cache[prg] + else: + GPUCodegen.kernel_cnt[function_name] += 1 + if GPUCodegen.kernel_cnt[function_name]: function_name = f"{function_name}{'_N'+str(GPUCodegen.kernel_cnt[function_name])}" + GPUCodegen.kernel_name_cache[prg] = function_name + + return ASTRunner(function_name, prg.replace("KERNEL_NAME_PLACEHOLDER", function_name), self.bufs_to_delete, + list(self.output_shape[::-1]) if len(self.output_shape) > 0 else [1], + (self.group_for_reduce[::-1] + [1]*(len(self.output_shape)-len(self.group_for_reduce))) if self.group_for_reduce else None, + op_estimate=self.info.flops, mem_estimate=sum(prod(x._base_shape) for x in self.bufs if x is not None)) diff --git a/tinygrad_repo/tinygrad/graph.py b/tinygrad_repo/tinygrad/graph.py index 07762e874..d86bc7228 100644 --- a/tinygrad_repo/tinygrad/graph.py +++ b/tinygrad_repo/tinygrad/graph.py @@ -1,69 +1,74 @@ -import os -import atexit -import itertools +import os, atexit, itertools +try: + import networkx as nx # type: ignore +except ImportError: + nx = None # graph won't work from collections import defaultdict -from typing import Dict, List -from tinygrad.ops import DeviceBuffer, DEBUG, UnaryOps, BinaryOps, ReduceOps, MovementOps, ProcessingOps, LoadOps, Op, OpType +from typing import Dict, List, Optional +from tinygrad.ops import DeviceBuffer, UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, FusedOps, Op, OpType, LazyOp, get_buffers, get_lazyops +from tinygrad.helpers import getenv, DEBUG -GRAPH = int(os.getenv("GRAPH", "0")) +GRAPH, PRUNEGRAPH, GRAPHPATH = getenv("GRAPH", 0), getenv("PRUNEGRAPH", 0), getenv("GRAPHPATH", "/tmp/net") # **** debugging and graphing **** +G = nx.DiGraph() if nx is not None else None cnts : Dict[OpType, int] = defaultdict(int) if GRAPH: - import networkx as nx # type: ignore - G = nx.DiGraph() def save_graph_exit(): - for k,v in cnts.items(): - print(k, v) - if int(os.getenv("PRUNEGRAPH", "0")): - dead_nodes = [] - for n in G.nodes: - # prune movementops and loadops - if 'fillcolor' in G.nodes[n] and G.nodes[n]['fillcolor'] in ["#80ff8080", "#80ff80", "#FFFF8080", "#FFFF80"]: - for (x,_),(_,y) in itertools.product(G.in_edges(n), G.out_edges(n)): - G.add_edge(x, y) - dead_nodes.append(n) - for n in dead_nodes: - G.remove_node(n) + for k,v in cnts.items(): print(k, v) + if PRUNEGRAPH: prune_graph() print("saving", G) - nx.drawing.nx_pydot.write_dot(G, '/tmp/net.dot') + nx.drawing.nx_pydot.write_dot(G, f'{GRAPHPATH}.dot') # -Gnslimit=100 can make it finish, but you won't like results - os.system('dot -Tsvg /tmp/net.dot -o /tmp/net.svg') + os.system(f'dot -Tsvg {GRAPHPATH}.dot -o {GRAPHPATH}.svg') atexit.register(save_graph_exit) -global_num_max = 0 -def log_op(optype : OpType, op : List[Op], ret : DeviceBuffer, inp : List[DeviceBuffer]): - cnts[optype] += 1 - if DEBUG >= 3: - print(f"{op} : {', '.join([str(x.shape) for x in inp])} -> {ret.shape}") - if GRAPH: - def nm(x): - global global_num_max - if not hasattr(x, 'global_num'): - setattr(x, 'global_num', global_num_max) - global_num_max += 1 - return f"<<< {x.global_num} >>>" +node_count = 0 +def nm(x): + global node_count + if not hasattr(x, 'node_id'): + setattr(x, 'node_id', node_count) + node_count += 1 + return x.node_id - top_colors = {LoadOps: '#FFFF80', UnaryOps: "#c0c0c0", ReduceOps: "#8080ff", BinaryOps: "#c0c0c0", MovementOps: "#80ff80", ProcessingOps: "#ff8080"} +def get_sop(op : List[Op]): + if len(op) <= 2: return '.'.join([str(y).split(".")[1] for y in op][::-1]) + if len(op) <= 4: return '.'.join([str(y).split(".")[1][0:2] for y in op][::-1]) + return str(len(op)) + +def log_op(ret : DeviceBuffer, ast : LazyOp, show_graph : Optional[bool] = None): + if show_graph is None: show_graph = GRAPH + if not DEBUG and not show_graph: return + op : List[Op] = [x.op for x in get_lazyops(ast)] + inp : List[DeviceBuffer] = get_buffers(ast) + if len(inp) == 1 and inp[0] == ret: + if show_graph and nm(ret) in G.nodes: G.nodes[nm(ret)]['style'] += ', bold' + return # don't log self loops + oporder = [LoadOps, FusedOps, ReduceOps, BinaryOps, UnaryOps, MovementOps] + optype = type(sorted(op, key=lambda x: oporder.index(type(x)))[0]) + cnts[optype] += 1 + if DEBUG >= 4: print(f"{op} : {', '.join([f'{x.shape}-<{nm(x)}>' for x in inp])} -> {ret.shape}-<{nm(ret)}>") + if show_graph: + top_colors = {LoadOps: '#FFFF80', UnaryOps: "#c0c0c0", ReduceOps: "#8080ff", BinaryOps: "#c0c0c0", MovementOps: "#80ff80", FusedOps: "#ff8080"} dashed = (optype == LoadOps and hasattr(ret, "_backing")) or (hasattr(ret, "st") and not ret.st.contiguous) # type: ignore for x in inp: - if len(op) <= 2: - sop = '.'.join([str(y).split(".")[1] for y in op][::-1]) - elif len(op) <= 4: - sop = '.'.join([str(y).split(".")[1][0:2] for y in op][::-1]) - else: - sop = str(len(op)) - G.add_edge(nm(x), nm(ret), label=sop) + G.add_edge(nm(x), nm(ret), label=get_sop(op)) if 'label' not in G.nodes[nm(x)]: G.nodes[nm(x)]['label'] = str(x.shape) - if nm(ret) not in G.nodes: - G.add_node(nm(ret)) + if nm(ret) not in G.nodes: G.add_node(nm(ret)) - if optype == ReduceOps: - G.nodes[nm(ret)]['label'] = str(set(x.shape for x in inp))+"\n"+str(ret.shape) - else: - G.nodes[nm(ret)]['label'] = str(ret.shape) - G.nodes[nm(ret)]['fillcolor'] = (top_colors[optype] + ('80' if dashed else '')) if optype in top_colors else "#ffffff" + G.nodes[nm(ret)]['label'] = str(set(x.shape for x in inp))+"\n"+str(ret.shape) if optype == ReduceOps else str(ret.shape) + G.nodes[nm(ret)]['fillcolor'] = (top_colors[optype] + ('80' if dashed else str())) if optype in top_colors else "#ffffff" G.nodes[nm(ret)]['style'] = 'filled, dashed' if dashed else 'filled' + G.nodes[nm(ret)]['prunable'] = optype in [LoadOps, MovementOps] + +# prune movementops and loadops +def prune_graph(): + dead_nodes = [] + for n in G.nodes: + if 'prunable' in G.nodes[n] and G.nodes[n]['prunable']: + G.add_edges_from([(x, y) for (x,_),(_,y) in itertools.product(G.in_edges(n), G.out_edges(n))]) + dead_nodes.append(n) + G.remove_nodes_from(dead_nodes) diff --git a/tinygrad_repo/tinygrad/helpers.py b/tinygrad_repo/tinygrad/helpers.py index 889004cba..006b376ff 100644 --- a/tinygrad_repo/tinygrad/helpers.py +++ b/tinygrad_repo/tinygrad/helpers.py @@ -1,56 +1,18 @@ -from collections import namedtuple -import os, math +import os, math, functools +from typing import Tuple, Union, List def dedup(x): return list(dict.fromkeys(x)) # retains list order -def prod(x): return math.prod(x) -def argfix(*x): return tuple() if len(x) == 0 else tuple(x[0]) if isinstance(x[0], tuple) or isinstance(x[0], list) else tuple(x) +def prod(x:Union[List[int], Tuple[int, ...]]) -> int: return math.prod(x) +def argfix(*x): return tuple() if len(x) == 0 else tuple(x[0]) if isinstance(x[0], (tuple, list)) else tuple(x) def argsort(x): return sorted(range(len(x)), key=x.__getitem__) # https://stackoverflow.com/questions/3382352/equivalent-of-numpy-argsort-in-basic-python def all_same(items): return all(x == items[0] for x in items) if len(items) > 0 else True -def colored(st, color): return f"\u001b[{30+['black', 'red', 'green', 'yellow', 'blue', 'magenta', 'cyan', 'white'].index(color)}m{st}\u001b[0m" # replace the termcolor library with one line +def colored(st, color, background=False, bright=False): return f"\u001b[{10*background+60*bright+30+['black', 'red', 'green', 'yellow', 'blue', 'magenta', 'cyan', 'white'].index(color)}m{st}\u001b[0m" # replace the termcolor library with one line def partition(lst, fxn): return [x for x in lst if fxn(x)], [x for x in lst if not fxn(x)] -def modn(x, a): return -((-x)%a) if x < 0 else x%a +def make_pair(x:Union[int, Tuple[int, ...]], cnt=2) -> Tuple[int, ...]: return (x,)*cnt if isinstance(x, int) else x +def flatten(l): return [item for sublist in l for item in sublist] +def mnum(i) -> str: return str(i) if i >= 0 else f"m{-i}" -def reduce_shape(shape, axis): return tuple(1 if i in axis else shape[i] for i in range(len(shape))) -def shape_to_axis(old_shape, new_shape): - assert len(old_shape) == len(new_shape), "reduce shapes must have same dimensions" - return tuple([i for i,(a,b) in enumerate(zip(old_shape, new_shape)) if a != b]) +@functools.lru_cache(maxsize=None) +def getenv(key, default=0): return type(default)(os.getenv(key, default)) -ConvArgs = namedtuple('ConvArgs', ['H', 'W', 'groups', 'rcout', 'cin', 'oy', 'ox', 'iy', 'ix', 'sy', 'sx', 'bs', 'cout', 'py', 'py_', 'px', 'px_', 'dy', 'dx', 'out_shape']) -def get_conv_args(x_shape, w_shape, stride=1, groups=1, padding=0, dilation=1, out_shape=None): - # TODO: https://docs.nvidia.com/deeplearning/performance/dl-performance-convolutional/index.html#tensor-layout - cout,cin,H,W = w_shape - sy,sx = (stride, stride) if isinstance(stride, int) else stride - if not isinstance(padding, int) and len(padding) == 4: - px,px_,py,py_ = padding - else: - py,px = (padding, padding) if isinstance(padding, int) else padding - py_, px_ = py, px - dy,dx = (dilation, dilation) if isinstance(dilation, int) else dilation - bs,cin_,iy,ix = x_shape - - # this can change px_ and py_ to make the out_shape right - # TODO: copy padding names from http://nvdla.org/hw/v1/ias/unit_description.html - if out_shape is not None: - py_ = (out_shape[2] - 1) * sy + 1 + dy * (H-1) - iy - py - px_ = (out_shape[3] - 1) * sx + 1 + dx * (W-1) - ix - px - - # TODO: should be easy to support asymmetric padding by changing output size - # https://pytorch.org/docs/stable/generated/torch.nn.Conv2d.html describes these sizes well - oy = (iy + py + py_ - dy * (H-1) - 1)//sy + 1 - ox = (ix + px + px_ - dx * (W-1) - 1)//sx + 1 - if cin*groups != cin_: - raise Exception(f"Input Tensor shape {x_shape} does not match the shape of the weights {w_shape}. ({cin*groups} vs. {cin_})") - assert cout % groups == 0 and (out_shape is None or out_shape == (bs, cout, oy, ox)) - return ConvArgs(H, W, groups, cout//groups, cin, oy, ox, iy, ix, sy, sx, bs, cout, py, py_, px, px_, dy, dx, (bs, cout, oy, ox)) - -def get_available_llops(): - import importlib, inspect - _buffers, DEFAULT = {}, "CPU" - for op in [os.path.splitext(x)[0] for x in sorted(os.listdir(os.path.join(os.path.dirname(os.path.realpath(__file__)), "llops"))) if x.startswith("ops_")]: - name = op[len("ops_"):].upper() - DEFAULT = name if os.environ.get(name, 0) == "1" else DEFAULT - try: - _buffers[name] = [cls for cname, cls in inspect.getmembers(importlib.import_module('tinygrad.llops.'+op), inspect.isclass) if (cname.upper() == name + "BUFFER")][0] - except ImportError as e: # NOTE: this can't be put on one line due to mypy issue - print(op, "not available", e) - return _buffers, DEFAULT \ No newline at end of file +DEBUG, IMAGE = getenv("DEBUG", 0), getenv("IMAGE", 0) diff --git a/tinygrad_repo/tinygrad/image.py b/tinygrad_repo/tinygrad/image.py new file mode 100644 index 000000000..068566a39 --- /dev/null +++ b/tinygrad_repo/tinygrad/image.py @@ -0,0 +1,74 @@ +from tinygrad.helpers import IMAGE +from tinygrad.lazy import get_single_root + +def image_conv2d_decorator(normal_conv): + if IMAGE == 0: return normal_conv + + def image_conv2d(self, weight, bias=None, groups=1, stride=1, dilation=1, padding=0): + (bs,_,iy,ix), (cout,cin,H,W) = self.shape, weight.shape + rcout = cout//groups + x, w = self, weight.reshape(groups, rcout, cin, H, W) + + # hack for non multiples of 4 on cin + if cin % 4 != 0 and not (cin == 1 and groups%4 == 0): + x = x.reshape(bs, groups, cin, iy, ix) # do this always? + added_input_channels = 4 - (cin % 4) + w = w.pad(tuple((0, added_input_channels) if i == 2 else (0, 0) for i in range(len(w.shape)))) + x = x.pad(tuple((0, added_input_channels) if i == 2 else (0, 0) for i in range(len(x.shape)))) + cin = cin + added_input_channels + x = x.reshape(bs, groups*cin, iy, ix) + + # hack for non multiples of 4 on rcout + added_output_channels = 0 + if rcout % 4 != 0 and not (rcout == 1 and groups%4 == 0): + added_output_channels = 4 - (rcout % 4) + rcout += added_output_channels + cout = groups * rcout + w = w.slice(tuple((0, rcout) if i == 1 else (0, w.shape[i]) for i in range(len(w.shape)))) + + # packed (note: flipping bs and iy would make the auto-padding work) + x = x.permute(0,2,3,1).reshape(bs * iy, ix * groups * cin//4, 4) + cin_last = iy == 1 and ix == 1 + if cin == 1: w = w.reshape(cout//4,4,H*W).permute(0,2,1) + elif cin_last: w = w.reshape(cout//4,4,cin//4,4,H,W).permute(0,4,2,5,1,3).reshape(cout//4, H*cin//4*W*4, 4) + else: w = w.reshape(cout//4,4,cin//4,4,H,W).permute(0,4,2,5,3,1).reshape(cout//4, H*cin//4*W*4, 4) + + # contiguous creates the image, and early realize static weights (TODO: test for the static weight) + x, w = x.contiguous(), w.contiguous() + if get_single_root(w.lazydata).realized: w.realize() + + # expand out + rcin_hi, rcin_lo = cin//4 if cin >= 4 else 1, 4 if cin >= 4 else 1 + cout_expand = [groups//4 if cin == 1 else groups, 4 if cin == 1 else 1, rcout//4 if rcout >= 4 else 1, 4 if rcout >= 4 else 1] + x = x.reshape(bs, iy, ix, groups, rcin_hi, rcin_lo) + if cin_last: w = w.reshape(cout//4, H, rcin_hi, W, 4, rcin_lo) + else: w = w.reshape(cout//4, H, rcin_hi, W, rcin_lo, 4).permute(0,1,2,3,5,4) + + # padding + padding_ = [padding]*4 if isinstance(padding, int) else (padding if len(padding) == 4 else [padding[1], padding[1], padding[0], padding[0]]) + x = x.slice((None, (-padding_[2], x.shape[1]+padding_[3]), (-padding_[0], x.shape[2]+padding_[1]), None, None, None)) + + # prepare input + x = x.permute(0,3,4,5,1,2)._pool((H, W), stride, dilation) # -> (bs, groups, rcin_hi, rcin_lo, oy, ox, H, W) + oy, ox = x.shape[4:6] + x = x.permute(0,4,5,1,2,3,6,7).reshape(bs, oy, ox, *cout_expand[0:2], 1, 1, rcin_hi, rcin_lo, H, W) + x = x.expand(bs, oy, ox, *cout_expand, rcin_hi, rcin_lo, H, W) + + # prepare weights + w = w.permute(0,4,2,5,1,3) + w = w.reshape((1, 1, 1, *cout_expand, rcin_hi, rcin_lo, H, W)) + + # the conv! + ret = (x*w).sum((-4, -3, -2, -1)).reshape(bs*oy, ox*cout//4, 4) + if IMAGE >= 3: ret = ret.contiguous() + + # undo hack for non multiples of 4 on C.rcout + if added_output_channels != 0: + ret = ret.reshape(bs, oy, ox, groups, rcout)[:, :, :, :, :-added_output_channels] + rcout -= added_output_channels + cout = groups * rcout + + # NCHW output + ret = ret.reshape(bs, oy, ox, cout).permute(0,3,1,2) + return ret if bias is None else ret.add(bias.reshape(1, -1, 1, 1)) + return image_conv2d diff --git a/tinygrad_repo/tinygrad/jit.py b/tinygrad_repo/tinygrad/jit.py new file mode 100644 index 000000000..3cdb757c4 --- /dev/null +++ b/tinygrad_repo/tinygrad/jit.py @@ -0,0 +1,46 @@ +from typing import Callable, List, Tuple, Any, Dict, cast, Union +import itertools +from tinygrad.helpers import DEBUG, colored + +from tinygrad.lazy import Device +from tinygrad.tensor import Tensor +from tinygrad.ops import GlobalCounters, CompiledBuffer, RawBuffer + +class TinyJit: + def __init__(self, fxn:Callable): + self.fxn : Callable = fxn + self.cnt : int = 0 + self.jit_cache : List[Tuple[Callable, Any]] = [] # TODO: Any should be List[RawBuffer], but this fails + self.ret : Any = None + self.input_replace : Dict[Tuple[int, int], Union[int, str]]= {} + + def __call__(self, *args, **kwargs) -> Any: + if Device.DEFAULT not in ["GPU", "CLANG", "METAL", "CUDA"]: return self.fxn(*args, **kwargs) # only jit on the GPU codegen + # NOTE: this cast is needed since although we know realize will create a ".realized" DeviceBuffer, the type checker doesn't + input_rawbuffers : Dict[Union[int, str], RawBuffer] = {cast(Union[int, str], k):cast(CompiledBuffer, v.realize().lazydata.realized).raw() for k,v in itertools.chain(enumerate(args), kwargs.items()) if isinstance(v, Tensor)} + assert len(input_rawbuffers) != 0, "no inputs to JIT" + if self.cnt >= 2: + for (j,i),idx in self.input_replace.items(): self.jit_cache[j][1][i] = input_rawbuffers[idx] + if DEBUG >= 2: print(colored("TOK", 'magenta').split("TOK")[0], end="") + for prg, args in self.jit_cache: prg(args) + if DEBUG >= 2: print(colored("TOK", 'magenta').split("TOK")[1], end="") + for (j,i),idx in self.input_replace.items(): self.jit_cache[j][1][i] = None + elif self.cnt == 1: + GlobalCounters.cache = [] + self.ret = self.fxn(*args, **kwargs) + self.jit_cache = GlobalCounters.cache + GlobalCounters.cache = None + assert len(self.jit_cache) != 0, "didn't JIT anything!" + if DEBUG >= 1: print(f"JIT captured {len(self.jit_cache)} kernels with {len(input_rawbuffers)} inputs") + + # get the inputs for replacement + for j,(prg,args) in enumerate(self.jit_cache): # pylint: disable=E1133 + for i,a in enumerate(args): + if a in input_rawbuffers.values(): + self.input_replace[(j,i)] = [k for k,v in input_rawbuffers.items() if v == a][0] + assert set(self.input_replace.values()) == set(input_rawbuffers.keys()), "some input tensors not found" + for (j,i),idx in self.input_replace.items(): self.jit_cache[j][1][i] = None + elif self.cnt == 0: + self.ret = self.fxn(*args, **kwargs) + self.cnt += 1 + return self.ret diff --git a/tinygrad_repo/tinygrad/lazy.py b/tinygrad_repo/tinygrad/lazy.py index 9ace5422e..7b397aa1c 100644 --- a/tinygrad_repo/tinygrad/lazy.py +++ b/tinygrad_repo/tinygrad/lazy.py @@ -1,108 +1,59 @@ from __future__ import annotations -from typing import Optional, Tuple, Union, List, Dict -from copy import copy -import os, sys, weakref -from tinygrad.helpers import ConvArgs, get_available_llops, prod -from tinygrad.shape import ShapeTracker -from tinygrad.ops import DeviceBuffer, UnaryOps, BinaryOps, ReduceOps, MovementOps, ProcessingOps, LoadOps, OpType, LazyOp, get_buffers, get_lazyops, DEBUG +from typing import Optional, Tuple, Union, List, Dict, Any, ClassVar, Type +import os, sys, weakref, importlib, inspect, functools +from weakref import WeakValueDictionary +from tinygrad.helpers import prod, getenv +from tinygrad.shape import ShapeTracker, get_contraction +from tinygrad.ops import DeviceBuffer, UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, OpType, LazyOp, get_buffers, get_lazyops, map_buffers from tinygrad.graph import log_op # lazy can recurse a lot sys.setrecursionlimit(10000) -OPT = int(os.getenv("OPT", "2")) -NOCONV = int(os.getenv("NOCONV", "0")) -IMAGE = int(os.getenv("IMAGE", "0")) +OPT = getenv("OPT", 2) +LAZY = getenv("LAZY", 1) + +def get_buffer(name, base='tinygrad.runtime'): + try: + return [cls for cname, cls in inspect.getmembers(importlib.import_module(f'{base}.ops_{name}'), inspect.isclass) if (cname.lower() == name + "buffer")][0] + except Exception as e: # NOTE: this can't be put on one line due to mypy issue + print(name, "backend not available", e, file=sys.stderr) + +class _Device: + def __init__(self) -> None: + self._buffers : Dict[str, Type[DeviceBuffer]] = {x.upper():get_buffer(x) for x in + [os.path.splitext(x)[0][len("ops_"):] for x in sorted(os.listdir(os.path.join(os.path.dirname(os.path.realpath(__file__)), "runtime"))) if x.startswith("ops_")] if x is not None} + self.DEFAULT : str = "CPU" + for name in self._buffers: + if getenv(name) == 1: self.DEFAULT = name # note: DEFAULT can be a Device that can't be imported. better than silent use of a different device + if self._buffers[name] is not None: self.__setattr__(name, name) +Device = _Device() # TODO: movement ops that only change shape are really nops. treat them as such REMOVE_MOVEMENT_NOPS, MERGE_UNARY_OPS, MERGE_ELEMENTWISE_INTO_REDUCE, SHUFFLE_MOVEMENT_OPS = OPT>=1, OPT>=1, OPT>=1, OPT>=1 MERGE_ELEMENTWISE_OPS, MERGE_ONE_REDUCE_INTO_ELEMENTWISE = OPT>=2, OPT>=2 -SHUFFLE_PAD_OPS = OPT>=3 # NOTE: 0/0 is NaN if you pad, so this can change the output - -# **** enumerate supported devices **** - -class Device: - _buffers, DEFAULT = get_available_llops() - for name in _buffers.keys(): - vars()[name] = name - -# **** realize helpers **** -def realize_buffers(real_srcs, x:LazyOp) -> LazyOp: - if x in real_srcs: - return realize_buffers(real_srcs, real_srcs[x]) if isinstance(real_srcs[x], LazyOp) else real_srcs[x] - return LazyOp(x.op, tuple(realize_buffers(real_srcs, y) for y in x.src), x.arg) +PUSH_PERMUTES, PUSH_CONTIGUOUS = OPT>=3, OPT>=3 # **** realize functions **** -# TODO: make all _realize functions return an AST, perhaps unrealized - -def _realize_loadops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], Optional[OpType]]: - if self.op.op == LoadOps.FROMCPU: - return Device._buffers[self.device].fromCPU(self.op.arg), [], LoadOps - elif self.op.op == LoadOps.CONTIGUOUS: - real_src = self.op.src[0].realize(self.device) - ret = real_src.contiguous() - return ret, [real_src], LoadOps if id(ret) != id(real_src) else None - else: - raise NotImplementedError(f"unknown LoadOp {self.op.op}") - -def _realize_movementops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: - src = self.op.src[0] - - # fuse RESHAPE and ReduceOps - if src.realized is None and src.optype == ReduceOps and self.op.op == MovementOps.RESHAPE and len(src.children) <= 1: - return _realize_reduceops_w_shape(src, output_shape = self.op.arg) - - real_src = src.realize(self.device) - return real_src.movement_op(self.op.op, self.op.arg), [real_src], MovementOps - -def _realize_processingops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: - real_src_x, real_src_w = [x.realize(self.device) for x in self.op.src] - return real_src_x.processing_op(self.op.op, real_src_w, self.op.arg), [real_src_x, real_src_w], ProcessingOps - -# this supports late merging an upstream Elementwise op -def _realize_reduceops_w_shape(self:LazyBuffer, output_shape=None) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: +def _ast_reduceops(self:LazyBuffer) -> LazyOp: # TODO: this can also corealize a binary op after the reduce, not just before src = self.op.src[0] if MERGE_ELEMENTWISE_INTO_REDUCE and src.realized is None and src.optype == BinaryOps and len(src.children) <= 1: - # this is the new version, deprecate _processing_op - real_srcs : Dict[LazyBuffer, DeviceBuffer] = {x:x.realize(self.device) for x in get_buffers(src.op)} - ast = LazyOp(self.op.op, (realize_buffers(real_srcs, src.op),), self.op.arg) - else: - real_src = src.realize(self.device) - real_srcs = {src:real_src} - ast = LazyOp(self.op.op, (real_src,), self.op.arg) - if output_shape is not None: ast = LazyOp(MovementOps.RESHAPE, (ast, ), output_shape) - return self.dbuffer.exec_ast(ast), list(real_srcs.values()), ReduceOps -def _realize_reduceops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: return _realize_reduceops_w_shape(self) + src = src.op + return LazyOp(self.op.op, (src,), self.op.arg) # this supports late merging an upstream Reduce op and even an Elementwise op above that -def _realize_binaryops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: - real_srcs : Dict[LazyBuffer, Union[None, LazyOp, DeviceBuffer]] = {x:None for x in get_buffers(self.op)} - op_type : OpType = BinaryOps - if DEBUG >= 3: - for k,x in zip(real_srcs.keys(), map(get_movementroot_contiguous, real_srcs.keys())): - if x.optype in [ProcessingOps,ReduceOps] and x.realized is None: - print("\nHIT", k,x) - for tk in k.children: print("k", tk) - for tx in x.children: print("x", tx) +def _ast_binaryops(self:LazyBuffer) -> LazyOp: + real_srcs : Dict[LazyBuffer, Union[None, LazyOp, LazyBuffer]] = {x:None for x in get_buffers(self.op)} # NOTE: contiguous does not always mean the same size with SHRINK. this is still mergeable but requires more thought how - psrcs : List[Tuple[LazyBuffer, LazyBuffer]] = [(k,x) for k,x in zip(real_srcs.keys(), map(get_movementroot_contiguous, real_srcs.keys())) if x.optype in [ProcessingOps,ReduceOps] and x.realized is None and prod(k.shape) == prod(x.shape) and len(x.children) <= 1 and len(k.children) <= 1] - intermediate_shape = self.shape - if len(psrcs) == 1 and MERGE_ONE_REDUCE_INTO_ELEMENTWISE and (self.device != "OPENCL" or self.shape[-1] == 4): - if psrcs[0][1].optype == ProcessingOps: - real_srcs[psrcs[0][0]] = psrcs[0][1].op - for x in psrcs[0][1].op.src: - real_srcs[x] = x.realize(self.device) - op_type = ProcessingOps - elif psrcs[0][1].optype == ReduceOps: - src = psrcs[0][1].op.src[0] - if MERGE_ELEMENTWISE_INTO_REDUCE and src.realized is None and src.optype == BinaryOps and len(src.children) <= 1: - src = src.op - real_srcs[psrcs[0][0]] = LazyOp(psrcs[0][1].op.op, (src,), psrcs[0][1].op.arg) - for x in get_buffers(real_srcs[psrcs[0][0]]): # type: ignore - # these are the early buffers - real_srcs[x] = x.realize(self.device) - op_type = ReduceOps + psrcs : List[Tuple[LazyBuffer, LazyBuffer]] = [(k,x) for k,x in zip(real_srcs.keys(), map(get_movementroot_contiguous, real_srcs.keys())) if x.optype == ReduceOps and x.realized is None and prod(k.shape) == prod(x.shape) and len(x.children) <= 1 and len(k.children) <= 1] + intermediate_shape : Tuple[int, ...] = self.shape + if len(psrcs) == 1 and MERGE_ONE_REDUCE_INTO_ELEMENTWISE: + if psrcs[0][1].optype == ReduceOps: + top = _ast_reduceops(psrcs[0][1]) + real_srcs[psrcs[0][0]] = top + real_srcs.update({x:x for x in get_buffers(top)}) # the reduce op buffers are not modified + # if the ReduceOp is followed by a reshape, we push this reshape before all the ElementwiseOp inputs if psrcs[0][0].shape != psrcs[0][1].shape: intermediate_shape = psrcs[0][1].shape @@ -111,69 +62,95 @@ def _realize_binaryops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer # reshape all the late ops into the output shape # NOTE: these RESHAPEs will return self if they don't change the shape for x in real_srcs.keys(): - if real_srcs[x] is None: - real_srcs[x] = x.movement_op(MovementOps.RESHAPE, intermediate_shape).realize(self.device) - ast = LazyOp(MovementOps.RESHAPE, (realize_buffers(real_srcs, self.op), ), self.shape) - ret = self.dbuffer.exec_ast(ast) - return ret, [x for x in real_srcs.values() if not isinstance(x, LazyOp) and x is not None], op_type - -_realize = {LoadOps:_realize_loadops, ReduceOps:_realize_reduceops, MovementOps:_realize_movementops, BinaryOps:_realize_binaryops, ProcessingOps:_realize_processingops} + if real_srcs[x] is None: real_srcs[x] = x.movement_op(MovementOps.RESHAPE, intermediate_shape) + ast = map_buffers(real_srcs, self.op) + return LazyOp(MovementOps.RESHAPE, (ast, ), self.shape) if intermediate_shape != self.shape else ast # **** lazy operations **** def get_weakop(op:LazyOp) -> LazyOp: return LazyOp(op.op, tuple(get_weakop(x) if isinstance(x, LazyOp) else weakref.ref(x) for x in op.src), op.arg) -def get_movementroot(root:LazyBuffer) -> LazyBuffer: return get_movementroot(root.op.src[0]) if root.realized is None and (root.optype == MovementOps or (root.op.op == LoadOps.CONTIGUOUS and root.op.src[0].st.contiguous)) else root -def get_movementroot_contiguous(x:LazyBuffer) -> LazyBuffer: return get_movementroot(x) if x.optype == MovementOps and x.st.contiguous else x +def get_single_root(root:LazyBuffer) -> LazyBuffer: return get_single_root(root.op.src[0]) if getattr(root, 'op', None) and len(root.op.src) == 1 else root +def get_movementroot(root:LazyBuffer, allow_contiguous=False) -> LazyBuffer: return get_movementroot(root.op.src[0], allow_contiguous) if root.realized is None and (root.optype == MovementOps or (root.op.op == LoadOps.CONTIGUOUS and allow_contiguous and root.op.src[0].st.contiguous)) else root +def get_movementroot_contiguous(x:LazyBuffer) -> LazyBuffer: return get_movementroot_contiguous(x.op.src[0]) if x.realized is None and x.op.op == LoadOps.CONTIGUOUS else (get_movementroot(x, True) if x.optype == MovementOps and x.st.contiguous else x) -LAZY = int(os.getenv("LAZY", "1")) +def replace_with_movement_op(y:Union[LazyOp, LazyBuffer], op:MovementOps, arg:Tuple[Any, ...]) -> LazyBuffer: + if isinstance(y, LazyBuffer): return y.movement_op(op, arg) + assert y.op in BinaryOps or y.op in UnaryOps + return elementwise_op(y.op, *[replace_with_movement_op(z, op, arg) for z in y.src]) # type: ignore +def support_weakref(x): return x +@support_weakref # needed for mypyc, this prevents LazyBuffer from becoming a native class class LazyBuffer: - lazycache : weakref.WeakValueDictionary[LazyOp, LazyBuffer] = weakref.WeakValueDictionary() - def __new__(cls, device, shape, optype, op): + __deletable__ = ('op',) + lazycache : ClassVar[WeakValueDictionary[Tuple[str, OpType, LazyOp], LazyBuffer]] = WeakValueDictionary() + def __new__(cls, device:str, shape:Union[ShapeTracker, Tuple[int, ...]], optype:OpType, op:LazyOp): # fromcpu aren't cached if optype == LoadOps and op.op == LoadOps.FROMCPU: return super().__new__(cls) wop = (device, optype, get_weakop(op)) # NOTE: shape should be deterministic. annoying to cache with the ShapeTracker # NOTE: we need "ret" to prevent the new buffer from being immediately deleted - if wop not in LazyBuffer.lazycache: - LazyBuffer.lazycache[wop] = ret = super().__new__(cls) # noqa: F841, pylint: disable=W0612 - return LazyBuffer.lazycache[wop] + if wop not in LazyBuffer.lazycache: LazyBuffer.lazycache[wop] = ret = super().__new__(cls) + else: ret = LazyBuffer.lazycache[wop] + return ret - def __init__(self, device, shape:Union[ShapeTracker, Tuple[int, ...]], optype:OpType, op:LazyOp): + def __init__(self, device:str, shape:Union[ShapeTracker, Tuple[int, ...]], optype:OpType, op:LazyOp): if hasattr(self, 'device'): return # cache hit, we return and don't reinit self.st = shape if isinstance(shape, ShapeTracker) else ShapeTracker(tuple(shape)) self.shape, self.optype, self.op = self.st.shape, optype, op self.realized : Optional[DeviceBuffer] = None + self.output_buffer : Optional[DeviceBuffer] = None self.device, self.dbuffer = device, Device._buffers[device] + # TODO: does children have to be a ref count instead of a set? can a Buffer be a double child? self.children : weakref.WeakSet[LazyBuffer] = weakref.WeakSet() # NOTE: op should be read only after construction of LazyBuffer - for x in get_buffers(op): - x.children.add(self) - if not LAZY: - self.realize() + for x in get_buffers(op): x.children.add(self) + if not LAZY: self.realize() def __repr__(self): return f"" # this produces a device buffer def realize(self:LazyBuffer, required_device=None) -> DeviceBuffer: - if required_device is not None: - assert required_device == self.device + assert required_device is None or required_device == self.device if self.realized is None: - # we haven't realized the Buffer yet - self.realized, real_srcs, real_type = _realize[self.optype](self) - # in lazy mode, we don't log until we realize - if real_type is not None: - log_op(real_type, [x.op for x in get_lazyops(self.op)], self.realized, real_srcs) + # get real ops first + if self.op.op == LoadOps.FROMCPU: + self.realized = Device._buffers[self.device].fromCPU(self.op.arg) + ast = LazyOp(self.op.op, tuple()) + elif self.op.op == LoadOps.CONTIGUOUS: + real_src = self.op.src[0].realize(self.device) + self.realized = real_src.contiguous() + ast = LazyOp(self.op.op, (real_src, )) + elif self.optype == MovementOps: + src = self.op.src[0] + + # fuse RESHAPE and ReduceOps + if src.realized is None and src.optype == ReduceOps and self.op.op == MovementOps.RESHAPE and len(src.children) <= 1: + # it's okay to add a RESHAPE to the ast here + ast = LazyOp(MovementOps.RESHAPE, (_ast_reduceops(src), ), self.op.arg) + else: + # movement ops aren't an AST, just run them + real_src = src.realize(self.device) + self.realized = real_src.movement_op(self.op.op, self.op.arg) + ast = LazyOp(self.op.op, (real_src, )) + elif self.optype == ReduceOps: ast = _ast_reduceops(self) + elif self.optype == BinaryOps: ast = _ast_binaryops(self) + # no need to keep the op after realization del self.op - assert self.realized.shape == self.shape + # run the ast if we still have to, and log the op + if self.realized is None: + ast = map_buffers({x:x.realize(self.device) for x in get_buffers(ast)}, ast) + self.realized = self.dbuffer.exec_ast(ast, output_buffer=self.output_buffer) + log_op(self.realized, ast) + + assert self.realized.shape == self.shape, f"shape mismatch on realize {self.realized.shape} vs {self.shape}" assert isinstance(self.realized, Device._buffers[self.device]) return self.realized @staticmethod - def fromCPU(x, device): return LazyBuffer(device, x.shape, LoadOps, LazyOp(LoadOps.FROMCPU, tuple(), x.copy())) + def fromCPU(x, device) -> LazyBuffer: return LazyBuffer(device, x.shape, LoadOps, LazyOp(LoadOps.FROMCPU, tuple(), x.copy())) def toCPU(self): return self.realize().toCPU() def unary_op(self:LazyBuffer, op:UnaryOps) -> LazyBuffer: return elementwise_op(op, self) @@ -181,57 +158,61 @@ class LazyBuffer: def contiguous(self:LazyBuffer) -> LazyBuffer: return LazyBuffer(self.device, self.shape, LoadOps, LazyOp(LoadOps.CONTIGUOUS, (self,))) def reduce_op(self:LazyBuffer, op:ReduceOps, new_shape:Tuple[int, ...]) -> LazyBuffer: - if self.shape == tuple(new_shape): - return self + if self.shape == tuple(new_shape): return self reduce = list(enumerate(zip(self.shape, new_shape))) # move the reduce axes to the end - x = self.movement_op(MovementOps.PERMUTE, [i for i,(s,n) in reduce if s == n] + [i for i,(s,n) in reduce if s != n]) + x = self.movement_op(MovementOps.PERMUTE, tuple([i for i,(s,n) in reduce if s == n] + [i for i,(s,n) in reduce if s != n])) new_tmp_shape = tuple([n for _,(s,n) in reduce if s == n] + [n for _,(s,n) in reduce if s != n]) # NOTE: this reshape can only move around 1s return LazyBuffer(x.device, new_tmp_shape, ReduceOps, LazyOp(op, (x,), new_tmp_shape)).movement_op(MovementOps.RESHAPE, new_shape) - # syntactic sugar around PAD and SHRINK - # TODO: turn RESHAPE into EXPAND and CONTRACT (current EXPAND should be REPEAT) - def slice(self:LazyBuffer, arg): - padding = [(max(0, -p[0]), max(0, p[1]-self.shape[i])) for i,p in enumerate(arg)] - return self.movement_op(MovementOps.PAD, padding).movement_op(MovementOps.SHRINK, tuple((p[0] + padding[i][0], p[1] + padding[i][0]) for i,p in enumerate(arg))) + def movement_op(self:LazyBuffer, op:MovementOps, arg:Tuple[Any, ...]) -> LazyBuffer: + # very instant nop + if op == MovementOps.RESHAPE and self.shape == arg: return self - def movement_op(self:LazyBuffer, op:MovementOps, arg) -> LazyBuffer: # TODO: look into why that copy is needed - arg = tuple(copy(arg)) local_st = ShapeTracker(self.shape).movement_op(op, arg) # instant nops - if local_st.contiguous and self.shape == local_st.shape and op != MovementOps.STRIDED: - return self + if local_st.contiguous and self.shape == local_st.shape: return self # two ops in a row is one op. merge them if unresolved if self.realized is None and self.op.op == op: - if op in [MovementOps.RESHAPE, MovementOps.EXPAND, MovementOps.SHRINK]: - return self.op.src[0].movement_op(op, arg) - if op == MovementOps.PERMUTE: - return self.op.src[0].movement_op(op, tuple(self.op.arg[i] for i in arg)) - if op == MovementOps.PAD: - return self.op.src[0].movement_op(op, tuple((b1+b2, e1+e2) for (b1,e1),(b2,e2) in zip(self.op.arg, arg))) - # TODO: MovementOps.FLIP / MovementOps.STRIDED? + # TODO: why is deleting self from children needed? shouldn't GC do it? + self.op.src[0].children.discard(self) + if op in [MovementOps.RESHAPE, MovementOps.EXPAND, MovementOps.SHRINK]: return self.op.src[0].movement_op(op, arg) + if op == MovementOps.PERMUTE: return self.op.src[0].movement_op(op, tuple(self.op.arg[i] for i in arg)) + if op == MovementOps.PAD: return self.op.src[0].movement_op(op, tuple((b1+b2, e1+e2) for (b1,e1),(b2,e2) in zip(self.op.arg, arg))) + if op == MovementOps.FLIP: return self.op.src[0].movement_op(op, tuple(i for i in arg+self.op.arg if not (i in arg and i in self.op.arg))) + + # push permutes before reduce ops + if op == MovementOps.PERMUTE and PUSH_PERMUTES and self.realized is None and self.optype == ReduceOps: + # reduceops have one buffer input, permute it + narg = tuple(self.op.arg[arg[i]] for i in range(len(arg))) + src, rop = self.op.src[0], self.op.op + src.children.discard(self) + del self # TODO: why doesn't this delete remove it from the children + return src.movement_op(op, arg).reduce_op(rop, narg) # some permutes are actually just reshapes - if op == MovementOps.PERMUTE and local_st.contiguous: - return self.movement_op(MovementOps.RESHAPE, tuple(self.shape[i] for i in arg)) + if op == MovementOps.PERMUTE and local_st.contiguous: return self.movement_op(MovementOps.RESHAPE, tuple(self.shape[i] for i in arg)) - # some strideds are actually just reshapes - # NOTE: due to how strided works, we have to check the parent to be contiguous also - if op == MovementOps.STRIDED and local_st.contiguous and self.st.contiguous: - return self.movement_op(MovementOps.RESHAPE, tuple(i for i,_ in arg)) + # move permutes before expands + if op == MovementOps.PERMUTE and PUSH_PERMUTES and self.realized is None and self.op.op == MovementOps.EXPAND: + self.op.src[0].children.discard(self) + return self.op.src[0].movement_op(MovementOps.PERMUTE, arg).movement_op(MovementOps.EXPAND, tuple(self.op.arg[a] for a in arg)) - # if this MovementOp is being applied to a BinaryOp, apply the MovementOp to all the BinaryOp inputs instead - if SHUFFLE_MOVEMENT_OPS and self.optype == BinaryOps and self.realized is None and len(self.children) == 0 and (SHUFFLE_PAD_OPS or op != MovementOps.PAD) and op not in [MovementOps.EXPAND, MovementOps.STRIDED]: - def replace_with_movement_op(y:Union[LazyOp, LazyBuffer]) -> LazyBuffer: - if isinstance(y, LazyBuffer): - return y.movement_op(op, arg) - assert y.op in BinaryOps or y.op in UnaryOps - return elementwise_op(y.op, *[replace_with_movement_op(z) for z in y.src]) # type: ignore - return replace_with_movement_op(self.op) + # move permutes before reshapes if we can + if op == MovementOps.PERMUTE and PUSH_PERMUTES and self.realized is None and self.op.op == MovementOps.RESHAPE and isinstance(self.op.src[0], LazyBuffer): + if shape_idx_groups := get_contraction(self.op.src[0].shape, self.shape): + new_arg : List[int] = functools.reduce(lambda r, x: r + shape_idx_groups[x], arg, []) + self.op.src[0].children.discard(self) # this changes nothing? + return self.op.src[0].movement_op(MovementOps.PERMUTE, tuple(new_arg)) \ + .movement_op(MovementOps.RESHAPE, ShapeTracker(self.st).movement_op(op, arg).shape) + + # if this MovementOp is being applied to a BinaryOp, apply the MovementOp to all the BinaryOp inputs instead. NOTE: UnaryOps is never an OpType + if SHUFFLE_MOVEMENT_OPS and self.optype == BinaryOps and self.realized is None and len(self.children) == 0 and op != MovementOps.EXPAND and (op != MovementOps.PAD or all(x.op != BinaryOps.DIV for x in get_lazyops(self.op))): + return replace_with_movement_op(self.op, op, arg) # create the buffer ret = LazyBuffer(self.device, ShapeTracker(self.st).movement_op(op, arg), MovementOps, LazyOp(op, (self,), arg)) @@ -242,87 +223,24 @@ class LazyBuffer: # MovementOps aren't stacked any more, they each have one parent, find the root root = get_movementroot(self) if root.st.contiguous and root != self and prod(ret.st.shape) == prod(root.shape): - return root.movement_op(MovementOps.RESHAPE, ret.st.shape) if ret.st.shape != root.shape else root + return root.movement_op(MovementOps.RESHAPE, ret.st.shape) return ret - def processing_op(self:LazyBuffer, op:ProcessingOps, w:LazyBuffer, C:ConvArgs) -> LazyBuffer: - x = self - - if IMAGE >= 1: - from accel.opencl.preprocessing import preprocessing_op, postprocessing_op # type: ignore - Cold = C - x,w,C = preprocessing_op(x, w, Cold, False) - - # set up the conv - # (C.bs*C.iy, C.ix*C.groups*C.cin//4, 4) - x = x.movement_op(MovementOps.RESHAPE, (C.bs, C.iy, C.ix, C.groups, C.cin)) - # padding (implicit is fine in image) - x = x.slice(((0, x.shape[0]), (-C.py, x.shape[1]+C.py_), (-C.px, x.shape[2]+C.px_), (0, x.shape[3]), (0, x.shape[4]))) - - x = x.movement_op(MovementOps.STRIDED, ( - (C.bs, x.shape[1]*x.shape[2]*C.groups*C.cin), - (C.oy, C.sy*x.shape[2]*C.groups*C.cin), (C.ox, C.sx*C.groups*C.cin), - (C.groups, C.cin), (1, 1), (1, 1), - (C.H, C.dy*x.shape[2]*C.groups*C.cin), (C.W, C.dx*C.groups*C.cin), (C.cin//4 if C.cin >= 4 else 1, 4), (4 if C.cin >= 4 else 1, 1) - )) - x = x.movement_op(MovementOps.EXPAND, (C.bs, C.oy, C.ox, C.groups, C.rcout//4 if C.rcout >= 4 else 1, 4 if C.rcout >= 4 else 1, C.H, C.W, C.cin//4 if C.cin >= 4 else 1, 4 if C.cin >= 4 else 1)) - x = x.movement_op(MovementOps.RESHAPE, (C.bs, C.oy, C.ox, C.cout//4, 4, C.H, C.W, C.cin//4 if C.cin >= 4 else 1, 4 if C.cin >= 4 else 1)) - - # set up the weights - if C.cin == 1: - # depthwise - w = w.movement_op(MovementOps.RESHAPE, (C.cout//4, C.H, C.W, 4)) - w = w.movement_op(MovementOps.PERMUTE, (0,3,1,2)) - w = w.movement_op(MovementOps.RESHAPE, (1, 1, 1, C.cout//4, 4, C.H, C.W, 1, 1)) \ - .movement_op(MovementOps.EXPAND, (C.bs, C.oy, C.ox, C.cout//4, 4, C.H, C.W, 1, 1)) - else: - w = w.movement_op(MovementOps.RESHAPE, (C.cout//4, C.H, C.cin//4, C.W, 4, 4)) - w = w.movement_op(MovementOps.PERMUTE, (0,4,1,3,2,5)) - w = w.movement_op(MovementOps.RESHAPE, (1, 1, 1, C.cout//4, 4, C.H, C.W, C.cin//4, 4)) \ - .movement_op(MovementOps.EXPAND, (C.bs, C.oy, C.ox, C.cout//4, 4, C.H, C.W, C.cin//4, 4)) - - # now do the conv in this space - ret = x.binary_op(BinaryOps.MUL, w).reduce_op(ReduceOps.SUM, (C.bs, C.oy, C.ox, C.cout//4, 4, 1, 1, 1, 1)) - ret = ret.movement_op(MovementOps.RESHAPE, (C.bs*C.oy, C.ox*C.cout//4, 4)).contiguous() #True) - return postprocessing_op(ret, C, Cold) - - # TODO: fixup C? - if NOCONV or not getattr(x.dbuffer, "SUPPORTS_PADDING", False): - x = x.slice(((0, x.shape[0]), (0, x.shape[1]), (-C.py, x.shape[2]+C.py_), (-C.px, x.shape[3]+C.px_))) - - if NOCONV or not getattr(x.dbuffer, "processing_op", False): - # universal conv, just mul and reduce - # TODO: is there any way to replace strided with other movement ops? answer: not really - if C.sy == 1 and C.sx == 1 and C.H == 1 and C.W == 1 and False: - # TODO: this doesn't belong here, ShapeTracker or lazy should be able to infer this from STRIDED - # TODO: this is disabled. it breaks fusion of ops without pushing PERMUTES. this is also a depthwise conv - x = x.movement_op(MovementOps.RESHAPE, (C.bs, C.groups, C.cin, C.oy, C.ox, 1, C.H, C.W)) - x = x.movement_op(MovementOps.PERMUTE, (0,1,5,3,4,2,6,7)) - else: - x = x.movement_op(MovementOps.STRIDED, ( - (C.bs, C.groups*C.cin*x.shape[2]*x.shape[3]), (C.groups, C.cin*x.shape[2]*x.shape[3]), - (1, 1), (C.oy, C.sy*x.shape[3]), (C.ox, C.sx), - (C.cin, x.shape[2]*x.shape[3]), (C.H, C.dy*x.shape[3]), (C.W, C.dx))) - #if C.H <= 3 and C.W <= 3: # max 9x the RAM overhead, this is im2col - # x = x.contiguous() - x = x.movement_op(MovementOps.EXPAND, (C.bs, C.groups, C.rcout, C.oy, C.ox, C.cin, C.H, C.W)) - w = w.movement_op(MovementOps.RESHAPE, (1, C.groups, C.rcout, 1, 1, C.cin, C.H, C.W)) \ - .movement_op(MovementOps.EXPAND, (C.bs, C.groups, C.rcout, C.oy, C.ox, C.cin, C.H, C.W)) - return x.binary_op(BinaryOps.MUL, w).reduce_op(ReduceOps.SUM, (C.bs, C.groups, C.rcout, C.oy, C.ox, 1, 1, 1)) \ - .movement_op(MovementOps.RESHAPE, (C.bs, C.cout, C.oy, C.ox)) - elif x.device == "OPENCL": - # TODO: these can be properties on the device buffer - from accel.opencl.preprocessing import preprocessing_op, postprocessing_op # type: ignore - x,w,Cn = preprocessing_op(x, w, C) - ret = LazyBuffer(x.device, Cn.out_shape, ProcessingOps, LazyOp(op, (x, w), Cn)) - return postprocessing_op(ret, Cn, C) - else: - return LazyBuffer(x.device, C.out_shape, ProcessingOps, LazyOp(op, (x, w), C)) - def elementwise_op(op:Union[UnaryOps, BinaryOps], *srcs:LazyBuffer) -> LazyBuffer: out_device, out_shape = srcs[0].device, srcs[0].shape + # push all contiguous to the end of BinaryOps. kernels 198 -> 196 + if PUSH_CONTIGUOUS and any(x.realized is None and x.op.op == LoadOps.CONTIGUOUS and len(x.op.src[0].children) <= 1 for x in srcs): + new_srcs = [] + for x in srcs: + if x.realized is None and x.op.op == LoadOps.CONTIGUOUS and len(x.op.src[0].children) <= 1: + x.op.src[0].children.discard(x) + new_srcs.append(x.op.src[0]) + else: + new_srcs.append(x) + return elementwise_op(op, *new_srcs).contiguous() + if MERGE_ELEMENTWISE_OPS or (MERGE_UNARY_OPS and len(set(srcs)) == 1): # remove the buffers from any (childless) BinaryOps that feed into this srcs = tuple(x.op if x.optype == BinaryOps and len(x.children) == 0 and x.realized is None else x for x in srcs) # type: ignore diff --git a/tinygrad_repo/tinygrad/llops/ops_gpu.py b/tinygrad_repo/tinygrad/llops/ops_gpu.py deleted file mode 100644 index bb75ad16b..000000000 --- a/tinygrad_repo/tinygrad/llops/ops_gpu.py +++ /dev/null @@ -1,432 +0,0 @@ -from __future__ import annotations -import os, functools -import numpy as np -import pyopencl as cl # type: ignore -from collections import defaultdict -from typing import List, Tuple, Optional, Dict, Union, Set -from tinygrad.helpers import prod -from tinygrad.ops import DEBUG, UnaryOps, BinaryOps, ReduceOps, MovementOps, LazyOp, Op, ExplicitExecAST, GlobalCounters -from tinygrad.ast import ASTKernel, Token, Types -from tinygrad.lazy import IMAGE -from tinygrad.shape import ShapeTracker, View, ZeroView -from tinygrad.shape.symbolic import Variable, ModNode - -VALIDHACKS = int(os.getenv("VALIDHACKS", "0")) # TODO: remove the need for this -NATIVE_EXPLOG = int(os.getenv("NATIVE_EXPLOG", "0")) # this is needed as a switch for the tests to pass - -CLCACHE = int(os.getenv("CLCACHE", "1")) -FLOAT16 = int(os.getenv("FLOAT16", "0")) -PRINT_AST = int(os.getenv("PRINT_AST", "0")) -TEST_AST = int(os.getenv("TEST_AST", "0")) - -class CLBuffer: - def __init__(self, size): - if len(CL.BUFFER_CACHE[size]) > 0: - self.cl = CL.BUFFER_CACHE[size].pop() - else: - # TODO: on GPU OOM, clear the cache - self.cl = cl.Buffer(CL().cl_ctx, cl.mem_flags.READ_WRITE, size) - CL.mem_used += self.cl.size - - def __del__(self): - if CLCACHE: - CL.BUFFER_CACHE[self.cl.size].append(self.cl) - else: - CL.mem_used -= self.cl.size - -class CLImage: - fmt = cl.ImageFormat(cl.channel_order.RGBA, cl.channel_type.HALF_FLOAT if FLOAT16 else cl.channel_type.FLOAT) - - def __init__(self, shape): - self.cl = cl.Image(CL().cl_ctx, cl.mem_flags.READ_WRITE, CLImage.fmt, shape=(shape[1], shape[0])) - CL.mem_used += self.cl.row_pitch * self.cl.height - - def __del__(self): - CL.mem_used -= self.cl.row_pitch * self.cl.height - -class CL: - CACHE, kernel_count, mem_used, time_sum, ops_sum = None, -1, 0, 0.0, 0.0 - BUFFER_CACHE : Dict[int, List[cl.Buffer]] = defaultdict(list) - cl_ctx : Optional[cl.Context] = None - cl_queue : Optional[cl.CommandQueue] = None - def __init__(self): - if CL.cl_queue is not None: return # already initted - devices = sum([x.get_devices(device_type=cl.device_type.GPU) for x in cl.get_platforms()], []) - if len(devices) == 0: # settle for CPU - devices = sum([x.get_devices(device_type=cl.device_type.CPU) for x in cl.get_platforms()], []) - CL.cl_ctx = cl.Context(devices=[devices[int(os.getenv("CL_DEVICE", "0"))]]) - if len(devices) > 1 or DEBUG >= 1: print(f"using {CL.cl_ctx.devices}") - CL.cl_queue = cl.CommandQueue(self.cl_ctx, properties=cl.command_queue_properties.PROFILING_ENABLE) # this is an in-order command queue - - @staticmethod - def enqueue_copy(a, b, is_blocking=False): - if CL.CACHE is not None: assert False, f"can't copy {a} -> {b} while caching" - if DEBUG >= 1: print(f"**CL** copy in {b.shape}" if isinstance(b, np.ndarray) else f"**CL** copy OUT {a.shape}") - cl.enqueue_copy(CL().cl_queue, a, b, is_blocking=is_blocking) - -@functools.lru_cache(maxsize=None) -class CLProgram: - kernel_cnt : Dict[str, int] = defaultdict(int) - def __init__(self, name:str, prg:str, options:Tuple[str, ...]=tuple(), argdtypes=None, rename=True, binary=False, op_estimate=0): - self.name = f"{name}{('_N'+str(CLProgram.kernel_cnt[name])) if CLProgram.kernel_cnt[name] else ''}" if rename else name - self.prg, self.options, self.argdtypes, self.op_estimate = prg.replace(f"{name}(", f"{self.name}(") if rename else prg, options, argdtypes, op_estimate - self.clprogram = cl.Program(CL().cl_ctx, CL().cl_ctx.devices, [self.prg]) if binary else cl.Program(CL().cl_ctx, self.prg) # type: ignore - try: - self.clprg = self.clprogram.build(options=list(self.options)).__getattr__(self.name) - except cl.RuntimeError as e: - print("FAILED TO BUILD", self.prg) - raise e - if self.argdtypes is not None: - self.clprg.set_scalar_arg_dtypes(self.argdtypes) - CLProgram.kernel_cnt[name] += 1 - def __call__(self, *args): - CL.kernel_count += 1 - if CL.CACHE is not None: CL.CACHE.append((self, args)) - else: e = self.clprg(CL().cl_queue, *args) - if DEBUG >= 4: print(self.prg) - if DEBUG >= 2: CL.cl_queue.finish() - if DEBUG >= 1: - CL.time_sum += 0 if DEBUG <= 1 or CL.CACHE is not None else (e.profile.end - e.profile.start) - CL.ops_sum += self.op_estimate - print(f"**CL** {CL.kernel_count:6d} {self.name:28s} args {len(args[2:]):5d} kernels {str(args[0]):18s} {str(args[1]):12s} OPs {self.op_estimate/1e6:7.1f}M/{CL.ops_sum/1e9:7.2f}G mem {CL.mem_used/1e9:5.2f} GB " + - (str() if DEBUG <= 1 or CL.CACHE is not None else f"tm {(e.profile.end - e.profile.start)/1e3:9.2f}us/{CL.time_sum/1e6:9.2f}ms ({self.op_estimate/(e.profile.end - e.profile.start):8.2f} GFLOPS)")) - GlobalCounters.global_ops += self.op_estimate - GlobalCounters.global_mem += sum([x.size//4 for x in args[2:] if isinstance(x, cl.Buffer)]) - -# **** end CL wrappers **** - -def group_float4(x): - assert all(y.typ == Types.FLOAT for y in x) and len(x)%4 == 0 - return [Token(f"(float4)({','.join([x[i+j].tok for j in range(4)])})", Types.FLOAT4) for i in range(0, len(x), 4)] -def split_float4(x): - assert all(y.typ == Types.FLOAT4 for y in x) - return sum([[Token(acc.tok+f".s{s}", Types.FLOAT) for s in range(4)] for acc in x], []) - -class CLASTKernel(ASTKernel): - code_for_op : Dict[Op, str] = { - UnaryOps.NOOP: "(A)", UnaryOps.NEG: "(-(A))", UnaryOps.RELU: "max(A, (float)0.)", UnaryOps.SIGN: "sign(A)", - UnaryOps.EXP: "native_exp(A)" if NATIVE_EXPLOG else "exp(A)", - UnaryOps.LOG: "native_log(A)" if NATIVE_EXPLOG else "log(A)", - UnaryOps.RECIPROCAL: "native_recip(A)" if NATIVE_EXPLOG else "((float)1.0/A)", - BinaryOps.ADD: "(A+B)", BinaryOps.SUB: "(A-B)", BinaryOps.MUL: "(A*B)", - BinaryOps.DIV: "(A/B)", BinaryOps.POW: "pow(A,B)", BinaryOps.CMPEQ: "(A==B)", - ReduceOps.SUM: "A+=B", ReduceOps.MAX: "A=max(A,B)" - } - start_for_op = {ReduceOps.SUM: "0.0", ReduceOps.MAX: "-INFINITY"} - - # TODO: move to shapetracker - def compute_buf_index_symbolic(self, st, buf_index, offset=0): - view = View(self.shapes[buf_index], self.strides[buf_index], self.offsets[buf_index] + offset) - idx = view.expr_idxs([f"idx{i}" for i in range(self.shape_len)]) - valid = Variable.num(1) - for v in st.views[0:-1][::-1]: - if isinstance(v, ZeroView): valid = v.expr_node(valid, idx) - else: idx = v.expr_node(idx) - return idx, valid - - def store(self, buf_index, value:List[Token]): - if len(value) == self.buftokens[buf_index].size()*4: value = group_float4(value) - if len(value)*4 == self.buftokens[buf_index].size(): value = split_float4(value) - assert len(value) == self.buftokens[buf_index].size(), f"size mismatch {len(value)} != {self.buftokens[buf_index].size()}" - for v, o in zip(value, self.buftokens[buf_index].offsets()): - idxy, valid = self.compute_buf_index_symbolic(self.bufs[buf_index].st, buf_index, o) - assert str(valid) == "1" - if isinstance(self.bufs[buf_index]._buf, CLImage): - assert self.buftokens[buf_index].typ == Types.FLOAT4, "image must be FLOAT4" - idx = (idxy//4)%self.bufs[buf_index]._base_shape[1] - idy = (idxy//(4*self.bufs[buf_index]._base_shape[1]))%self.bufs[buf_index]._base_shape[0] - self.kernel.append(f"write_imagef(data{buf_index}, (int2)({idx.cl}, {idy.cl}), {v.tok}); /* {self.bufs[buf_index]._base_shape} */\n") - else: - assert self.buftokens[buf_index].typ == v.typ, f"buf must be {v.typ}" - self.kernel.append(f"data{buf_index}[{(idxy//(4 if v.typ == Types.FLOAT4 else 1)).cl}] = {v.tok};\n") - - def load(self, buf_index:int) -> List[Token]: - tokens = [] - - # constant folding - if self.bufs[buf_index]._base_shape == (1,) and self.bufs[buf_index]._backing is not None: - assert self.buftokens[buf_index].typ == Types.FLOAT - self.bufs_to_delete.add(buf_index) - const = Token(f"({self.bufs[buf_index]._backing[0]}f)", self.buftokens[buf_index].typ) - if self.bufs[buf_index].st.needs_valid(): - for o in self.buftokens[buf_index].offsets(): - _, valid = self.compute_buf_index_symbolic(self.bufs[buf_index].st, buf_index, o) - tokens.append(Token(f"({valid.cl} ? {const.tok} : 0.0f)", const.typ) if str(valid) != "1" else const) - return tokens - else: - return [const]*self.buftokens[buf_index].size() - - # not constant folded - for o in self.buftokens[buf_index].offsets(): - if (buf_index, o) not in self.loaded_keys: - idxy, valid = self.compute_buf_index_symbolic(self.bufs[buf_index].st, buf_index, o) - if isinstance(self.bufs[buf_index]._buf, CLImage): - assert self.buftokens[buf_index].typ == Types.FLOAT4, f"image must be FLOAT4 {self.buftokens[buf_index]} {self.bufs[buf_index].st}" - idx = (idxy//4)%self.bufs[buf_index]._base_shape[1] - idy = (idxy//(4*self.bufs[buf_index]._base_shape[1]))%self.bufs[buf_index]._base_shape[0] - - if VALIDHACKS: - if isinstance(idx, ModNode) and idx.max < idx.b*2: idx = idx.a - if isinstance(idy, ModNode) and idy.max < idy.b*2: idy = idy.a - valid = None - - ldrt = f"read_imagef({self.buftokens[buf_index].tok}, smp, (int2)({idx.cl}, {idy.cl})) /* {self.bufs[buf_index]._base_shape} */" - ldr = Token(f"({valid.cl} ? \\ \n {ldrt} : (float4)(0.0, 0.0, 0.0, 0.0))" if str(valid) != "1" and valid is not None else ldrt, Types.FLOAT4) - else: - ldr = Token(f"{self.buftokens[buf_index].tok}[{(idxy//(4 if self.buftokens[buf_index].typ == Types.FLOAT4 else 1)).cl}]", self.buftokens[buf_index].typ) - ldr = Token(f"({valid.cl} ? {ldr.tok} : 0.0f)", ldr.typ) if str(valid) != "1" else ldr - self.kernel.append(f"{ldr.decltype()} val{buf_index}_{o} = {ldr.tok};\n") - self.loaded_keys[(buf_index,o)] = Token(f"val{buf_index}_{o}", ldr.typ) - tokens.append(self.loaded_keys[(buf_index,o)]) - return tokens - - def ast_parse(self, x:Union[GPUBuffer, LazyOp], acc:List[Token], do_reduce=False) -> List[Token]: - if not isinstance(x, LazyOp): return self.load(self.bufs.index(x)) - if isinstance(x.op, ReduceOps) and not do_reduce: return acc - values = ([acc] if isinstance(x.op, ReduceOps) else []) + [self.ast_parse(v, acc, do_reduce) for v in x.src] - code = CLASTKernel.code_for_op[x.op] # TODO: replace this with a function - if len(values) == 2: - # TODO: sometimes this is split, sometimes it's multiply - if isinstance(x.op, ReduceOps) and values[0][0].typ == Types.FLOAT4 and len(values[0])*4 == len(values[1]): values[0] = split_float4(values[0]) - if values[0][0].typ != values[1][0].typ: - if isinstance(x.op, ReduceOps): - if x.op == ReduceOps.SUM: self.prekernel.add("float clreduce(float4 x) { return x.x + x.y + x.z + x.w; }\n") - elif x.op == ReduceOps.MAX: self.prekernel.add("float clreduce(float4 x) { return max(max(x.x, x.y), max(x.z, x.w)); }\n") - values[1] = [Token(f"clreduce({x.tok})", Types.FLOAT) for x in values[1]] - elif values[0][0].typ == Types.FLOAT: values[0] = group_float4(values[0]) - elif values[1][0].typ == Types.FLOAT: values[1] = group_float4(values[1]) - assert len(values[0]) == len(values[1]), f"values mismatch {values}" - return [Token(code.replace("A", a.tok).replace("B", b.tok), a.typ) for a,b in zip(values[0], values[1])] - else: - return [Token(code.replace("A", a.tok), a.typ) for a in values[0]] - - def codegen(self): - # TODO: fetch from quick cache before processing - self.process() - if DEBUG >= 3: - print("old:", self.shapes) - print("old:", self.strides) - - self.prekernel = set() - - # if there's images in the earlybufs, we have to make an axis the 4 loading one - # shove the axis to the end and remove - if any(isinstance(buf._buf, CLImage) for buf in self.earlybufs): - eb_valids = [True] * len(self.shapes[0]) - for i in range(len(self.bufs)): - if isinstance(self.bufs[i]._buf, CLImage) and self.bufs[i] in self.earlybufs: - valids = [self.shapes[i][j]%4 == 0 and self.strides[i][j] == 1 for j in range(len(self.shapes[i]))] - eb_valids = [x and y for x,y in zip(eb_valids, valids)] - assert any(eb_valids), f"invalid op with images {eb_valids}" - eb_valid = eb_valids.index(True) - if DEBUG >= 3: print(f"early merging axis {eb_valid} from {eb_valids}") - - # no change, we added a dimension - self.reshape_and_permute( - lambda x: list(x[0:eb_valid]) + ([x[eb_valid]//4, 4] if x[eb_valid] > 1 else [1,1]) + list(x[eb_valid+1:]), - [i for i in range(self.shape_len+1) if i != eb_valid+1] + [eb_valid+1]) - - # drop the last dimension - self.upcast() - - # simplify (sets first_reduce) - self.simplify_ones() - - # are we grouping? - self.group_for_reduce = [] - if self.buftokens[0].typ != Types.FLOAT4 and self.first_reduce <= 2 and self.first_reduce + 1 <= self.shape_len and prod(self.shapes[0][:self.first_reduce]) <= 2048: - for sz in ([256, 16] if prod(self.shapes[0][:self.first_reduce]) <= 32 else [16]): - if all([x[self.first_reduce] % sz == 0 or x[self.first_reduce] == 1 for x in self.shapes]): - self.group_for_reduce.append(sz) - break - - # if there's images in the latebufs, we have to make an axis the 4 storing one. this affects the kernel shape - self.upcast_in_mid_reduce = False - if any(isinstance(buf._buf, CLImage) for buf in self.bufs if buf not in self.earlybufs) and self.buftokens[0].typ != Types.FLOAT4: - lb_valids = [True] * len(self.shapes[0]) - for i in range(len(self.bufs)): - valids = [self.shapes[i][j]%4 == 0 and (self.strides[i][j] == 1 or not isinstance(self.bufs[i]._buf, CLImage) or self.bufs[i] in self.earlybufs) for j in range(len(self.shapes[i]))] - lb_valids = [x and y for x,y in zip(lb_valids, valids)] - assert any(lb_valids), f"invalid op with images {lb_valids}" - lb_valid = lb_valids.index(True) - assert lb_valid < self.first_reduce, f"can't be in the reduce {lb_valid}" - if DEBUG >= 3: print(f"late merging axis {lb_valid} from {lb_valids}") - - # no change, we added a dimension - self.reshape_and_permute( - lambda x: list(x[0:lb_valid]) + [x[lb_valid]//4, 4] + list(x[lb_valid+1:]), - [i for i in range(self.shape_len+1) if i != lb_valid+1] + [lb_valid+1]) - - if self.group_for_reduce and self.first_reduce <= 2: - self.upcast_in_mid_reduce = True - self.group_for_reduce.append(4) - else: - # drop the last dimension - self.upcast() - - # simplify (sets first_reduce) - self.simplify_ones() - - # split to 4 float4s - if self.buftokens[0].typ == Types.FLOAT4 and any(isinstance(buf._buf, CLImage) for buf in self.earlybufs) and prod(self.shapes[0][:self.first_reduce]) >= 2048 and not self.group_for_reduce: - xb_choices = [] - for i in range(self.first_reduce): - if all(x[i]%4 == 0 for x in self.shapes): - xb_choices.append((sum(x[i]>0 for x in self.strides), sum(x[i] for x in self.strides), i)) - - if len(xb_choices): - xb_choice = sorted(xb_choices)[0][2] - if DEBUG >= 3: print(f"float4 merging axis {xb_choice} : {xb_choices}") - - # this leaves the last axis in place - self.reshape_and_permute( - lambda x: list(x[0:xb_choice]) + [x[xb_choice]//4, 4] + list(x[xb_choice+1:]), - [i for i in range(self.shape_len+1) if i != xb_choice+1] + [xb_choice+1]) - - # drop the last dimension - self.upcast() - - # re-simplify - self.simplify_ones() - - # use more opencl indexing - if self.first_reduce == 2 and isinstance(self.bufs[0]._buf, CLImage): - base_shape = self.bufs[0]._base_shape - if all([(base_shape[0]*base_shape[1])%x[0] == 0 and x[0]//base_shape[0] != 0 for x in self.shapes]): - if DEBUG >= 3: print("split opencl", base_shape, self.shapes[0]) - self.reshape_and_permute(lambda x: [base_shape[0], x[0]//base_shape[0]]+list(x[1:]), None) - self.simplify_ones() - - # group for reduce - self.output_shape = self.shapes[0][:self.first_reduce] - if len(self.group_for_reduce): - # with permute for memory coalesing - if len(self.group_for_reduce) == 2: - permute_axis = list(range(0, self.first_reduce)) + [self.first_reduce+1, self.shape_len, self.first_reduce] + list(range(self.first_reduce+2, self.shape_len)) - else: - permute_axis = list(range(0, self.first_reduce)) + [self.first_reduce+1, self.first_reduce] + list(range(self.first_reduce+2, self.shape_len+1)) - self.reshape_and_permute(lambda x: list(x[0:self.first_reduce]) + [max(1, x[self.first_reduce]//self.group_for_reduce[0]), min(x[self.first_reduce], self.group_for_reduce[0])] + list(x[self.first_reduce+1:]), permute_axis) - - self.first_reduce += len(self.group_for_reduce) - self.output_shape += self.group_for_reduce - - if DEBUG >= 3: - print(f"first_reduce: {self.first_reduce} shape_len: {self.shape_len}") - print("output shape", self.output_shape) - for i in range(len(self.bufs)): - print(self.buftokens[i], self.bufs[i] in self.earlybufs, self.shapes[i], self.strides[i]) - - self.bufs_to_delete : Set[int] = set() - self.loaded_keys : Dict[Tuple[int,int], Token] = {} - - self.kernel : List[str] = ["const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;\n"] - self.kernel += [f"int idx{i} = get_global_id({min(3, len(self.output_shape))-1-i}); /* {self.output_shape[i]} */\n" for i in range(min(3, len(self.output_shape)))] - if len(self.output_shape) > 3: - # compact all the dimensions into the final one - for i in range(len(self.output_shape)-1, 2, -1): - self.kernel += [f"int idx{i} = idx2 % {self.output_shape[i]};", f"idx2 = idx2 / {self.output_shape[i]};\n"] - self.output_shape = list(self.output_shape[0:2]) + [prod(self.output_shape[2:])] - - # early ast - accumulators : List[Token] = [Token("acc%d" % i, self.buftokens[0].typ) for i in range(self.buftokens[0].size())] - if self.reduceop: - full_shape = [x for x in self.shapes if x != self.shapes[0]] - full_shape = self.shapes[0] if len(full_shape) == 0 else full_shape[0] - - self.kernel += [f"{accumulator.decltype()} {accumulator.tok} = {CLASTKernel.start_for_op[self.reduceop.op]};\n" for accumulator in accumulators] - self.kernel += [f"for (int idx{i} = 0; idx{i} < {full_shape[i]}; idx{i}++) {{\n" for i in range(self.first_reduce, self.shape_len)] - self.kernel += [f"{x.tok};\n" for x in self.ast_parse(self.reduceop, accumulators, do_reduce=True)] + ["}\n"] * (self.shape_len - self.first_reduce) - - # middle - if self.group_for_reduce: - self.kernel.append(f"__local {accumulators[0].decltype()} temp[{prod(self.group_for_reduce)}]; // second stage\n") - - if self.upcast_in_mid_reduce: - # it should be the last dimension - self.kernel.append(f"int mid_idx = idx{self.first_reduce-2}*{self.group_for_reduce[1]} + idx{self.first_reduce-1}; temp[mid_idx] = {accumulators[0].tok}; barrier(CLK_LOCAL_MEM_FENCE);\n") - self.reshape_and_permute(None, [i for i in range(self.shape_len) if i != self.first_reduce-1] + [self.first_reduce-1]) - self.upcast() - else: - self.kernel.append(f"int mid_idx = idx{self.first_reduce-1}; temp[mid_idx] = {accumulators[0].tok}; barrier(CLK_LOCAL_MEM_FENCE);\n") - - self.kernel.append("if (mid_idx == 0) {\n") - accumulators = [Token("output", self.buftokens[0].typ)] - self.kernel.append(f"{accumulators[0].decltype()} {accumulators[0].tok} = 0.0;\n") - if self.upcast_in_mid_reduce: - self.kernel.append(f"for (int mid = 0; mid < {prod(self.group_for_reduce)//4}; mid++) {{ {CLASTKernel.code_for_op[self.reduceop.op].replace('A', accumulators[0].tok).replace('B', 'vload4(0, &temp[mid*4])')}; }}\n") - else: - self.kernel.append(f"for (int mid = 0; mid < {prod(self.group_for_reduce)}; mid++) {{ {CLASTKernel.code_for_op[self.reduceop.op].replace('A', accumulators[0].tok).replace('B', 'temp[mid]')}; }}\n") - - # late ast - self.store(0, self.ast_parse(self.ast, accumulators)) - if self.group_for_reduce: self.kernel.append("}") - self.kernel.append("}") - - # kernel function definition - function_name = ("re_S" if self.reduceop else "ew_S") + '_'.join([str(x) for x in self.bufs[0].shape if x != 1]) - buftypes = [f"{'read_only' if i > 0 else 'write_only'} image2d_t" if isinstance(x._buf, CLImage) else ("__global "+self.buftokens[i].decltype()) for i,x in enumerate(self.bufs)] - self.kernel = list(self.prekernel) + [f"__kernel void {function_name}(",] + \ - [', '.join([f'{t} data{i}' for i,t in enumerate(buftypes) if i not in self.bufs_to_delete])] + \ - [") {\n"] + self.kernel - - # compile kernel - self.fxn = CLProgram(function_name, ' '.join(self.kernel), op_estimate=self.info.flops) - - if DEBUG >= 3 and len(self.bufs_to_delete): print(f"deleting buffers {self.bufs_to_delete}") - def runner(*bufs): - clbufs = [x.cl for i,x in enumerate(bufs) if i not in self.bufs_to_delete] - return self.fxn(self.output_shape[::-1] if len(self.output_shape) > 0 else [1], (self.group_for_reduce[::-1] + [1]*(len(self.output_shape)-len(self.group_for_reduce))) if self.group_for_reduce else None, *clbufs) - return runner - - def print(self): - super().print() - for i in range(len(self.bufs)): - print(self.buftokens[i], self.bufs[i] in self.earlybufs, self.shapes[i], self.strides[i]) - print(self.fxn.prg) - -class GPUBuffer(ExplicitExecAST): - def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]], hostbuf:Optional[GPUBuffer]=None, backing:Optional[np.ndarray]=None, force_create=False): - super().__init__(shape, hostbuf) - self._buf : Optional[CLBuffer] = hostbuf._buf if hostbuf is not None else None - self._base_shape : Tuple[int, ...] = hostbuf._base_shape if hostbuf is not None else self.shape - self._backing : Optional[np.ndarray] = hostbuf._backing if hostbuf is not None else backing - # early copy in for large buffers - if (self._backing is not None and self._backing.shape != (1,)) or force_create: - self.cl - - @property - def cl(self): - if self._buf is None: - possible_split_shape = [x for x in self._base_shape if x != 1] - # TODO: this is broken, and a hack. I suspect the issue is unaligned float4 accesses, would be caught by the Image valid thing if it worked. - if IMAGE >= 3 and len(possible_split_shape) == 1 and possible_split_shape[0] % 4 == 0 and self._backing is None and possible_split_shape[0] != 6140: - self._base_shape = (1, possible_split_shape[0]//4, 4) - self._buf = CLImage(self._base_shape) if (len(self._base_shape) == 3 and self._base_shape[2] == 4 and IMAGE >= 2) else CLBuffer(4*prod(self._base_shape)) - if self._backing is not None: - CL().enqueue_copy(self._buf.cl, self._backing, is_blocking=False) - self._backing = None - return self._buf.cl - - def __repr__(self): return f"GPUBuffer(shape={self.st}, hostbuf=GPUBuffer(shape={self._base_shape}" + (f", backing=np.array({self._backing}, dtype=np.float32)))" if self._backing else ", force_create=True))") - - @staticmethod - def fromCPU(x): return GPUBuffer(x.shape, backing=x.view(np.ndarray).astype(np.float32).ravel()) - - def toCPU(self): - data = np.empty(self.shape, dtype=np.float32) - cl_buf = self.contiguous() - cl_buf = cl_buf if isinstance(cl_buf._buf, CLBuffer) else self.movement_op(MovementOps.RESHAPE, list(self.shape)+[1]).unary_op(UnaryOps.NOOP) - CL().enqueue_copy(data, cl_buf.cl, is_blocking=True) - return data - - @classmethod - def exec_ast(cls, ast:LazyOp): - k = CLASTKernel(ast) - k.codegen()(*k.bufs) - if PRINT_AST: - print(k.fxn.name) - k.print() - if TEST_AST: - from test.lib_test_ast import test_ast # type: ignore - test_ast(k) - return k.ret diff --git a/tinygrad_repo/tinygrad/llops/ops_opencl.py b/tinygrad_repo/tinygrad/llops/ops_opencl.py deleted file mode 120000 index 02c9307b0..000000000 --- a/tinygrad_repo/tinygrad/llops/ops_opencl.py +++ /dev/null @@ -1 +0,0 @@ -../../accel/opencl/ops_opencl.py \ No newline at end of file diff --git a/tinygrad_repo/tinygrad/mlops.py b/tinygrad_repo/tinygrad/mlops.py index 7b72123e7..2eba53844 100644 --- a/tinygrad_repo/tinygrad/mlops.py +++ b/tinygrad_repo/tinygrad/mlops.py @@ -1,5 +1,5 @@ -from tinygrad.helpers import prod, argsort, reduce_shape, get_conv_args -from tinygrad.ops import UnaryOps, BinaryOps, ReduceOps, MovementOps, ProcessingOps +from tinygrad.helpers import argsort +from tinygrad.ops import UnaryOps, BinaryOps, ReduceOps, MovementOps from tinygrad.tensor import Function class Contiguous(Function): @@ -8,74 +8,65 @@ class Contiguous(Function): # ************* unary ops ************* -class ReLU(Function): - def forward(self, x): - ret = x.unary_op(UnaryOps.RELU) - self.save_for_backward(ret) - return ret - - def backward(self, grad_output): - return self.saved_tensors[0].unary_op(UnaryOps.SIGN).binary_op(BinaryOps.MUL, grad_output) - class Log(Function): def forward(self, x): - self.save_for_backward(x) + self.x = x return x.unary_op(UnaryOps.LOG) def backward(self, grad_output): - return grad_output.binary_op(BinaryOps.DIV, self.saved_tensors[0]) + return grad_output.binary_op(BinaryOps.DIV, self.x) class Exp(Function): def forward(self, x): - ret = x.unary_op(UnaryOps.EXP) - self.save_for_backward(ret) - return ret + self.ret = x.unary_op(UnaryOps.EXP) + return self.ret def backward(self, grad_output): - return self.saved_tensors[0].binary_op(BinaryOps.MUL, grad_output) - -class Reciprocal(Function): - def forward(self, x): - ret = x.unary_op(UnaryOps.RECIPROCAL) - self.save_for_backward(ret) - return ret - - def backward(self, grad_output): - return grad_output.unary_op(UnaryOps.NEG).binary_op(BinaryOps.MUL, self.saved_tensors[0]).binary_op(BinaryOps.MUL, self.saved_tensors[0]) - -# TODO: add Neg? confirm the optimizer on Sub good enough + return self.ret.binary_op(BinaryOps.MUL, grad_output) # ************* reduce ops ************* class Sum(Function): - def forward(self, x, axis=None): + def forward(self, x, new_shape): self.input_shape = x.shape - return x.reduce_op(ReduceOps.SUM, reduce_shape(x.shape, axis)) + return x.reduce_op(ReduceOps.SUM, new_shape) def backward(self, grad_output): return grad_output.movement_op(MovementOps.EXPAND, self.input_shape) class Max(Function): - def forward(self, x, axis=None): - ret = x.reduce_op(ReduceOps.MAX, reduce_shape(x.shape, axis)) - self.save_for_backward(x, ret) - return ret + def forward(self, x, new_shape): + self.x, self.ret = x, x.reduce_op(ReduceOps.MAX, new_shape) + return self.ret def backward(self, grad_output): - x, ret = self.saved_tensors - # 1s in locations where the max was chosen (can be two locations) - max_is_1s = x.binary_op(BinaryOps.CMPEQ, ret.movement_op(MovementOps.EXPAND, x.shape)) + max_is_1s = self.x.binary_op(BinaryOps.CMPEQ, self.ret.movement_op(MovementOps.EXPAND, self.x.shape)) # sum of locations, averaged - div = max_is_1s.reduce_op(ReduceOps.SUM, grad_output.shape).movement_op(MovementOps.EXPAND, x.shape) + div = max_is_1s.reduce_op(ReduceOps.SUM, grad_output.shape).movement_op(MovementOps.EXPAND, self.x.shape) max_is_amount = max_is_1s.binary_op(BinaryOps.DIV, div) - grad_output_expanded = grad_output.movement_op(MovementOps.EXPAND, x.shape) + grad_output_expanded = grad_output.movement_op(MovementOps.EXPAND, self.x.shape) return max_is_amount.binary_op(BinaryOps.MUL, grad_output_expanded) # ************* binary ops ************* +class Equal(Function): + def forward(self, x, y): + return x.binary_op(BinaryOps.CMPEQ, y) + +class Maximum(Function): + def forward(self, x, y): + self.y, self.ret = y, x.binary_op(BinaryOps.MAX, y) + return self.ret + + def backward(self, grad_output): + mask = self.y.binary_op(BinaryOps.CMPEQ, self.ret) + # TODO: if they are equal, do they split the gradient? + return grad_output.binary_op(BinaryOps.MUL, mask.unary_op(UnaryOps.NOT)) if self.needs_input_grad[0] else None, \ + grad_output.binary_op(BinaryOps.MUL, mask) if self.needs_input_grad[1] else None + class Add(Function): def forward(self, x, y): return x.binary_op(BinaryOps.ADD, y) @@ -94,25 +85,30 @@ class Sub(Function): class Mul(Function): def forward(self, x, y): - self.save_for_backward(x, y) + self.x, self.y = x, y return x.binary_op(BinaryOps.MUL, y) def backward(self, grad_output): - return self.saved_tensors[1].binary_op(BinaryOps.MUL, grad_output) if self.needs_input_grad[0] else None, \ - self.saved_tensors[0].binary_op(BinaryOps.MUL, grad_output) if self.needs_input_grad[1] else None + return self.y.binary_op(BinaryOps.MUL, grad_output) if self.needs_input_grad[0] else None, \ + self.x.binary_op(BinaryOps.MUL, grad_output) if self.needs_input_grad[1] else None class Pow(Function): def forward(self, x, y): - ret = x.binary_op(BinaryOps.POW, y) - self.save_for_backward(x, y, ret) - return ret + self.x, self.y, self.ret = x, y, x.binary_op(BinaryOps.POW, y) + return self.ret def backward(self, grad_output): - x,y,powxy = self.saved_tensors - # grad_x = grad_output * y * (pow(x,y)/x) - # grad_y = grad_output * log(x) * pow(x,y) - return grad_output.binary_op(BinaryOps.MUL, y.binary_op(BinaryOps.MUL, powxy.binary_op(BinaryOps.DIV, x))) if self.needs_input_grad[0] else None, \ - grad_output.binary_op(BinaryOps.MUL, x.unary_op(UnaryOps.LOG).binary_op(BinaryOps.MUL, powxy)) if self.needs_input_grad[1] else None + return grad_output.binary_op(BinaryOps.MUL, self.y.binary_op(BinaryOps.MUL, self.ret.binary_op(BinaryOps.DIV, self.x))) if self.needs_input_grad[0] else None, \ + grad_output.binary_op(BinaryOps.MUL, self.x.unary_op(UnaryOps.LOG).binary_op(BinaryOps.MUL, self.ret)) if self.needs_input_grad[1] else None + +class Div(Function): + def forward(self, x, y): + self.x, self.y = x, y + return x.binary_op(BinaryOps.DIV, y) + + def backward(self, grad_output): + return grad_output.binary_op(BinaryOps.DIV, self.y) if self.needs_input_grad[0] else None, \ + grad_output.unary_op(UnaryOps.NEG).binary_op(BinaryOps.MUL, self.x).binary_op(BinaryOps.DIV, self.y.binary_op(BinaryOps.MUL, self.y)) if self.needs_input_grad[1] else None # ************* movement ops ************* @@ -128,7 +124,6 @@ class Expand(Function): class Reshape(Function): def forward(self, x, shape): self.input_shape = x.shape - shape = tuple(-prod(x.shape) // prod(shape) if s == -1 else s for s in shape) return x.movement_op(MovementOps.RESHAPE, shape) def backward(self, grad_output): @@ -142,14 +137,21 @@ class Permute(Function): def backward(self, grad_output): return grad_output.movement_op(MovementOps.PERMUTE, tuple(argsort(self.input_order))) -# TODO: merge Slice and Flip into Stride with the 3 arguments -class Slice(Function): - def forward(self, x, arg=None): - self.narg = tuple((0-p[0], x.shape[i]-p[0]) for i,p in enumerate(arg)) - return x.slice(tuple(arg)) +class Pad(Function): + def forward(self, x, arg): + self.narg = tuple((p[0], s+p[0]) for s,p in zip(x.shape, arg)) + return x.movement_op(MovementOps.PAD, arg) def backward(self, grad_output): - return grad_output.slice(self.narg) + return grad_output.movement_op(MovementOps.SHRINK, self.narg) + +class Shrink(Function): + def forward(self, x, arg): + self.narg = tuple((p[0], s-p[1]) for s,p in zip(x.shape, arg)) + return x.movement_op(MovementOps.SHRINK, arg) + + def backward(self, grad_output): + return grad_output.movement_op(MovementOps.PAD, self.narg) class Flip(Function): def forward(self, x, axis): @@ -158,37 +160,3 @@ class Flip(Function): def backward(self, grad_output): return grad_output.movement_op(MovementOps.FLIP, self.axis) - -# ************* processing ops ************* - -class Conv2D(Function): - def forward(self, x, w, stride=1, groups=1, dilation=1, padding=0): - self.C = get_conv_args(x.shape, w.shape, stride, groups, dilation=dilation, padding=padding) - self.save_for_backward(x,w) - return x.processing_op(ProcessingOps.CONV, w, self.C) - - def backward(self, grad_output): - x, w = self.saved_tensors - C = self.C # conv args from the context - dx, dw = None, None - - if self.needs_input_grad[0]: # compute derivative of inputs using ProcessingOps.CONV (this is a transposed conv) - xt = grad_output - if C.sx > 1 or C.sy > 1: # unstride. NOTE: this is really memory intensive for big strides. (but only when we contiguous it) - xt = xt.movement_op(MovementOps.RESHAPE, (grad_output.shape[0], grad_output.shape[1], grad_output.shape[2], 1, grad_output.shape[3], 1)) - xt = xt.movement_op(MovementOps.PAD, ((0,0), (0,0), (0,0), (0,C.sy-1), (0,0), (0,C.sx-1))) - xt = xt.movement_op(MovementOps.RESHAPE, (xt.shape[0], xt.shape[1], xt.shape[2]*C.sy, xt.shape[4]*C.sx)) - wt = w.movement_op(MovementOps.RESHAPE, (C.groups, C.rcout, C.cin, C.H, C.W)).movement_op(MovementOps.PERMUTE, (0, 2, 1, 3, 4)).movement_op(MovementOps.FLIP, (3, 4)) - wt = wt.movement_op(MovementOps.RESHAPE, (C.groups*C.cin, C.rcout, C.H, C.W)) - py, px = (C.H-1)*C.dy - C.py, (C.W-1)*C.dx - C.px - Cdx = get_conv_args(xt.shape, wt.shape, out_shape=x.shape, dilation=(C.dy, C.dx), padding=(py, px), groups=C.groups) - dx = xt.processing_op(ProcessingOps.CONV, wt, Cdx) - - if self.needs_input_grad[1]: # compute derivative of weights using ProcessingOps.CONV - xdw = x.movement_op(MovementOps.RESHAPE, (C.bs, C.groups, C.cin, C.iy, C.ix)).movement_op(MovementOps.PERMUTE, (2, 1, 0, 3, 4)) - xdw = xdw.movement_op(MovementOps.RESHAPE, (C.cin, C.groups*C.bs, C.iy, C.ix)) - grad_output_dw = grad_output.movement_op(MovementOps.PERMUTE, (1,0,2,3)) - Cdw = get_conv_args(xdw.shape, grad_output_dw.shape, out_shape=(w.shape[1], w.shape[0], w.shape[2], w.shape[3]), padding=(C.py, C.px), stride=(C.dy, C.dx), dilation=(C.sy, C.sx), groups=C.groups) - dw = xdw.processing_op(ProcessingOps.CONV, grad_output_dw, Cdw).movement_op(MovementOps.PERMUTE, (1,0,2,3)) - - return dx, dw diff --git a/tinygrad_repo/tinygrad/nn/__init__.py b/tinygrad_repo/tinygrad/nn/__init__.py index 989634905..31b919304 100644 --- a/tinygrad_repo/tinygrad/nn/__init__.py +++ b/tinygrad_repo/tinygrad/nn/__init__.py @@ -1,8 +1,9 @@ +from typing import Optional, Union, Tuple from tinygrad.tensor import Tensor -class BatchNorm2D: +class BatchNorm2d: def __init__(self, sz, eps=1e-5, affine=True, track_running_stats=True, momentum=0.1): - assert affine, "BatchNorm2D is only supported with affine" + assert affine, "BatchNorm2d is only supported with affine" self.eps, self.track_running_stats, self.momentum = eps, track_running_stats, momentum self.weight, self.bias = Tensor.ones(sz), Tensor.zeros(sz) @@ -10,7 +11,7 @@ class BatchNorm2D: self.running_mean, self.running_var = Tensor.zeros(sz, requires_grad=False), Tensor.ones(sz, requires_grad=False) self.num_batches_tracked = Tensor.zeros(1, requires_grad=False) - def __call__(self, x): + def __call__(self, x:Tensor): if Tensor.training: # This requires two full memory accesses to x # https://github.com/pytorch/pytorch/blob/c618dc13d2aa23625cb0d7ada694137532a4fa33/aten/src/ATen/native/cuda/Normalization.cuh @@ -19,19 +20,19 @@ class BatchNorm2D: batch_mean = x_detached.mean(axis=(0,2,3)) y = (x_detached - batch_mean.reshape(shape=[1, -1, 1, 1])) batch_var = (y*y).mean(axis=(0,2,3)) - batch_invstd = batch_var.add(self.eps)**-0.5 + batch_invstd = batch_var.add(self.eps).pow(-0.5) self.batch_invstd = None # NOTE: wow, this is done all throughout training in most PyTorch models if self.track_running_stats: - self.running_mean = (1 - self.momentum) * self.running_mean + self.momentum * batch_mean - self.running_var = (1 - self.momentum) * self.running_var + self.momentum * batch_var + self.running_mean.assign((1 - self.momentum) * self.running_mean + self.momentum * batch_mean) + self.running_var.assign((1 - self.momentum) * self.running_var + self.momentum * batch_var) self.num_batches_tracked += 1 else: batch_mean, batch_var = self.running_mean, self.running_var # NOTE: this can be precomputed for static inference. if you manually update running_var, you have to reset this if not hasattr(self, "batch_invstd") or not self.batch_invstd: - self.batch_invstd = batch_var.add(self.eps)**-0.5 + self.batch_invstd = batch_var.add(self.eps).pow(-0.5) batch_invstd = self.batch_invstd return x.batchnorm(self.weight, self.bias, batch_mean, batch_invstd) @@ -42,16 +43,43 @@ class Conv2d: self.kernel_size = (kernel_size, kernel_size) if isinstance(kernel_size, int) else (kernel_size[0], kernel_size[1]) self.stride = (stride, stride) if isinstance(stride, int) else (stride[0], stride[1]) self.padding = (padding, ) * 4 if isinstance(padding, int) else ((padding[0], padding[0], padding[1], padding[1]) if len(padding) == 2 else padding) - self.weight = Tensor.glorot_uniform(out_channels, in_channels, self.kernel_size[0], self.kernel_size[1]) - self.bias = Tensor.zeros(out_channels) if bias else None + # TODO: why is this realize needed? shouldn't it realize on the first run? + self.weight = Tensor.glorot_uniform(out_channels, in_channels, self.kernel_size[0], self.kernel_size[1]).realize() + self.bias = Tensor.zeros(out_channels).contiguous().realize() if bias else None def __call__(self, x): return x.conv2d(self.weight, self.bias, padding=self.padding, stride=self.stride) class Linear: def __init__(self, in_features, out_features, bias=True): - self.weight = Tensor.glorot_uniform(out_features, in_features) - self.bias = Tensor.zeros(out_features) if bias else None + self.weight = Tensor.glorot_uniform(out_features, in_features).realize() + self.bias = Tensor.zeros(out_features).contiguous().realize() if bias else None def __call__(self, x): return x.linear(self.weight.transpose(), self.bias) + +class GroupNorm: + def __init__(self, num_groups:int, num_channels:int, eps:float=1e-5, affine:bool=True): + self.num_groups, self.num_channels, self.eps = num_groups, num_channels, eps + self.weight : Optional[Tensor] = Tensor.ones(num_channels) if affine else None + self.bias : Optional[Tensor] = Tensor.zeros(num_channels) if affine else None + + def __call__(self, x:Tensor): + # reshape for layernorm to work as group norm + # subtract mean and divide stddev + x = x.reshape(x.shape[0], self.num_groups, -1).layernorm(eps=self.eps).reshape(x.shape) + + if self.weight is None or self.bias is None: return x + # elementwise_affine on channels + return x * self.weight.reshape(1, -1, 1, 1) + self.bias.reshape(1, -1, 1, 1) + +class LayerNorm: + def __init__(self, normalized_shape:Union[int, Tuple[int, ...]], eps:float=1e-5, elementwise_affine:bool=True): + normalized_shape = (normalized_shape,) if isinstance(normalized_shape, int) else tuple(normalized_shape) + self.axis, self.eps, self.elementwise_affine = tuple(-1-i for i in range(len(normalized_shape))), eps, elementwise_affine + self.weight, self.bias = (Tensor.ones(*normalized_shape), Tensor.zeros(*normalized_shape)) if elementwise_affine else (None, None) + + def __call__(self, x:Tensor): + x = x.layernorm(eps=self.eps, axis=self.axis) + if not self.elementwise_affine: return x + return x * self.weight + self.bias diff --git a/tinygrad_repo/tinygrad/nn/optim.py b/tinygrad_repo/tinygrad/nn/optim.py index db513a41c..759886c04 100644 --- a/tinygrad_repo/tinygrad/nn/optim.py +++ b/tinygrad_repo/tinygrad/nn/optim.py @@ -1,78 +1,87 @@ # sorted in order of increasing complexity +from typing import List from tinygrad.tensor import Tensor class Optimizer: - def __init__(self, params): + def __init__(self, params : List[Tensor]): # if it's None, but being put into an optimizer, set it to True for x in params: - if x.requires_grad is None: - x.requires_grad = True + if x.requires_grad is None: x.requires_grad = True - self.params = [x for x in params if x.requires_grad] + self.params : List[Tensor] = [x for x in params if x.requires_grad] + self.buffers : List[Tensor] = [x for x in params if not x.requires_grad] # buffers are still realized # TODO: this probably shouldn't change the gradients, just the ones used by the optimizer def clipnorm(self, amount=1): for param in self.params: + assert param.grad is not None # clipnorm is the L2 norm, not value: is this right? param.grad.assign(param.grad.clip(-(amount**2), (amount**2))) def zero_grad(self): - for param in self.params: - param.grad = None + for param in self.params: param.grad = None def realize(self, extra=None): # TODO: corealize - for p in self.params + extra if extra is not None else self.params: + for p in extra + self.params + self.buffers if extra is not None else self.params + self.buffers: p.realize() class SGD(Optimizer): - def __init__(self, params, lr=0.001): + def __init__(self, params : List[Tensor], lr=0.001, momentum=0, nesterov=False): super().__init__(params) - self.lr = lr + self.lr, self.momentum, self.nesterov = lr, momentum, nesterov + self.b = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params] if self.momentum else [] - def step(self): - for t in self.params: - t.assign(t.detach() - t.grad * self.lr) - self.realize() + # https://pytorch.org/docs/stable/generated/torch.optim.SGD.html + def step(self) -> None: + for i, t in enumerate(self.params): + assert t.grad is not None + g = t.grad + if self.momentum: + self.b[i].assign(self.momentum * self.b[i] + g) + g = (g + self.momentum * self.b[i]) if self.nesterov else self.b[i] + t.assign(t.detach() - g * self.lr) + self.realize(self.b) class RMSprop(Optimizer): - def __init__(self, params, lr=0.001, decay=0.9, eps=1e-8): + def __init__(self, params : List[Tensor], lr=0.001, decay=0.9, eps=1e-8): super().__init__(params) self.lr, self.decay, self.eps = lr, decay, eps self.v = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params] - def step(self): + def step(self) -> None: for i, t in enumerate(self.params): - self.v[i] = self.decay * self.v[i] + (1.0 - self.decay) * (t.grad * t.grad) + assert t.grad is not None + self.v[i].assign(self.decay * self.v[i] + (1.0 - self.decay) * (t.grad * t.grad)) t.assign(t.detach() - (t.grad * self.lr).div(self.v[i].sqrt() + self.eps)) self.realize(self.v) class Adam(Optimizer): - def __init__(self, params, lr=0.001, b1=0.9, b2=0.999, eps=1e-8): + def __init__(self, params : List[Tensor], lr=0.001, b1=0.9, b2=0.999, eps=1e-8): super().__init__(params) - self.lr, self.b1, self.b2, self.eps, self.t = lr, b1, b2, eps, 0 + # NOTE: self.t is a tensor so Adam can be jitted + self.lr, self.b1, self.b2, self.eps, self.t = lr, b1, b2, eps, Tensor([0], requires_grad=False).realize() self.m = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params] self.v = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params] - def step(self): + def step(self) -> None: self.t = self.t + 1 a = self.lr * ((1.0 - self.b2**self.t)**0.5) / (1.0 - self.b1**self.t) for i, t in enumerate(self.params): - self.m[i] = self.b1 * self.m[i] + (1.0 - self.b1) * t.grad - self.v[i] = self.b2 * self.v[i] + (1.0 - self.b2) * (t.grad * t.grad) + assert t.grad is not None + self.m[i].assign(self.b1 * self.m[i] + (1.0 - self.b1) * t.grad) + self.v[i].assign(self.b2 * self.v[i] + (1.0 - self.b2) * (t.grad * t.grad)) t.assign(t.detach() - a * self.m[i].div(self.v[i].sqrt() + self.eps)) - self.realize(self.m + self.v) + self.realize([self.t] + self.m + self.v) -def get_parameters(obj): - parameters = [] +def get_parameters(obj) -> List[Tensor]: + parameters : List[Tensor] = [] if isinstance(obj, Tensor): parameters.append(obj) - elif isinstance(obj, list) or isinstance(obj, tuple): - for x in obj: - parameters.extend(get_parameters(x)) + elif isinstance(obj, (list, tuple)): + for x in obj: parameters.extend(get_parameters(x)) elif hasattr(obj, '__dict__'): - for v in obj.__dict__.values(): - parameters.extend(get_parameters(v)) + for v in obj.__dict__.values(): parameters.extend(get_parameters(v)) return parameters diff --git a/tinygrad_repo/tinygrad/ops.py b/tinygrad_repo/tinygrad/ops.py index e523fd558..6c0eb9bb0 100644 --- a/tinygrad_repo/tinygrad/ops.py +++ b/tinygrad_repo/tinygrad/ops.py @@ -1,22 +1,22 @@ -import os -from enum import Enum -from typing import Union, Type, NamedTuple, Tuple, Any, List -import functools, operator -from tinygrad.helpers import prod +from __future__ import annotations +import functools, itertools, operator, random +import numpy as np +from enum import Enum, auto +from typing import Union, Type, NamedTuple, Tuple, Any, List, ClassVar, Optional, Callable, Dict, TypeVar, Set +from tinygrad.helpers import prod, DEBUG, getenv from tinygrad.shape import ShapeTracker -DEBUG = int(os.getenv("DEBUG", "0")) - # these are the llops your accelerator must implement, along with toCpu -UnaryOps = Enum("UnaryOps", ["NOOP", "NEG", "RELU", "EXP", "LOG", "SIGN", "RECIPROCAL"]) -BinaryOps = Enum("BinaryOps", ["ADD", "SUB", "MUL", "DIV", "POW", "CMPEQ"]) -ReduceOps = Enum("ReduceOps", ["SUM", "MAX"]) -MovementOps = Enum("MovementOps", ["RESHAPE", "PERMUTE", "EXPAND", "FLIP", "STRIDED", "PAD", "SHRINK"]) -ProcessingOps = Enum("ProcessingOps", ["CONV"]) -LoadOps = Enum("LoadOps", ["FROMCPU", "CONTIGUOUS"]) +# the Enum class doesn't work with mypy, this is static. sorry it's ugly +class UnaryOps(Enum): NOOP = auto(); NEG = auto(); EXP = auto(); LOG = auto(); NOT = auto() # noqa: E702 +class BinaryOps(Enum): ADD = auto(); SUB = auto(); MUL = auto(); DIV = auto(); POW = auto(); CMPEQ = auto(); MAX = auto() # noqa: E702 +class ReduceOps(Enum): SUM = auto(); MAX = auto() # noqa: E702 +class MovementOps(Enum): RESHAPE = auto(); PERMUTE = auto(); EXPAND = auto(); FLIP = auto(); PAD = auto(); SHRINK = auto() # noqa: E702 +class FusedOps(Enum): MULACC = auto() # noqa: E702 +class LoadOps(Enum): FROMCPU = auto(); CONTIGUOUS = auto() # noqa: E702 -Op = Union[UnaryOps, BinaryOps, ReduceOps, MovementOps, ProcessingOps, LoadOps] -OpType = Union[Type[UnaryOps], Type[BinaryOps], Type[ReduceOps], Type[MovementOps], Type[ProcessingOps], Type[LoadOps]] +Op = Union[UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, FusedOps] +OpType = Union[Type[UnaryOps], Type[BinaryOps], Type[ReduceOps], Type[MovementOps], Type[LoadOps], Type[FusedOps]] class LazyOp(NamedTuple): op: Op @@ -28,64 +28,175 @@ class LazyOp(NamedTuple): # Any == Union[LazyBuffer, DeviceBuffer] def get_buffers(op:LazyOp) -> List[Any]: return functools.reduce(operator.add, [get_buffers(x) if isinstance(x, LazyOp) else [x] for x in op.src], []) def get_lazyops(op:LazyOp) -> List[LazyOp]: return functools.reduce(operator.add, [get_lazyops(x) for x in op.src if isinstance(x, LazyOp)], [op]) +def map_buffers(real_srcs, x:LazyOp) -> LazyOp: + if x in real_srcs: return map_buffers(real_srcs, real_srcs[x]) if isinstance(real_srcs[x], LazyOp) else real_srcs[x] + return LazyOp(x.op, tuple((map_buffers(real_srcs, y) if isinstance(y, LazyOp) else real_srcs[y]) for y in x.src), x.arg) -# a placeholder class to extend by the exec classes -class DeviceBuffer: - shape: Any # should be Tuple[int, ...] but ndarray and torch.tensor have incompatible types - -# extend this if you don't have an exec_ast function -# used in CPUBuffer and TorchBuffer -class GenericExecAST(DeviceBuffer): +_T = TypeVar("_T") +class RawBuffer: + def __init__(self, size): raise NotImplementedError("must be implemented") @classmethod - def exec_ast(cls, ast:LazyOp, preprocess=lambda x: x): - srcs = [cls.exec_ast(x, preprocess) if isinstance(x, LazyOp) else preprocess(x) for x in ast.src] - if ast.op in UnaryOps: - ret = srcs[0].unary_op(ast.op) - elif ast.op in BinaryOps: - assert srcs[0].shape == srcs[1].shape, f"BinaryOps shape mismatch {srcs[0].shape} != {srcs[1].shape}" - ret = srcs[0].binary_op(ast.op, srcs[1]) - elif ast.op in ReduceOps: - assert all(r == n or n == 1 for r,n in zip(srcs[0].shape, ast.arg)), f"ReduceOps can't reduce {srcs[0].shape} -> {ast.arg}" - ret = srcs[0].reduce_op(ast.op, ast.arg) - elif ast.op in MovementOps: - ret = srcs[0].movement_op(ast.op, ast.arg) - elif ast.op in ProcessingOps: - ret = srcs[0].processing_op(ast.op, srcs[1], ast.arg) - else: - raise Exception("unknown op") + def fromCPU(cls:Type[_T], x:np.ndarray) -> _T: raise NotImplementedError("must be implemented") + def toCPU(self:RawBuffer) -> np.ndarray: raise NotImplementedError("must be implemented") + +class RawBufferCopyIn(RawBuffer): + def copyin(self, x:np.ndarray) -> None: raise NotImplementedError("must be implemented") + + @classmethod + def fromCPU(cls, x:np.ndarray): + ret = cls(4*prod(x.shape)) + ret.copyin(x) return ret -class GlobalCounters: - global_ops, global_mem = 0, 0 +class RawBufferCopyInOut(RawBufferCopyIn): + size : int + def copyout(self, x:np.ndarray) -> None: raise NotImplementedError("must be implemented") -class GenericShape(GenericExecAST): - def __init__(self, shape, flops=0): self.shape, self.flops = shape, flops - def unary_op(self, op:UnaryOps): return GenericShape(self.shape, self.flops + prod(self.shape)) - def binary_op(self, op:BinaryOps, y): return GenericShape(self.shape, self.flops + y.flops + prod(self.shape)) - def reduce_op(self, op:ReduceOps, new_shape:Tuple[int, ...]): return GenericShape(new_shape, self.flops + prod(self.shape)) - def movement_op(self, op:MovementOps, arg): return GenericShape(ShapeTracker(self.shape).movement_op(op, arg).shape, self.flops) - # https://docs.nvidia.com/deeplearning/performance/dl-performance-convolutional/index.html - def processing_op(self, op:ProcessingOps, w, C): return GenericShape(C.out_shape, 2 * (C.bs * C.cout * C.oy * C.ox) * (C.cin * C.H * C.W)) -def get_lazyop_info(ast:LazyOp): return GenericShape.exec_ast(ast, lambda x: GenericShape(x.shape)) + def toCPU(self) -> np.ndarray: + x = np.empty((self.size//4), dtype=np.float32) + self.copyout(x) + return x + +# a placeholder class to extend by the exec classes +class DeviceBuffer(RawBuffer): + _buf: Any # underlying buffer + shape: Tuple[int, ...] + @classmethod + def exec_ast(cls, ast:LazyOp, output_buffer=None): raise NotImplementedError("must be implemented") + +# this is a quick "buffer" class for flop tracking and getting the output shape +class GenericShape: + def __init__(self, shape:Tuple[int, ...], flops:int=0): self.shape, self.flops = shape, flops + def consume_flops(self): + self.flops, ret = 0, self.flops + return ret +shape_fxn_for_op : Dict[Op, Callable] = { + **{op:lambda self: GenericShape(self.shape, self.consume_flops() + prod(self.shape)) for op in UnaryOps}, + **{op:lambda self,y: GenericShape(self.shape, self.consume_flops() + y.consume_flops() + prod(self.shape)) for op in BinaryOps}, + **{op:lambda self,new_shape: GenericShape(new_shape, self.consume_flops() + prod(self.shape)) for op in ReduceOps}, + **{op:functools.partial(lambda mop,self,arg: GenericShape(ShapeTracker(self.shape).movement_op(mop, arg).shape, self.consume_flops()), op) for op in MovementOps}} + +# used in CPUBuffer and TorchBuffer +class InterpretedBuffer(DeviceBuffer): # pylint: disable=abstract-method + fxn_for_op : ClassVar = shape_fxn_for_op + # TODO: use generic types here to remove __init__ in specialized classes + def __init__(self, lbuf:Any): self._buf, self.shape = lbuf, tuple(lbuf.shape) + def contiguous(self): return type(self).exec_ast(LazyOp(op=UnaryOps.NOOP, src=(self,))) + def movement_op(self, op:MovementOps, arg=None): return type(self)(self.fxn_for_op[op](self._buf, arg)) if op in self.fxn_for_op else type(self)(getattr(self._buf, op.name.lower())(arg)) + @classmethod + def exec_ast(cls, ast:LazyOp, output_buffer:Optional[InterpretedBuffer]=None, context=None): + if FusedOps.MULACC in cls.fxn_for_op and ast.op == ReduceOps.SUM and isinstance(ast.src[0], LazyOp) and ast.src[0].op == BinaryOps.MUL: + ast = LazyOp(FusedOps.MULACC, ast.src[0].src, ast.arg) + if context is None: context = dict() + if ast in context: return context[ast] + srcs = [cls.exec_ast(x, context=context) if isinstance(x, LazyOp) else x for x in ast.src] + if DEBUG >= 4: print("exec_ast", ast.op, [x.shape for x in srcs], ast.arg) + if ast.op in BinaryOps: assert srcs[0].shape == srcs[1].shape, f"BinaryOps shape mismatch {srcs[0].shape} != {srcs[1].shape}" + if ast.op in ReduceOps: assert all(r == n or n == 1 for r,n in zip(srcs[0].shape, ast.arg)), f"ReduceOps can't reduce {srcs[0].shape} -> {ast.arg}" + if ast.op in MovementOps: ret = srcs[0].movement_op(ast.op, ast.arg) + else: ret = cls(cls.fxn_for_op[ast.op](*([x._buf for x in srcs] + ([ast.arg] if ast.arg else [])))) + context[ast] = ret + if output_buffer is not None: + assert output_buffer.shape == ret.shape + output_buffer._buf = ret._buf + return output_buffer + else: + return ret +def get_lazyop_info(ast:LazyOp): return InterpretedBuffer.exec_ast(map_buffers({x:InterpretedBuffer(GenericShape(x.shape)) for x in get_buffers(ast)}, ast))._buf + +class ASTRunner: + def __init__(self, name, prg, bufs_to_delete:Optional[Set[int]]=None, global_size:Optional[List[int]]=None, local_size:Optional[List[int]]=None, op_estimate=0, mem_estimate=0): + if DEBUG >= 4: print(prg) + self.name, self.prg, self.global_size, self.local_size, self.bufs_to_delete, self.op_estimate, self.mem_estimate = name, prg, global_size, local_size, bufs_to_delete if bufs_to_delete else set(), op_estimate, mem_estimate + def build(self, runtime): + self.clprg = runtime(self.name, self.prg) + return self + def timeit(self, bufs, local_override=None) -> float: + try: return self.clprg(self.global_size, local_override if local_override is not None else self.local_size, *bufs, wait=True) + except Exception: return float('inf') + def optimize_local_size(self, bufs) -> List[int]: + assert self.global_size is not None, "needs a global size to optimize local size" + MAX_WORKGROUP = self.clprg.max_work_group_size() if hasattr(self.clprg, 'max_work_group_size') else 1024 + local_dims = [[x for x in set([sz, 1, 2, 4, 8, 16, 32, 64, 128, 256, MAX_WORKGROUP]) if x<=sz] for sz in self.global_size] + local_sizes = [list(x) for x in itertools.product(*local_dims) if prod(x) <= MAX_WORKGROUP] * 2 # try each valid size twice + return min([(self.timeit(bufs, local_size), local_size) for local_size in random.sample(local_sizes, len(local_sizes))])[1] + def lower(self, bufs) -> List[RawBuffer]: return [x.raw() for i,x in enumerate(bufs) if x is not None and i not in self.bufs_to_delete] + def __call__(self, bufs): + if getenv("OPTLOCAL") and self.global_size is not None and self.local_size is None: self.local_size = self.optimize_local_size(bufs) + if et := self.clprg(self.global_size, self.local_size, *bufs, wait=DEBUG>=2): GlobalCounters.time_sum_s += et + if DEBUG >= 1: + print(f"**** {GlobalCounters.kernel_count:4d} {self.name:20s} args {len(bufs):5d} kernels {str(self.global_size):18s} {str(self.local_size):12s} OPs {self.op_estimate/1e6:7.1f}M/{GlobalCounters.global_ops/1e9:7.2f}G mem {GlobalCounters.mem_used/1e9:5.2f} GB " + + (str() if et is None else f"tm {et*1e6:9.2f}us/{GlobalCounters.time_sum_s*1e3:9.2f}ms ({self.op_estimate/(et*1e9):8.2f} GFLOPS)")) + GlobalCounters.log_kernel(self.op_estimate, self.mem_estimate) + return et # assumes you are using ShapeTracker -# used in GPUBuffer, OpenCLBuffer, and LLVMBuffer -class ExplicitExecAST(DeviceBuffer): - def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]], hostbuf=None): +# used in GPUBuffer and LLVMBuffer +class CompiledBuffer(DeviceBuffer): # pylint: disable=abstract-method + def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]], hostbuf:Optional[CompiledBuffer]=None, backing:Optional[np.ndarray]=None, force_create=False): self.st = shape if isinstance(shape, ShapeTracker) else ShapeTracker(tuple(shape)) self.shape = self.st.shape + self._base_shape : Tuple[int, ...] = hostbuf._base_shape if hostbuf is not None else self.shape + self._buf = hostbuf._buf if hostbuf is not None else None + self._backing : Optional[np.ndarray] = hostbuf._backing if hostbuf is not None else backing + if (self._backing is not None and self._backing.shape != (1,)) or force_create: self.raw() + + # TODO: not GPUBuffer, get name of class + def __repr__(self): return f"GPUBuffer(shape={self.st}, hostbuf=GPUBuffer(shape={self._base_shape}" + (f", backing=np.array({self._backing}, dtype=np.float32)))" if self._backing else ", force_create=True))") + + raw_buffer_type : Type[RawBuffer] + @classmethod + def create_raw_buffer(cls, shape, backing) -> RawBuffer: + assert backing is None or prod(shape) == prod(backing.shape), "backing has the wrong shape" + assert backing is None or GlobalCounters.cache is None, f"can't copy in {backing.shape} while caching" + return cls.raw_buffer_type(4*prod(shape)) if backing is None else cls.raw_buffer_type.fromCPU(backing) + def raw(self) -> RawBuffer: + if self._buf is None: + self._buf = self.create_raw_buffer(self._base_shape, self._backing) + self._backing = None + return self._buf @classmethod - def exec_ast(cls, ast:LazyOp): raise NotImplementedError("must be implemented") + def fromCPU(cls, x:np.ndarray) -> CompiledBuffer: return cls(x.shape, backing=x.view(np.ndarray).astype(np.float32).ravel()) + def toCPU(self) -> np.ndarray: + assert GlobalCounters.cache is None, f"can't copy out {self} while caching" + return self.contiguous().raw().toCPU().reshape(self.shape) - # universal - def unary_op(self, op:UnaryOps): return type(self)(self.shape).exec_ast(LazyOp(op=op, src=(self,))) - def binary_op(self, op:BinaryOps, y): return type(self)(self.shape).exec_ast(LazyOp(op=op, src=(self, y))) - def reduce_op(self, op:ReduceOps, new_shape:Tuple[int, ...]): return type(self)(new_shape).exec_ast(LazyOp(op=op, src=(self,), arg=new_shape)) + codegen_type : Any + runtime_type : Type + method_cache : Dict[str, ASTRunner] = {} + @classmethod + def exec_ast(cls, ast:LazyOp, output_buffer:Optional[CompiledBuffer]=None): + k = cls.codegen_type(ast, output_buffer) + if getenv("ENABLE_METHOD_CACHE"): # TODO: this breaks the ops test! + if k.key not in cls.method_cache: cls.method_cache[k.key] = k.codegen().build(cls.runtime_type) + elif DEBUG >= 4: print(f"method cache hit : {k.key}") + prg = cls.method_cache[k.key] + else: + prg = k.codegen().build(cls.runtime_type) + if getenv("PRINT_AST", "") == prg.name: + k.print() + print(prg.prg) + rawbufs = prg.lower(k.bufs) + if GlobalCounters.cache is not None: GlobalCounters.cache.append((prg, rawbufs)) + prg(rawbufs) + return k.ret # universal for shape tracked + def contiguous(self): return self if self.st.contiguous and prod(self._base_shape) == prod(self.shape) else type(self).exec_ast(LazyOp(op=UnaryOps.NOOP, src=(self,))) def movement_op(self, op:MovementOps, arg): return type(self)(ShapeTracker(self.st).movement_op(op, arg), self) - # TODO: creating a new object is making a copy, breaking the thneed compiler - def contiguous(self): return self if self.st.contiguous else self.unary_op(UnaryOps.NOOP) - #def contiguous(self): return type(self)(self.shape, hostbuf=self) if self.st.contiguous else self.unary_op(UnaryOps.NOOP) \ No newline at end of file +class GlobalCounters: + global_ops : ClassVar[int] = 0 + global_mem : ClassVar[int] = 0 + time_sum_s : ClassVar[float] = 0.0 + kernel_count : ClassVar[int] = 0 + mem_used : ClassVar[int] = 0 # NOTE: this is not reset + cache : ClassVar[Optional[List[Tuple[Callable, Any]]]] = None + @staticmethod + def reset(): GlobalCounters.global_ops, GlobalCounters.global_mem, GlobalCounters.time_sum_s, GlobalCounters.kernel_count, GlobalCounters.cache = 0,0,0.0,0,None + @staticmethod + def log_kernel(op_estimate:int, mem_estimate:int): + GlobalCounters.kernel_count += 1 + GlobalCounters.global_ops += op_estimate + GlobalCounters.global_mem += mem_estimate \ No newline at end of file diff --git a/tinygrad_repo/tinygrad/runtime/ops_gpu.py b/tinygrad_repo/tinygrad/runtime/ops_gpu.py new file mode 100644 index 000000000..6c84e4f19 --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_gpu.py @@ -0,0 +1,103 @@ +from __future__ import annotations +import platform, functools +import numpy as np +import pyopencl as cl # type: ignore +from typing import Dict, Optional, List, ClassVar, Final +from collections import defaultdict +from tinygrad.helpers import IMAGE, DEBUG, getenv +from tinygrad.ops import CompiledBuffer, GlobalCounters, RawBufferCopyInOut, RawBuffer +from tinygrad.codegen.gpu import GPUCodegen, GPULanguage + +OSX = platform.system() == "Darwin" +OSX_TIMING_RATIO = (125/3) if OSX else 1.0 # see test/external_osx_profiling.py to determine this ratio. it's in like GPU clocks or something +CLCACHE = getenv("CLCACHE", 1) +FLOAT16 = getenv("FLOAT16", 0) + +class _CL: + @functools.cached_property + def cl_ctx(self) -> cl.Context: + devices : List[cl.Device] = sum([x.get_devices(device_type=cl.device_type.GPU) for x in cl.get_platforms()], []) + if len(devices) == 0: devices = sum([x.get_devices(device_type=cl.device_type.CPU) for x in cl.get_platforms()], []) # settle for CPU + if len(devices) > 1 or DEBUG >= 1: print(f"using {devices[getenv('CL_DEVICE', 0)]}") + return cl.Context(devices=[devices[getenv("CL_DEVICE", 0)]]) + + @functools.cached_property + def cl_queue(self) -> cl.CommandQueue: + return cl.CommandQueue(CL.cl_ctx, properties=cl.command_queue_properties.PROFILING_ENABLE) # this is an in-order command queue +CL = _CL() + +class CLBuffer(RawBufferCopyInOut): + # TODO: this can be in RawBuffer generically + BUFFER_CACHE : ClassVar[Dict[int, List[cl.Buffer]]] = defaultdict(list) + + def __init__(self, size): + self.size = size + if len(CLBuffer.BUFFER_CACHE[size]) > 0: + self._cl = CLBuffer.BUFFER_CACHE[size].pop() + else: + # TODO: on GPU OOM, clear the cache + self._cl = cl.Buffer(CL.cl_ctx, cl.mem_flags.READ_WRITE, size) + GlobalCounters.mem_used += self._cl.size + + def __del__(self): + if CLCACHE: CLBuffer.BUFFER_CACHE[self._cl.size].append(self._cl) + else: GlobalCounters.mem_used -= self._cl.size + + def copyin(self, x:np.ndarray): cl.enqueue_copy(CL.cl_queue, self._cl, x, is_blocking=False) + def copyout(self, x:np.ndarray): cl.enqueue_copy(CL.cl_queue, x, self._cl, is_blocking=True) + +class CLImage(RawBuffer): # pylint: disable=abstract-method + fmt : Final = cl.ImageFormat(cl.channel_order.RGBA, cl.channel_type.HALF_FLOAT if FLOAT16 else cl.channel_type.FLOAT) + IMAGE : Final = True + + def __init__(self, shape): + self._cl = cl.Image(CL.cl_ctx, cl.mem_flags.READ_WRITE, CLImage.fmt, shape=(shape[1], shape[0])) + GlobalCounters.mem_used += self._cl.row_pitch * self._cl.height + + def __del__(self): GlobalCounters.mem_used -= self._cl.row_pitch * self._cl.height + +@functools.lru_cache(maxsize=None) +class CLProgram: + def __init__(self, name:str, prg:str, binary=False, argdtypes=None): + self.name, self.argdtypes, self.clprogram = name, argdtypes, cl.Program(CL.cl_ctx, CL.cl_ctx.devices, [prg]) if binary else cl.Program(CL.cl_ctx, prg) # type: ignore + try: + self._clprg = self.clprogram.build() + except cl.RuntimeError as e: + if DEBUG >= 3: print("FAILED TO BUILD", prg) + raise e + self.clprg = self._clprg.__getattr__(name) + if DEBUG >= 5 and not OSX: + binary = self.clprogram.get_info(cl.program_info.BINARIES)[0] + if 'Adreno' in CL.cl_ctx.devices[0].name: + from disassemblers.adreno import disasm + disasm(binary) + else: + # print the PTX for NVIDIA. TODO: probably broken for everything else + print(binary.decode('utf-8')) + if self.argdtypes is not None: self.clprg.set_scalar_arg_dtypes(self.argdtypes) + + @staticmethod + def max_work_group_size(): return CL.cl_ctx.devices[0].max_work_group_size + + def __call__(self, global_size, local_size, *bufs, wait=False) -> Optional[float]: + e = self.clprg(CL.cl_queue, global_size, local_size, *[x._cl if isinstance(x, (CLBuffer, CLImage)) else x for x in bufs]) + if wait: + CL.cl_queue.finish() + return ((e.profile.end - e.profile.start) * OSX_TIMING_RATIO) * 1e-9 + return None + +class CLCodegen(GPUCodegen): + lang = GPULanguage( + kernel_prefix = "__kernel", buffer_prefix = "__global ", smem_prefix = "__local ", + barrier = "barrier(CLK_LOCAL_MEM_FENCE);", float4 = "(float4)", + gid = [f'get_global_id({i})' for i in range(3)], lid = [f'get_local_id({i})' for i in range(3)]) + +class GPUBuffer(CompiledBuffer): + raw_buffer_type = CLBuffer + # override this method for image + @classmethod + def create_raw_buffer(cls, shape, backing) -> RawBuffer: + if len(shape) == 3 and shape[2] == 4 and IMAGE >= 2 and backing is None: return CLImage(shape) + else: return super().create_raw_buffer(shape, backing) + codegen_type = CLCodegen + runtime_type = CLProgram diff --git a/tinygrad_repo/tinygrad/shape/__init__.py b/tinygrad_repo/tinygrad/shape/__init__.py index a2da527be..53468d15c 100644 --- a/tinygrad_repo/tinygrad/shape/__init__.py +++ b/tinygrad_repo/tinygrad/shape/__init__.py @@ -1,13 +1,9 @@ # ShapeTracker allows movement operations to a buffer that don't require a copy to be made. from __future__ import annotations -import os import functools -from typing import Tuple, Union, List, Optional -from tinygrad.helpers import prod -from tinygrad.shape.symbolic import Variable - -# TODO: fix DEBUG import -DEBUG = int(os.getenv("DEBUG", "0")) +from typing import Tuple, Union, List, Optional, cast +from tinygrad.helpers import prod, DEBUG +from tinygrad.shape.symbolic import Variable, MulNode, NumNode, Node @functools.lru_cache(maxsize=None) def to_shape_strides(shape:Tuple[int, ...], strides:Tuple[int, ...]) -> List[Tuple[int, int]]: @@ -21,197 +17,199 @@ def to_shape_strides(shape:Tuple[int, ...], strides:Tuple[int, ...]) -> List[Tup return ret class View: - def __init__(self, shape, strides, offset:int=0): - self.shape, self.strides, self.offset = tuple(shape), tuple(strides), offset + def __init__(self, shape:Tuple[int, ...], strides:Tuple[int, ...], offset:int=0): + self.shape, self.strides, self.offset = shape, tuple(stride if shp != 1 else 0 for stride,shp in zip(strides, shape)), offset self.shape_strides = to_shape_strides(self.shape, self.strides) + self.contiguous : bool = self.offset == 0 and all(s1 == s2 or s == 1 for s,s1,s2 in zip(self.shape, self.strides, strides_for_shape(self.shape))) def __repr__(self): return f"View({self.shape}, {self.strides}, {self.offset})" - @functools.cached_property - def contiguous(self): - return self.offset == 0 and all(s1 == s2 or s == 1 for s,s1,s2 in zip(self.shape, self.strides, strides_for_shape(self.shape))) - - def expr_node(self, idx): - ret = [Variable.num(self.offset)] + def expr_node(self, idx=None, offset:Union[Node, int]=0): + if idx is None: idx = Variable('idx', 0, prod(self.shape)) + ret = [Variable.num(self.offset)+offset] acc = 1 for d,s in self.shape_strides[::-1]: - if d != 1 and s != 0: - ret.append(((idx//acc)%d)*s) + ret.append(((idx//acc)%d)*s) acc *= d return Variable.sum(ret) - @functools.cached_property - def expr(self): - return 'idx=' + str(self.expr_node(Variable('idx', 0, prod([x[0] for x in self.shape_strides])-1))) - # generate an expression if you have a variable or expression for each index - def expr_idxs(self, idxs): - return Variable.sum([Variable.num(self.offset)] + [Variable(idxs[i], 0, sh-1)*st for i,(sh,st) in enumerate(zip(self.shape, self.strides)) if sh != 1 and st != 0]) + def expr_idxs(self, idxs, offset:Union[Node, int]=0): + return Variable.sum([Variable.num(self.offset)+offset] + [Variable(idx, 0, sh-1)*st for idx,sh,st in zip(idxs, self.shape, self.strides) if sh != 1 and st != 0]) class ZeroView: - def __init__(self, old_shape, arg): - self.old_shape, self.arg, self.shape = old_shape, arg, [] - - def expr_node(self, valid, idx): - expr, acc = [valid] if valid is not None else [], 1 - for s,(x,y) in list(zip(self.old_shape, self.arg))[::-1]: - self.shape = [y-x] + self.shape - base = idx//acc - base = (base % self.shape[0]) + x - expr += ([base >= 0] if x < 0 else []) + ([base < s] if y > s else []) - acc *= self.shape[0] - return Variable.ands(expr) - - @functools.cached_property - def expr(self): - max_idx = prod([y-x for x,y in self.arg]) - return 'valid=' + str(self.expr_node(Variable('valid', 0, 1), Variable('idx', 0, max_idx-1))) + def __init__(self, old_shape:Tuple[int, ...], arg): + self.old_shape, self.arg = old_shape, arg + self.shape : Tuple[int, ...] = tuple([y-x for x,y in self.arg]) + # fake properties + self.strides, self.contiguous, self.offset = strides_for_shape(self.shape), False, 0 def __repr__(self): return f"ZeroView({self.old_shape}, {self.arg})" + def expr_node(self, idx=None, valid=None): + if idx is None: idx = Variable('idx', 0, prod([y-x for x,y in self.arg])) + expr, acc = [valid] if valid is not None else [], 1 + for s,ns,(x,y) in list(zip(self.old_shape, self.shape, self.arg))[::-1]: + base = ((idx//acc) % ns) + x + expr += ([base >= 0] if x < 0 else []) + ([base < s] if y > s else []) + acc *= ns + return Variable.ands(expr) + + def expr_idxs(self, idxs, offset=0): raise NotImplementedError("ZeroView doesn't support expr_idxs") + ViewTypes = Union[View, ZeroView] @functools.lru_cache(maxsize=None) def strides_for_shape(shape:Tuple[int, ...]) -> Tuple[int, ...]: strides = [1] - for d in shape[::-1][:-1]: - strides = [d*strides[0]] + strides - return tuple(strides) + for d in shape[::-1][:-1]: strides = [d*strides[0]] + strides + return tuple(st if s != 1 else 0 for st, s in zip(strides, shape)) @functools.lru_cache(maxsize=None) def view_from_shape(shape:Tuple[int, ...]) -> View: assert all(isinstance(x, int) for x in shape) and len(shape) != 0 return View(tuple(shape), strides_for_shape(shape)) +def merge_views(vm2:View, vm1:View) -> Optional[View]: + new_strides, new_offset = [], vm2.expr_node(Variable.num(vm1.offset)) + assert isinstance(new_offset, NumNode), "new_offset wasn't a number?!?" + for s,st in zip(vm1.shape, vm1.strides): + this_dim = View(vm2.shape, vm2.strides).expr_node(Variable('idx', 0, s-1)*st) + if s == 1: + new_strides.append(0) # all shape 1 can have stride 0 + elif isinstance(this_dim, NumNode) and this_dim.b == 0: + new_strides.append(0) + elif isinstance(this_dim, Variable): + new_strides.append(1) + elif isinstance(this_dim, MulNode) and isinstance(this_dim.a, Variable): + new_strides.append(this_dim.b) + else: + if DEBUG >= 4: print("can't simplify", s, this_dim.render()) + break + return View(vm1.shape, tuple(new_strides), new_offset.b) if len(new_strides) == len(vm1.strides) else None + class ShapeTracker: def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]], views:Optional[List[ViewTypes]]=None): self.views : List[ViewTypes] = views if views is not None else (shape.views[:] if isinstance(shape, ShapeTracker) else [view_from_shape(shape)]) def __repr__(self): return f"ShapeTracker(shape={self.shape}, views={self.views})" + def copy(self) -> ShapeTracker: return ShapeTracker(self.shape, self.views[:]) @property - def contiguous(self): return len(self.views) == 1 and self.views[-1].contiguous + def contiguous(self) -> bool: return len(self.views) == 1 and self.views[-1].contiguous @property - def shape(self): return self.views[-1].shape + def shape(self) -> Tuple[int, ...]: return self.views[-1].shape @property - def strides(self): return self.views[-1].strides + def strides(self) -> Tuple[int, ...]: return self.views[-1].strides @property - def offset(self): return self.views[-1].offset + def offset(self) -> int: return self.views[-1].offset - def expr_node(self): - idx = Variable('idx', 0, prod(self.shape)-1) - valid = None #Variable.num(1) - for v in self.views[::-1]: - if isinstance(v, ZeroView): valid = v.expr_node(valid, idx) + # this is the real size + def size(self): return prod([s for s,st in zip(self.shape, self.strides) if st != 0]) + + def _expr_idx(self, idx): + valid = Variable.num(1) + for v in self.views[0:-1][::-1]: + if isinstance(v, ZeroView): valid = v.expr_node(idx, valid) else: idx = v.expr_node(idx) return idx, valid - - def expr(self): - idx, valid = self.expr_node() - if valid is not None and str(valid) != "valid": return f"valid={valid};idx={idx}" - else: return f"idx={idx}" - #def expr(self): return ';'.join([v.expr for v in self.views[::-1] if v.expr != 'idx=idx' and v.expr != 'valid=valid']) - def movement_op(self, op, arg): return getattr(self, str(op).split(".")[1].lower())(*arg) - def needs_valid(self): return any(isinstance(v, ZeroView) for v in self.views) + def simplify(self): + if len(self.views) >= 2 and isinstance(self.views[-2], View) and isinstance(self.views[-1], View): + new_view = merge_views(self.views[-2], self.views[-1]) + if new_view: + if DEBUG >= 4: print(f"st simplify : {self.views[-2]} + {self.views[-1]} = {new_view}") + self.views = self.views[:-2] + [new_view] + self.simplify() - # TODO: do we really need this for conv? - # if we replace, confirm the ops taken fold into one view - def strided(self, *arg): - view = View([x[0] for x in arg], [x[1] for x in arg]) - # TODO: this does not always require a new view if non contiguous - if self.views[-1].contiguous: - self.views[-1] = view - else: - self.views.append(view) - return self + # TODO: arg order is reversed here + def expr_idxs(self, offset=0, idxs=None): + if idxs is None: idxs = [f"idx{i}" for i in range(len(self.shape))] + return self._expr_idx(self.views[-1].expr_idxs(idxs, offset)) - def reshape(self, *new_shape): + def expr_node(self, idx='idx', offset=0): + return self._expr_idx(self.views[-1].expr_node(Variable(idx, 0, prod(self.shape)-1), offset)) + + def movement_op(self, op, arg:Union[Tuple[int, ...], Tuple[Tuple[int, int], ...]]) -> ShapeTracker: + return getattr(self, str(op).split(".")[1].lower())(arg) + def needs_valid(self) -> bool: + return any(isinstance(v, ZeroView) for v in self.views) + + def reshape(self, new_shape : Tuple[int, ...]) -> ShapeTracker: + assert isinstance(new_shape, tuple) + if self.shape == new_shape: return self assert all(isinstance(x, int) and x != 0 for x in new_shape), f"shape must be ints and can't contain 0 {new_shape}" assert prod(self.shape) == prod(new_shape), f"can't reshape {self.shape} -> {new_shape}" - # check if this is adding or removing 1s (only) - if tuple([x for x in self.shape if x != 1]) == tuple([x for x in new_shape if x != 1]): - old_strides = [y for x,y in zip(self.shape, self.strides) if x != 1] - new_strides = [0 if x == 1 else old_strides.pop(0) for x in new_shape] - self.views[-1] = View(new_shape, new_strides, self.offset) - return self - - # check if the new dimensions factorize from the old ones - # NOTE: if you don't make a copy here, the list is popped in the lrucache - min_shape_strides = to_shape_strides(self.shape, self.strides)[:] - curr_dim, curr_stride = min_shape_strides.pop(0) - new_strides = [] - for s in new_shape: - if curr_dim%s == 0: - curr_dim //= s - new_strides.append(curr_stride * curr_dim) - if curr_dim == 1: - if len(min_shape_strides) == 0: - # there might still be 1s in the shape - while len(new_strides) != len(new_shape): - assert new_shape[len(new_strides)] == 1 - new_strides.append(1) - break - curr_dim, curr_stride = min_shape_strides.pop(0) - else: - break # didn't factorize - - if len(new_shape) == len(new_strides): - self.views[-1] = View(new_shape, new_strides, self.offset) - return self - view = View(new_shape, strides_for_shape(new_shape)) - if self.contiguous: - self.views[-1] = view # NOTE: if it's contiguous it can't have an offset + if self.contiguous: self.views[-1] = view # NOTE: if it's contiguous it can't have an offset else: - if DEBUG >= 2: - print(f"WARNING: reshape from {self.shape} w strides {self.strides} -> {new_shape} is creating another view") - self.views.append(view) + # NOTE: the last view in self.views is never a ZeroView + if (merged_view := merge_views(cast(View, self.views[-1]), view)) is not None: self.views[-1] = merged_view + else: self.views.append(view) return self - def permute(self, *axis): + def permute(self, axis : Tuple[int, ...]) -> ShapeTracker: + assert isinstance(axis, tuple) assert all(isinstance(x, int) and x >= 0 and x < len(self.shape) for x in axis), f"invalid permute {axis} for {self.shape}" assert len(set(axis)) == len(axis) and len(axis) == len(self.shape), f"can't permute {self.shape} with {axis}" - self.views[-1] = View([self.shape[a] for a in axis], [self.strides[a] for a in axis], self.offset) + self.views[-1] = View(tuple(self.shape[a] for a in axis), tuple(self.strides[a] for a in axis), self.offset) return self # TODO: this is a special case of slice with strides, remove it # though it's nice that it can't change size - def flip(self, *axis): return self.stride(*[-1 if i in axis else 1 for i in range(len((self.shape)))]) + def flip(self, axis : Tuple[int, ...]) -> ShapeTracker: + return self.stride(tuple(-1 if i in axis else 1 for i in range(len((self.shape))))) # *** under this line are not invertible *** # TODO: take this functionality out of slice - def pad(self, *arg): + def pad(self, arg : Tuple[Tuple[int, int], ...]) -> ShapeTracker: + assert isinstance(arg, tuple) assert all((b>=0 and e>=0) for b,e in arg) and len(arg) == len(self.shape) - return self.shrink(*[(-b,s+e) for s,(b,e) in zip(self.shape, arg)]) + return self.shrink(tuple((-b,s+e) for s,(b,e) in zip(self.shape, arg))) # TODO: take the pad functionality out of shrink - def shrink(self, *arg): + def shrink(self, arg : Tuple[Tuple[int, int], ...]) -> ShapeTracker: + assert isinstance(arg, tuple) assert len(arg) == len(self.shape) offset = sum([self.strides[i]*x for i,(x,_) in enumerate(arg)]) zeroview = ZeroView(self.shape, arg) - self.views[-1] = View([y-x for x,y in arg], self.strides, self.offset+offset) - if zeroview.expr != "valid=valid": + self.views[-1] = View(tuple(y-x for x,y in arg), self.strides, self.offset+offset) + if zeroview.expr_node().min == 0: # may be invalid # if we add a ZeroView, we add another (stock) view also for modding self.views += [zeroview, View(self.shape, strides_for_shape(self.shape))] return self - def expand(self, *new_shape): - assert all(isinstance(x, int) for x in new_shape) + def expand(self, new_shape : Tuple[int, ...]) -> ShapeTracker: + assert isinstance(new_shape, tuple) + assert all(isinstance(x, int) for x in new_shape), f"non ints for expand in {new_shape}" assert all(x == y or x == 1 for x,y in zip(self.shape, new_shape)), f"can't expand {self.shape} into {new_shape}" - strides = [s if x == y else 0 for s,(x,y) in zip(self.strides, zip(self.shape, new_shape))] + strides : Tuple[int, ...] = tuple(s if x == y else 0 for s,(x,y) in zip(self.strides, zip(self.shape, new_shape))) self.views[-1] = View(new_shape, strides, self.offset) return self # TODO: combine with slice? this doesn't require a ZeroView, though slice shouldn't always either - def stride(self, *mul): + def stride(self, mul : Tuple[int, ...]) -> ShapeTracker: + assert isinstance(mul, tuple) assert all(isinstance(x, int) for x in mul) - strides = [z*m for z,m in zip(self.strides, mul)] - new_shape = [(s+(abs(m)-1))//abs(m) for s,m in zip(self.shape, mul)] + strides = tuple(z*m for z,m in zip(self.strides, mul)) + new_shape = tuple((s+(abs(m)-1))//abs(m) for s,m in zip(self.shape, mul)) offset = sum([(s-1)*z for s,z,m in zip(self.shape, self.strides, mul) if m < 0]) self.views[-1] = View(new_shape, strides, self.offset + offset) return self + +# returns the axes to create new_shape if new_shape can be created by combining axis from old_shape +def get_contraction(old_shape:Tuple[int, ...], new_shape:Tuple[int, ...]): + if len(new_shape) > len(old_shape): return None + new_shape_i : int = 0 + shape_idx_groups : List[List[int]] = [[] for _ in range(len(new_shape))] + for old_shape_i, t in enumerate(old_shape): + if new_shape[new_shape_i] % t != 0 or prod([old_shape[x] for x in shape_idx_groups[new_shape_i]]) * t > new_shape[new_shape_i]: + return None + shape_idx_groups[new_shape_i].append(old_shape_i) + if prod([old_shape[x] for x in shape_idx_groups[new_shape_i]]) == new_shape[new_shape_i] and new_shape_i < len(new_shape) - 1: + new_shape_i += 1 + return shape_idx_groups diff --git a/tinygrad_repo/tinygrad/shape/symbolic.py b/tinygrad_repo/tinygrad/shape/symbolic.py index 7f583c397..be06ae5f6 100644 --- a/tinygrad_repo/tinygrad/shape/symbolic.py +++ b/tinygrad_repo/tinygrad/shape/symbolic.py @@ -1,38 +1,64 @@ from __future__ import annotations -import math -from typing import List -from tinygrad.helpers import partition, modn, all_same +import math, itertools, functools +from typing import List, Dict, Callable, Type, Union +from tinygrad.helpers import partition, all_same + +# NOTE: Python has different behavior for negative mod and floor div than c +# symbolic matches the Python behavior, but the code output is agnostic, and will never have negative numbers in div or mod + +def create_node(typ:Type[Node], *args): + ret = typ(*args) + assert ret.min <= ret.max, f"min greater than max! {ret.min} {ret.max} when creating {typ} {args}" + if ret.min == ret.max: return NumNode(ret.min) + return ret class Node: - b, min, max = 0, -math.inf, math.inf # make mypy happy - def __str__(self): - if self.min == self.max: return str(self.min) # this is universal - return self.expr - @property - def cl(self): - return str(self).replace('//', '/') - def __add__(self, b:int): return Variable.sum([self, Variable.num(b)]) + b: int + min: int + max: int + def render(self, ops=None, ctx=None) -> str: + if ops is None: ops = render_python + assert isinstance(self, NumNode) or self.min != self.max + return ops[type(self)](self, ops, ctx) + @functools.cached_property + def key(self) -> str: return self.render(ctx="DEBUG") + def __repr__(self): return "<"+self.key+">" + def __eq__(self, other:object) -> bool: + if not isinstance(other, Node): return NotImplemented + return self.key == other.key + def __neg__(self): return self*-1 + def __add__(self, b:Union[Node, int]): return Variable.sum([self, b if isinstance(b, Node) else Variable.num(b)]) + def __sub__(self, b:Union[Node, int]): return self+-b + def __ge__(self, b:int): return create_node(GeNode, self, b) + def __lt__(self, b:int): return create_node(LtNode, self, b) def __mul__(self, b:int): if b == 0: return NumNode(0) elif b == 1: return self - return MulNode(self, b) + if isinstance(self, MulNode): return self.a*(self.b*b) # two muls is one mul + if isinstance(self, SumNode): return Variable.sum([x*b for x in self.nodes]) # distribute mul into sum + return create_node(MulNode, self, b) + + # *** complex ops *** + def __floordiv__(self, b:int): assert b != 0 + if b < 0: return (self//-b)*-1 if b == 1: return self - if isinstance(self, MulNode) and self.b%b == 0: return self.a*(self.b//b) - if isinstance(self, MulNode) and b%self.b == 0: return self.a//(b//self.b) + if isinstance(self, DivNode): return self.a//(self.b*b) # two divs is one div + if isinstance(self, MulNode) and self.b % b == 0: return self.a*(self.b//b) + if isinstance(self, MulNode) and b % self.b == 0: return self.a//(b//self.b) if isinstance(self, SumNode): - factors, tmp_nofactor = partition(self.nodes, lambda x: (isinstance(x, MulNode) or isinstance(x, NumNode)) and x.b%b == 0) + factors, tmp_nofactor = partition(self.nodes, lambda x: (isinstance(x, (MulNode, NumNode))) and x.b%b == 0) nofactor = [] # ugh, i doubt this is universally right for x in tmp_nofactor: if isinstance(x, NumNode): - if modn(x.b, b) != x.b: - factors.append(Variable.num(x.b - modn(x.b, b))) # python does floor division - nofactor.append(Variable.num(modn(x.b, b))) + if (x.b%b) != x.b: + factors.append(Variable.num(x.b - (x.b%b))) # python does floor division + nofactor.append(Variable.num(x.b%b)) else: nofactor.append(x) - gcd = [math.gcd(x.b, b) if isinstance(x, MulNode) or isinstance(x, NumNode) else None for x in nofactor] + gcd = [math.gcd(x.b, b) if isinstance(x, (MulNode, NumNode)) else None for x in nofactor] if len(factors) > 0: # these don't have to be the same, just having a common factor if len(gcd) > 0 and all_same(gcd) and gcd[0] is not None and gcd[0] > 1: @@ -40,111 +66,126 @@ class Node: else: nofactor_term = Variable.sum(nofactor)//b return Variable.sum([(x.a * (x.b//b)) if isinstance(x, MulNode) else Variable.num(x.b//b) for x in factors] + [nofactor_term]) - return DivNode(self, b) + else: + muls = [x.b for x in nofactor if isinstance(x, MulNode)] + for m in muls: + if m > 1 and b%m == 0: + return (self//m)//(b//m) + if self.min < 0: + offset = self.min//b + return (self+offset*b)//b - offset + return create_node(DivNode, self, b) + def __mod__(self, b:int): + assert b > 0 if b == 1: return NumNode(0) if isinstance(self, SumNode): - a = Variable.sum([(x if not isinstance(x, NumNode) else Variable.num(modn(x.b, b))) for x in self.nodes if not (isinstance(x, MulNode) or isinstance(x, NumNode)) or (x.b%b != 0)]) + new_nodes = [] + for x in self.nodes: + if isinstance(x, NumNode): new_nodes.append(Variable.num(x.b%b)) + elif isinstance(x, MulNode): new_nodes.append(x.a * (x.b%b)) + else: new_nodes.append(x) + a = Variable.sum(new_nodes) + elif isinstance(self, MulNode): + a = self.a * (self.b%b) else: a = self if a.min >= 0 and a.max < b: return a - return ModNode(a, b) - def __ge__(self, b:int): - if self.max < b: return Variable.num(0) - if self.min >= b: return Variable.num(1) - return GeNode(self, b) - def __lt__(self, b:int): - if self.max < b: return Variable.num(1) - if self.min >= b: return Variable.num(0) - return LtNode(self, b) - -# constructors -class Variable(Node): - def __init__(self, expr:str, nmin:int, nmax:int): - self.expr, self.min, self.max = expr, nmin, nmax + if a.min < 0: return (a + ((a.min//b)*b)) % b + return create_node(ModNode, a, b) @staticmethod - def num(num:int) -> Node: - return NumNode(num) + def num(num:int) -> Node: return NumNode(num) @staticmethod def sum(nodes:List[Node]) -> Node: + # expand any sums inside one sum if any([isinstance(x, SumNode) for x in nodes]): nodes, sum_nodes = partition(nodes, lambda x: not isinstance(x, SumNode)) for x in sum_nodes: nodes += x.nodes return Variable.sum(nodes) + + # combine any numbers inside a sum + nodes, num_nodes = partition(nodes, lambda x: not isinstance(x, NumNode)) + nodes.append(NumNode(sum([x.b for x in num_nodes]))) + + # combine any MulNodes that factorize (big hack sticking the MulNode(x, 1) on things) + nodes, mul_nodes = partition(nodes, lambda x: not isinstance(x, MulNode)) + mul_nodes += [MulNode(x, 1) for x in nodes] + mul_nodes = sorted(mul_nodes, key=lambda x: x.a.render()) # group by equality (ugh, uses render!) + new_nodes = [k * sum(x.b for x in g) for k, g in itertools.groupby(mul_nodes, key=lambda x: x.a)] + nodes = [x if not isinstance(x, MulNode) or x.b != 1 else x.a for x in new_nodes] + + # filter 0s nodes = [x for x in nodes if x.min != 0 or x.max != 0] - if len(nodes) == 0: return NumNode(0) - elif len(nodes) == 1: return nodes[0] - return SumNode(nodes) + return create_node(SumNode, nodes) if len(nodes) > 1 else (nodes[0] if len(nodes) == 1 else NumNode(0)) @staticmethod def ands(nodes:List[Node]) -> Node: if any((x.min == 0 and x.max == 0) for x in nodes): return NumNode(0) + + # filter 1s nodes = [x for x in nodes if x.min != x.max] - if len(nodes) == 0: return NumNode(1) - elif len(nodes) == 1: return nodes[0] - return AndNode(nodes) + return create_node(AndNode, nodes) if len(nodes) > 1 else (nodes[0] if len(nodes) == 1 else NumNode(1)) + +# 4 basic node types + +class Variable(Node): + def __new__(cls, expr:str, nmin:int, nmax:int): + assert nmin >= 0 and nmin <= nmax + if nmin == nmax: return NumNode(nmin) + return super().__new__(cls) + + def __init__(self, expr:str, nmin:int, nmax:int): + self.expr, self.min, self.max = expr, nmin, nmax class NumNode(Node): def __init__(self, num:int): self.b, self.min, self.max = num, num, num -class MulNode(Node): +class OpNode(Node): def __init__(self, a:Node, b:int): self.a, self.b = a, b - self.min, self.max = a.min*b, a.max*b - @property - def expr(self): - return f"({self.a}*{self.b})" + self.min, self.max = self.minmax(a,b) + minmax = staticmethod(lambda a,b: (1//0, 1//0)) -class DivNode(Node): - def __init__(self, a:Node, b:int): - self.a, self.b = a, b - self.min, self.max = int(a.min/b), int(a.max/b) - @property - def expr(self): - return f"({self.a}//{self.b})" +class RedNode(Node): + def __init__(self, nodes:List[Node]): + self.nodes = nodes + self.min, self.max = self.minmax(nodes) + minmax = staticmethod(lambda nodes: (1//0, 1//0)) -class ModNode(Node): - def __init__(self, a:Node, b:int): - self.a, self.b = a, b - self.min, self.max = min(a.min, 0), max(a.max, b-1) - @property - def expr(self): - assert self.a != self - return f"({self.a}%{self.b})" +# operation nodes -class GeNode(Node): - def __init__(self, a:Node, b:int): - self.a, self.b = a, b - self.min, self.max = 0, 1 - @property - def expr(self): - return f"({self.a}>={self.b})" +class GeNode(OpNode): minmax = staticmethod(lambda a,b: (int(a.min >= b), int(a.max >= b))) +class LtNode(OpNode): minmax = staticmethod(lambda a,b: (int(a.max < b), int(a.min < b))) +class MulNode(OpNode): minmax = staticmethod(lambda a,b: (a.min*b, a.max*b) if b >= 0 else (a.max*b, a.min*b)) +class DivNode(OpNode): + @staticmethod + def minmax(a, b): + assert a.min >= 0 + return a.min//b, a.max//b -class LtNode(Node): - def __init__(self, a:Node, b:int): - self.a, self.b = a, b - self.min, self.max = 0, 1 - @property - def expr(self): - return f"({self.a}<{self.b})" +class ModNode(OpNode): + @staticmethod + def minmax(a, b): + assert a.min >= 0 + if a.max - a.min >= b or (a.min != a.max and a.min%b >= a.max%b): return (0, b-1) + return a.min%b, a.max%b # reduce nodes -class SumNode(Node): - def __init__(self, nodes:List[Node]): - self.nodes = nodes - self.min, self.max = sum([x.min for x in nodes]), sum([x.max for x in nodes]) - @property - def expr(self): - return f"({'+'.join([str(x) for x in self.nodes])})" +class SumNode(RedNode): minmax = staticmethod(lambda nodes: (sum([x.min for x in nodes]), sum([x.max for x in nodes]))) +class AndNode(RedNode): minmax = staticmethod(lambda nodes: (min([x.min for x in nodes]), max([x.max for x in nodes]))) -class AndNode(Node): - def __init__(self, nodes:List[Node]): - self.nodes = nodes - self.min, self.max = min([x.min for x in nodes]), max([x.max for x in nodes]) - @property - def expr(self): - return f"({'&&'.join([str(x) for x in self.nodes])})" +render_python : Dict[Type, Callable] = { + Variable: lambda self,ops,ctx: f"{self.expr}<{self.min},{self.max}>" if ctx == "DEBUG" else f"{self.expr}", + NumNode: lambda self,ops,ctx: f"{self.b}", + MulNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}*{self.b})", + DivNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}//{self.b})", + ModNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}%{self.b})", + GeNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}>={self.b})", + LtNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}<{self.b})", + SumNode: lambda self,ops,ctx: f"({'+'.join(sorted([x.render(ops,ctx) for x in self.nodes]))})", + AndNode: lambda self,ops,ctx: f"({'&&'.join(sorted([x.render(ops,ctx) for x in self.nodes]))})" +} \ No newline at end of file diff --git a/tinygrad_repo/tinygrad/tensor.py b/tinygrad_repo/tinygrad/tensor.py index 35d2222e7..b7bd2146b 100644 --- a/tinygrad_repo/tinygrad/tensor.py +++ b/tinygrad_repo/tinygrad/tensor.py @@ -1,17 +1,39 @@ # inspired by https://github.com/karpathy/micrograd/blob/master/micrograd/engine.py from __future__ import annotations -import inspect, functools, importlib, itertools +import math, functools, itertools import numpy as np -from tinygrad.helpers import prod, argfix -from typing import List, Tuple, Callable, Optional +from typing import List, Tuple, Callable, Optional, ClassVar, Type, Union, Sequence +from tinygrad.helpers import prod, argfix, make_pair, getenv, DEBUG, flatten from tinygrad.lazy import Device, LazyBuffer +from tinygrad.image import image_conv2d_decorator + +# An instantiation of the Function is the Context +class Function: + def __init__(self, device:str, *tensors:Tensor): + self.device, self.parents = device, tensors + self.needs_input_grad = [t.requires_grad for t in self.parents] + self.requires_grad = True if any(self.needs_input_grad) else (None if any(x is None for x in self.needs_input_grad) else False) + + def forward(self, *args, **kwargs): raise NotImplementedError(f"forward not implemented for {type(self)}") + def backward(self, *args, **kwargs): raise RuntimeError(f"backward not implemented for {type(self)}") + + @classmethod + def apply(fxn:Type[Function], *x:Tensor, **kwargs) -> Tensor: + ctx = fxn(x[0].device, *x) + ret = Tensor(ctx.forward(*[t.lazydata for t in x], **kwargs), device=ctx.device, requires_grad=ctx.requires_grad) + if ctx.requires_grad and not Tensor.no_grad: ret._ctx = ctx # used by autograd engine + return ret + +import tinygrad.mlops as mlops # **** start with two base classes, Tensor and Function **** class Tensor: - training, no_grad = False, False + __deletable__ = ('_ctx',) + training : ClassVar[bool] = False + no_grad : ClassVar[bool] = False - def __init__(self, data, device=Device.DEFAULT, requires_grad=None): + def __init__(self, data, device=Device.DEFAULT, requires_grad:Optional[bool]=None): if isinstance(data, list): data = np.array(data, dtype=np.float32) elif isinstance(data, LazyBuffer) and data.device != device: @@ -24,7 +46,7 @@ class Tensor: elif isinstance(data, LazyBuffer): self.lazydata = data else: - raise Exception(f"can't create Tensor from {data}") + raise RuntimeError(f"can't create Tensor from {data}") # tensors have gradients, buffers do not self.grad : Optional[Tensor] = None @@ -40,34 +62,32 @@ class Tensor: return f"" @property - def shape(self): return self.lazydata.shape + def shape(self) -> Tuple[int, ...]: return self.lazydata.shape # dtype handling was very broken. it's always float32 now @property - def dtype(self): return np.float32 + def dtype(self) -> type: return np.float32 @property - def device(self): return self.lazydata.device + def device(self) -> str: return self.lazydata.device # ***** data handlers **** - def realize(self): + def realize(self) -> Tensor: self.lazydata.realize() return self - def assign(self, x): - if not isinstance(x, Tensor): - x = Tensor(x) + def assign(self, x) -> Tensor: + if not isinstance(x, Tensor): x = Tensor(x) assert self.shape == x.shape + assert not x.requires_grad # self requires_grad is okay? + if DEBUG >= 4: print(f"assign {self.lazydata} <- {x.lazydata}") + if self.lazydata.realized is not None and not getenv("DISALLOW_ASSIGN"): x.lazydata.output_buffer = self.lazydata.realized self.lazydata = x.lazydata - return x + return self def detach(self): return Tensor(self.lazydata, device=self.device, requires_grad=False) - def numpy(self): return np.array(self.lazydata.toCPU()) - - # TODO: this keeps the legacy behavior working, remove it after refactor - @property - def data(self): return self.numpy() + def numpy(self) -> np.ndarray: return self.lazydata.toCPU() # TODO: if things are realized this won't work def to_(self, device:str): @@ -84,41 +104,51 @@ class Tensor: # ***** creation helper functions ***** - # TODO: remove use of numpy here + @staticmethod + def zeros(*shape, **kwargs): return Tensor([0], **kwargs).reshape([1]*len(shape)).expand(shape).contiguous() - @classmethod - def zeros_like(cls, tensor, **kwargs): return cls.zeros(*tensor.shape, **kwargs) + @staticmethod + def ones(*shape, **kwargs): return Tensor([1], **kwargs).reshape([1]*len(shape)).expand(shape).contiguous() - @classmethod - def zeros(cls, *shape, **kwargs): return cls(np.zeros(shape, dtype=np.float32), **kwargs) + @staticmethod + def zeros_like(tensor, **kwargs): return Tensor.zeros(*tensor.shape, **kwargs) - @classmethod - def ones(cls, *shape, **kwargs): return cls(np.ones(shape, dtype=np.float32), **kwargs) + @staticmethod + def empty(*shape, **kwargs): return Tensor.zeros(*shape, **kwargs) - @classmethod - def empty(cls, *shape, **kwargs): return cls(np.empty(shape, dtype=np.float32), **kwargs) + @staticmethod + def eye(dim, **kwargs): return Tensor([1], **kwargs).slice(((0,dim+1),)).reshape(1, dim+1).expand(dim, dim+1).reshape(dim*(dim+1)).slice(((0,dim*dim),)).reshape(dim, dim) - @classmethod - def randn(cls, *shape, **kwargs): return cls(np.random.default_rng().standard_normal(size=shape, dtype=np.float32), **kwargs) + # TODO: below line, remove use of numpy here and make lazy + # TODO: requires cumsum to remove numpy + @staticmethod + def arange(stop, start=0, step=1, **kwargs): return Tensor(np.arange(start=start, stop=stop, step=step, dtype=np.float32), **kwargs) - @classmethod - def arange(cls, stop, start=0, **kwargs): return cls(np.arange(start=start, stop=stop, dtype=np.float32), **kwargs) + # ***** (numpy) rng helper functions ***** + # TODO: move randomness generation out of numpy - # TODO: uniform should be a late binding thing - # Return random number between -1 and 1 - # NOTE: this behavior changed from depending on the shape to not - @classmethod - def uniform(cls, *shape, **kwargs): return cls((np.random.default_rng().random(size=shape, dtype=np.float32) * 2 - 1), **kwargs) + _rng : ClassVar[np.random.Generator] = np.random.default_rng() + @staticmethod + def manual_seed(seed=None): Tensor._rng = np.random.default_rng(seed=seed) - @classmethod - def scaled_uniform(cls, *shape, **kwargs): return cls((np.random.default_rng().random(size=shape, dtype=np.float32) * 2 - 1) * (prod(shape)**-0.5), **kwargs) + @staticmethod + def rand(*shape, **kwargs) -> Tensor: return Tensor(Tensor._rng.random(size=shape, dtype=np.float32), **kwargs) + + # TODO: replace with a transformation from uniform -> gaussian + @staticmethod + def randn(*shape, **kwargs) -> Tensor: return Tensor(Tensor._rng.standard_normal(size=shape, dtype=np.float32), **kwargs) + + # ***** rng hlops ***** + + @staticmethod + def uniform(*shape, **kwargs) -> Tensor: return Tensor.rand(*shape, **kwargs) * 2 - 1 + + @staticmethod + def scaled_uniform(*shape, **kwargs) -> Tensor: return Tensor.uniform(*shape, **kwargs).mul(prod(shape)**-0.5) - @classmethod # https://www.tensorflow.org/api_docs/python/tf/keras/initializers/GlorotUniform - def glorot_uniform(cls, *shape, **kwargs): return cls((np.random.default_rng().random(size=shape, dtype=np.float32) * 2 - 1) * ((6/(shape[0]+prod(shape[1:])))**0.5), **kwargs) - - @classmethod - def eye(cls, dim, **kwargs): return cls(np.eye(dim, dtype=np.float32), **kwargs) + @staticmethod + def glorot_uniform(*shape, **kwargs) -> Tensor: return Tensor.uniform(*shape, **kwargs).mul((6/(shape[0]+prod(shape[1:])))**0.5) # ***** toposort and backward pass ***** @@ -126,7 +156,8 @@ class Tensor: def _deepwalk(node, visited, nodes): visited.add(node) if node._ctx: - [_deepwalk(i, visited, nodes) for i in node._ctx.parents if i not in visited] + for i in node._ctx.parents: + if i not in visited: _deepwalk(i, visited, nodes) nodes.append(node) return nodes return _deepwalk(self, set(), []) @@ -151,47 +182,169 @@ class Tensor: t.grad = g if t.grad is None else (t.grad + g) del t0._ctx - # ***** non first class ops (hlops) ***** + # ***** movement mlops ***** + def reshape(self, shape, *args) -> Tensor: + new_shape = argfix(shape, *args) + assert len(new_shape) > 0 and all(x != 0 for x in new_shape), f"zeros not allowed in shape {new_shape}" + return mlops.Reshape.apply(self, shape=tuple(-prod(self.shape) // prod(new_shape) if s == -1 else s for s in new_shape)) + def expand(self, shape, *args) -> Tensor: return mlops.Expand.apply(self, shape=tuple(x if x != -1 else s for s,x in zip(self.shape, argfix(shape, *args)))) + def permute(self, order, *args) -> Tensor: return mlops.Permute.apply(self, order=argfix(order, *args)) + def flip(self, axis, *args) -> Tensor: return mlops.Flip.apply(self, axis=argfix(axis, *args)) + def pad(self, arg:Tuple[Tuple[int, int], ...]) -> Tensor: return mlops.Pad.apply(self, arg=arg) if any(x != (0,0) for x in arg) else self + def shrink(self, arg:Tuple[Tuple[int, int], ...]) -> Tensor: return mlops.Shrink.apply(self, arg=arg) if any(x != (0,s) for x,s in zip(arg, self.shape)) else self + + # ***** movement hlops ***** + + # NOTE: using slice is discouraged and things should migrate to pad and shrink + def slice(self, arg:Sequence[Optional[Tuple[int, int]]]) -> Tensor: + arg_ = tuple(a if a is not None else (0,s) for s,a in zip(self.shape, arg)) + padding = tuple((max(0, -p[0]), max(0, p[1]-self.shape[i])) for i,p in enumerate(arg_)) + return self.pad(padding).shrink(tuple((p[0] + padding[i][0], p[1] + padding[i][0]) for i,p in enumerate(arg_))) + + # Tensors mostly follow the normal python indexing / slicing behavior for sequences + # - Negative indices are taken relative to the end of the sequence, so X[-2] returns the 2nd-to-last element + # - A slice i:j returns the elements with indices in [i, j) + # - If omitted, i and j will default to 0 and N, respectively, where N is the length of the sequence + # - Negative values for i and j are taken relative to the end of the sequence + # - Both i and j will be clamped to the range (-N, N], where N in the length of the sequence + # - Indexing with np.newaxis or None on a given axis will add a new dimension of size one before that axis + # - Empty slices are not allowed + # - Strides other than 1 are not allowedå def __getitem__(self, val): - arg, new_shape = [], [] - for i, rs in enumerate(val if isinstance(val, (list, tuple)) else [val]) if val is not None else []: - s = slice(rs, rs+1, None) if isinstance(rs, int) else rs - arg.append((s.start if s.start is not None else 0, (s.stop if s.stop>=0 else self.shape[i]+s.stop) if s.stop is not None else self.shape[i])) - assert s.step is None or s.step == 1 - if not isinstance(rs, int): # don't include in shape if it's an int - new_shape.append(arg[-1][1] - arg[-1][0]) - new_shape += [self.shape[i] for i in range(len(arg), len(self.shape))] - return self.slice(arg = arg + [(0,self.shape[i]) for i in range(len(arg), len(self.shape))]).reshape(new_shape if len(new_shape) else (1,)) + def slcfix(i, sz, default): return default if i is None else max(0, min(sz, sz+i if i < 0 else i)) # Fix negative idxs, clamp to [0,N] + new_slice, new_shape = [], [] + val = [val] if not isinstance(val, (list, tuple)) else val + assert sum(s is not None for s in val) <= len(self.shape) + assert all(s.step is None or s.step == 1 for s in val if isinstance(s, slice)) + for i,(sz,s) in enumerate(zip(self.shape, [v for v in val if v is not None])): # Slicing only depends on ints + slices + if isinstance(s, int) and not (-sz <= s < sz): + raise IndexError(f"index {s} is out of bounds for dimension {i} with size {sz}") + new_slice.append((s%sz, s%sz+1) if isinstance(s, int) else (slcfix(s.start, sz, 0), slcfix(s.stop, sz, sz))) + for s,sz in zip(val, [self.shape[i-1] for i in itertools.accumulate([s is not None for s in val])]): # Shape depends on slices + positions of Nones + if not isinstance(s, int): + new_shape.append(1 if s is None else slcfix(s.stop, sz, sz) - slcfix(s.start, sz, 0)) + new_shape += [self.shape[i] for i in range(len(new_slice), len(self.shape))] + new_slice += [(0,self.shape[i]) for i in range(len(new_slice), len(self.shape))] + return self.slice(new_slice).reshape(new_shape if len(new_shape) else (1,)) def cat(self, *args, dim=0): dim = (dim + len(self.shape)) if dim < 0 else dim for y in args: assert len(y.shape) == len(self.shape) and all(y.shape[i] == s for i,s in enumerate(self.shape) if i != dim) - args = [self] + list(args) - shape_cumsum = [0, *itertools.accumulate(y.shape[dim] for y in args)] - slc = [[(0, s) for s in self.shape] for _ in args] + catargs = [self] + list(args) + shape_cumsum = [0, *itertools.accumulate([y.shape[dim] for y in catargs])] + slc = [[(0, s) for s in self.shape] for _ in catargs] for s,k in zip(slc, shape_cumsum): s[dim] = (-k, shape_cumsum[-1]-k) - return functools.reduce(Tensor.__iadd__, [arg.slice(arg=s) for arg,s in zip(args, slc)]) + return functools.reduce(Tensor.__add__, [arg.slice(s) for arg,s in zip(catargs, slc)]) # TODO: make this nicer with syntactic sugar in slice def chunk(self, num, dim): slice_params = [[(0, s) for s in self.shape] for _ in range(num)] for i,k in enumerate(range(0, self.shape[dim], self.shape[dim]//num)): slice_params[i][dim] = (k, min(self.shape[dim], k+self.shape[dim]//num)) - return [self.slice(arg=p) for p in slice_params] + return [self.slice(p) for p in slice_params] - def matmul(self:Tensor, w:Tensor): + def unsqueeze(self, dim): + if dim < 0: dim = len(self.shape) + dim + 1 + return self.reshape(self.shape[:dim] + (1,) + self.shape[dim:]) + + # (padding_left, padding_right, padding_top, padding_bottom) + def pad2d(self, padding:Tuple[int, ...]): return self.slice(((0,self.shape[0]), (0,self.shape[1]), (-padding[2],self.shape[2]+padding[3]), (-padding[0],self.shape[3]+padding[1]))) + # TODO: this is totally not transpose + def transpose(self, order=(1,0)) -> Tensor: return self.permute(order=order) + def flatten(self, start_dim=0): return self.reshape(shape=tuple(list(self.shape[0:start_dim]) + [-1])) + + # ***** reduce ops ***** + + def _reduce(self, fxn:Type[Function], axis:Optional[Union[int, Tuple[int, ...]]]=None, keepdim=False): + axis_ : List[int] = list(range(len(self.shape))) if axis is None else ([axis] if isinstance(axis, int) else list(axis)) + axis_ = [x if x >= 0 else x+len(self.shape) for x in axis_] + shape = [self.shape[i] for i in range(len(self.shape)) if i not in axis_] + ret = fxn.apply(self, new_shape=tuple(1 if i in axis_ else self.shape[i] for i in range(len(self.shape)))) + return ret if keepdim else ret.reshape(shape=[1] if shape == [] else shape) + + def sum(self, axis=None, keepdim=False): return self._reduce(mlops.Sum, axis, keepdim) + def max(self, axis=None, keepdim=False): return self._reduce(mlops.Max, axis, keepdim) + def min(self, axis=None, keepdim=False): return -((-self).max(axis=axis, keepdim=keepdim)) + + def mean(self, axis=None, keepdim=False): + out = self.sum(axis=axis, keepdim=keepdim) + return out * (prod(out.shape)/prod(self.shape)) + + def _softmax(self, axis): + m = self - self.max(axis=axis, keepdim=True) + e = m.exp() + return m, e, e.sum(axis=axis, keepdim=True) + + def softmax(self, axis=-1): + _, e, ss = self._softmax(axis) + return e.div(ss) + + def log_softmax(self, axis=-1): + m, _, ss = self._softmax(axis) + return m - ss.log() + + # ***** processing ops ***** + + def _pool(self, k_:Tuple[int, ...], stride:Union[Tuple[int, ...], int]=1, dilation:Union[Tuple[int, ...], int]=1) -> Tensor: + assert len(self.shape) >= len(k_), f"can't pool {self.shape} with {k_}" + s_, d_ = make_pair(stride, len(k_)), make_pair(dilation, len(k_)) + assert len(k_) == len(s_) and len(k_) == len(d_), f"stride/dilation mismatch kernel:{k_} stride:{s_} dilation:{d_}" + slc_prefix, prefix, i_ = [(0,x) for x in self.shape[0:-len(k_)]], self.shape[0:-len(k_)], self.shape[-len(k_):] + if any(k > s for k,s in zip(k_, s_)) or any(d != 1 for d in d_): + o_ = [(i - d * (k-1) - 1)//s + 1 for i,d,k,s in zip(i_, d_, k_, s_)] + e_ = [math.ceil(k*(i+d) / i) for k,i,d in zip(k_, i_, d_)] # expands such that we don't need padding + xup = self.reshape(*prefix, *flatten((1,i) for i in i_)).expand(*prefix, *flatten((e,i) for e,i in zip(e_, i_))).reshape(*prefix, *[e*i for e,i in zip(e_, i_)]) + # slide by dilation + xup = xup.slice(slc_prefix + [(0,k*(i+d)) for k,i,d in zip(k_, i_, d_)]) + xup = xup.reshape(*prefix, *flatten((k,i+d) for k,i,d in zip(k_, i_, d_))) + xup = xup.slice(slc_prefix + flatten(((0,k), (0,o*s)) for k,o,s in zip(k_, o_, s_))) + # handle stride, and permute to move reduce to the end + xup = xup.reshape(*prefix, *flatten((k,o,s) for k,o,s in zip(k_, o_, s_))) + xup = xup.slice(slc_prefix + flatten(((0,k), (0,o), (0,1)) for k,o in zip(k_, o_))) + xup = xup.reshape(*prefix, *flatten((k,o) for k,o in zip(k_, o_))) + return xup.permute(*range(len(prefix)), *[len(prefix)+i*2+1 for i in range(len(k_))], *[len(prefix)+i*2 for i in range(len(k_))]) + else: + # TODO: once the shapetracker can optimize well, remove this alternative implementation. or not if the CPU implementation doesn't use ShapeTracker + o_ = [(i+(s-k))//s for i,s,k in zip(i_, s_, k_)] + xup = self.slice(slc_prefix + [(0,o*s) for o,s in zip(o_, s_)]) + xup = xup.reshape(*prefix, *flatten(((o, s) for o,s in zip(o_, s_)))) + xup = xup.slice(slc_prefix + flatten(((0,o), (0,k)) for o,k in zip(o_, k_))) + return xup.permute(*range(len(prefix)), *[len(prefix)+i*2 for i in range(len(k_))], *[len(prefix)+i*2+1 for i in range(len(k_))]) + + # NOTE: these work for more than 2D + def avg_pool2d(self, kernel_size=(2,2), stride=None): return self._pool(make_pair(kernel_size), stride if stride is not None else kernel_size).mean(axis=tuple(range(0-len(make_pair(kernel_size)), 0))) + def max_pool2d(self, kernel_size=(2,2), stride=None): return self._pool(make_pair(kernel_size), stride if stride is not None else kernel_size).max(axis=tuple(range(0-len(make_pair(kernel_size)), 0))) + + @image_conv2d_decorator + def conv2d(self, weight:Tensor, bias:Optional[Tensor]=None, groups=1, stride=1, dilation=1, padding=0) -> Tensor: + (bs,cin_,_,_), (cout,cin,H,W) = self.shape, weight.shape + assert cin*groups == cin_, f"Input Tensor shape {self.shape} does not match the shape of the weights {weight.shape}. ({cin*groups} vs. {cin_})" + padding_ = [padding]*4 if isinstance(padding, int) else (padding if len(padding) == 4 else [padding[1], padding[1], padding[0], padding[0]]) + + # conv2d is a pooling op (with padding) + x = self.pad2d(padding_)._pool((H,W),stride, dilation) + + oy, ox, rcout = x.shape[2], x.shape[3], cout//groups + # NOTE: we do this expand explicitly so the permute isn't pushed in the binop + x = x.reshape(bs, groups, 1, cin, oy, ox, H, W).expand(bs, groups, rcout, cin, oy, ox, H, W).permute(0,1,2,4,5,3,6,7) + + # conv! broadcasted to (bs, groups, rcout, oy, ox, cin, H, W) + ret = (x * weight.reshape(1, groups, rcout, 1, 1, cin, H, W)).sum((-3, -2, -1)).reshape(bs, cout, oy, ox) + return ret if bias is None else ret.add(bias.reshape(1, -1, 1, 1)) + + def dot(self, w:Tensor) -> Tensor: # NOTE: we use a 1x1 conv2d to do the matmul. mxk @ kxn = (1,k,m,1).conv2d(n,k,1,1) bs, groups = prod(self.shape[0:-2]), prod(w.shape[0:-2]) cin, cout = w.shape[-2], w.shape[-1] - out_shape_t = tuple(list(self.shape[0:-2])+[cout,-1]) + out_shape_t = self.shape[0:-2] + (cout,-1) if len(self.shape) > 1: - order = tuple(list(range(len(self.shape)-2))+[len(self.shape)-1, len(self.shape)-2]) + order = tuple(range(len(self.shape)-2)) + (len(self.shape)-1, len(self.shape)-2) else: order, out_shape_t = (0,), (cout, ) - worder = tuple(list(range(len(w.shape)-2))+[len(w.shape)-1, len(w.shape)-2]) + worder = tuple(range(len(w.shape)-2)) + (len(w.shape)-1, len(w.shape)-2) # NOTE: with NHWC we can remove the transposes # bs x groups*cin x H x W @@ -200,63 +353,11 @@ class Tensor: cw = w.transpose(order=worder).reshape(shape=(groups*cout, cin, 1, 1)) return cx.conv2d(cw, groups=groups).reshape(shape=out_shape_t).transpose(order=order) - # TODO: what's the difference between dot and matmul? - dot = matmul + # ***** mlops (unary) ***** - # (padding_left, padding_right, padding_top, padding_bottom) - def pad2d(self, padding:Tuple[int, ...]): return self[:, :, -padding[2]:self.shape[2]+padding[3], -padding[0]:self.shape[3]+padding[1]] - # TODO: this is totally not transpose - def transpose(self, order=(1,0)): return self.permute(order=order) - def flatten(self, start_dim=0): return self.reshape(shape=tuple(list(self.shape[0:start_dim]) + [-1])) - - def _reduce(self, fxn, axis=None, keepdim=False): - if axis is None: - axis = range(len(self.shape)) - if isinstance(axis, int): - axis = [axis] - axis = tuple([x if x >= 0 else x+len(self.shape) for x in axis]) - shape = [self.shape[i] for i in range(len(self.shape)) if i not in axis] - ret = fxn(self, axis=axis) - return ret if keepdim else ret.reshape(shape=[1] if shape == [] else shape) - - def sum(self, axis=None, keepdim=False): return self._reduce(Tensor._sum, axis, keepdim) - def max(self, axis=None, keepdim=False): return self._reduce(Tensor._max, axis, keepdim) - def min(self, axis=None, keepdim=False): return -((-self).max(axis=axis, keepdim=keepdim)) - - def mean(self, axis=None, keepdim=False): - out = self.sum(axis=axis, keepdim=keepdim) - return out * (prod(out.shape)/prod(self.shape)) - - def _softmax(self): - m = self - self.max(axis=len(self.shape)-1, keepdim=True) - e = m.exp() - return m, e, e.sum(axis=len(self.shape)-1, keepdim=True) - - def softmax(self): - _, e, ss = self._softmax() - return e.div(ss) - - def logsoftmax(self): - m, _, ss = self._softmax() - return m - ss.log() - - def dropout(self, p=0.5): - if not Tensor.training: - return self - _mask = np.asarray(np.random.binomial(1, 1.0-p, size=self.shape), dtype=self.dtype) - return self * Tensor(_mask, requires_grad=False, device=self.device) * (1/(1.0 - p)) - - # TODO: support arbitrary strides - def _pool2d(self, py, px): - xup = self[:, :, :self.shape[2]-self.shape[2]%py, :self.shape[3]-self.shape[3]%px] if (self.shape[2]%py != 0) or (self.shape[3]%px != 0) else self - return xup.reshape(shape=(xup.shape[0], xup.shape[1], xup.shape[2]//py, py, xup.shape[3]//px, px)) - - def avg_pool2d(self, kernel_size=(2,2)): return self._pool2d(*kernel_size).mean(axis=(3,5)) - def max_pool2d(self, kernel_size=(2,2)): return self._pool2d(*kernel_size).max(axis=(3,5)) - - def conv2d(self, weight, bias=None, **kwargs): - ret = self._conv2d(weight, **kwargs) - return ret if bias is None else ret.add(bias.reshape(shape=[1, -1, 1, 1])) + def contiguous(self): return mlops.Contiguous.apply(self) + def log(self): return mlops.Log.apply(self) + def exp(self): return mlops.Exp.apply(self) # ***** math functions (unary) ***** @@ -266,13 +367,15 @@ class Tensor: def clip(self, min_, max_): return ((self-min_).relu()+min_) - (self-max_).relu() def abs(self): return self.relu() + (-self).relu() def sign(self): return self / (self.abs() + 1e-10) + def relu(self): return self.maximum(0) + def reciprocal(self): return 1.0/self # ***** activation functions (unary) ***** def sigmoid(self): return (1.0 + (-self).exp()).reciprocal() def elu(self, alpha=1.0): return self.relu() - alpha*(1-self.exp()).relu() def swish(self): return self * self.sigmoid() - silu = swish # The SiLU function is also known as the swish function. + def silu(self): return self.swish() # The SiLU function is also known as the swish function. def relu6(self): return self.relu() - (self-6).relu() def hardswish(self): return self * (self+3).relu6() * (1/6) def tanh(self): return 2.0 * ((2.0 * self).sigmoid()) - 1.0 @@ -280,84 +383,78 @@ class Tensor: def quick_gelu(self): return self * (self * 1.702).sigmoid() def leakyrelu(self, neg_slope=0.01): return self.relu() - (-neg_slope*self).relu() def mish(self): return self * self.softplus().tanh() - def softplus(self, limit=20, beta=1): return (1/beta) * (1 + (self*beta).exp()).log() + def softplus(self, beta=1): return (1/beta) * (1 + (self*beta).exp()).log() - # ***** broadcasted binary ops ***** + # ***** broadcasted binary mlops ***** - @staticmethod - def broadcasted(fxn, x, y): - tt = [arg for arg in [x,y] if isinstance(arg, Tensor)][0] # this is the prototype tensor - x,y = [Tensor([t], device=tt.device, requires_grad=False) if not isinstance(t, Tensor) else t for t in [x,y]] + def _broadcasted(self, fxn:Type[Function], other:Union[Tensor, float], reverse:bool=False) -> Tensor: + x,y = [Tensor([t], device=self.device, requires_grad=False) if not isinstance(t, Tensor) else t for t in ([other,self] if reverse else [self,other])] x,y = [t.reshape([1]*(max(len(x.shape), len(y.shape))-len(t.shape)) + list(t.shape)) for t in [x,y]] shape_ret = tuple(max(sx, sy) for sx,sy in zip(x.shape, y.shape)) - return fxn(x.expand(shape_ret), y.expand(shape_ret)) + return fxn.apply(x.expand(shape_ret), y.expand(shape_ret)) - # TODO: are these the only ones that can take number arguments? - def add(self, x): return Tensor.broadcasted(Tensor._add, self, x) - def sub(self, x): return Tensor.broadcasted(Tensor._sub, self, x) - def mul(self, x): return Tensor.broadcasted(Tensor._mul, self, x) - def pow(self, x): return Tensor.broadcasted(Tensor._pow, self, x) - def div(self, y): return self * (y.reciprocal() if isinstance(y, Tensor) else (1/y)) + def add(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Add, x, reverse) if isinstance(x, Tensor) or x != 0.0 else self + def sub(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Sub, x, reverse) if isinstance(x, Tensor) or x != 0.0 or reverse else self + def mul(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Mul, x, reverse) if isinstance(x, Tensor) or x != 1.0 else self + def pow(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Pow, x, reverse) if isinstance(x, Tensor) or x != 1.0 or reverse else self + def div(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Div, x, reverse) if isinstance(x, Tensor) or x != 1.0 or reverse else self + def matmul(self, x:Tensor, reverse=False) -> Tensor: return x.dot(self) if reverse else self.dot(x) + + def maximum(self, x:Union[Tensor, float]) -> Tensor: return self._broadcasted(mlops.Maximum, x) + def minimum(self, x:Union[Tensor, float]) -> Tensor: return -((-self).maximum(-x)) + def eq(self, x) -> Tensor: return self._broadcasted(mlops.Equal, x, False) + + # ***** binary op wrappers (18 wasted lines to make the typechecker happy) ***** + + # NOTE: __pow__ and friends are broken in mypyc with the ** operator + def __add__(self, x) -> Tensor: return self.add(x) + def __sub__(self, x) -> Tensor: return self.sub(x) + def __mul__(self, x) -> Tensor: return self.mul(x) + def __pow__(self, x) -> Tensor: return self.pow(x) + def __truediv__(self, x) -> Tensor: return self.div(x) + def __matmul__(self, x) -> Tensor: return self.matmul(x) + + def __radd__(self, x) -> Tensor: return self.add(x, True) + def __rsub__(self, x) -> Tensor: return self.sub(x, True) + def __rmul__(self, x) -> Tensor: return self.mul(x, True) + def __rpow__(self, x) -> Tensor: return self.pow(x, True) + def __rtruediv__(self, x) -> Tensor: return self.div(x, True) + def __rmatmul__(self, x) -> Tensor: return self.matmul(x, True) + + def __iadd__(self, x) -> Tensor: return self.assign(self.add(x)) + def __isub__(self, x) -> Tensor: return self.assign(self.sub(x)) + def __imul__(self, x) -> Tensor: return self.assign(self.mul(x)) + def __ipow__(self, x) -> Tensor: return self.assign(self.pow(x)) + def __itruediv__(self, x) -> Tensor: return self.assign(self.div(x)) + def __imatmul__(self, x) -> Tensor: return self.assign(self.matmul(x)) + + def __ge__(self, x) -> Tensor: return self.maximum(x).eq(self) + def __le__(self, x) -> Tensor: return self.maximum(x).eq(x) + def __lt__(self, x) -> Tensor: return 1.0-(self>=x) + def __gt__(self, x) -> Tensor: return 1.0-(self<=x) # ***** functional nn ops ***** - # TODO: fix the kwargs problem, then remove these (or not, since they now fix tuples) - def reshape(self, shape, *args): return self._reshape(shape=argfix(shape, *args)) - def expand(self, shape, *args): return self._expand(shape=tuple(x if x != -1 else s for s,x in zip(self.shape, argfix(shape, *args)))) - def permute(self, order, *args): return self._permute(order=argfix(order, *args)) - def linear(self, weight:Tensor, bias:Optional[Tensor]=None): x = self.mul(weight) if len(weight.shape) == 1 else self.dot(weight) return x.add(bias) if bias is not None else x def sequential(self, ll:List[Callable[[Tensor], Tensor]]): return functools.reduce(lambda x,f: f(x), ll, self) - def layernorm(self, axis=-1, eps=1e-5): + def layernorm(self, axis=-1, eps:float=1e-5) -> Tensor: y = (self - self.mean(axis=axis, keepdim=True)) return y.div((y*y).mean(axis=axis, keepdim=True).add(eps).sqrt()) - def batchnorm(self, weight:Tensor, bias:Tensor, mean:Tensor, invstd:Tensor): + def batchnorm(self, weight:Tensor, bias:Tensor, mean:Tensor, invstd:Tensor) -> Tensor: x = (self - mean.reshape(shape=[1, -1, 1, 1])) * weight.reshape(shape=[1, -1, 1, 1]) return x.mul(invstd.reshape(shape=[1, -1, 1, 1])) + bias.reshape(shape=[1, -1, 1, 1]) -# An instantiation of the Function is the Context -class Function: - def __init__(self, device:str, *tensors:Tensor): - self.device, self.parents = device, tensors - self.needs_input_grad = [t.requires_grad for t in self.parents] - self.requires_grad = True if any(self.needs_input_grad) else (None if any(x is None for x in self.needs_input_grad) else False) - self.saved_tensors : List[Tensor] = [] - - def forward(self, *args, **kwargs): raise NotImplementedError(f"forward not implemented for {type(self)}") - def backward(self, *args, **kwargs): raise NotImplementedError(f"backward not implemented for {type(self)}") - - # NOTE: it doesn't hurt to save this since the ctx will be freed fast without grad - def save_for_backward(self, *x): self.saved_tensors.extend(x) - - @classmethod - def apply(cls, *x:Tensor, **kwargs): - ctx = cls(x[0].device, *x) - ret = Tensor(ctx.forward(*[t.lazydata for t in x], **kwargs), device=ctx.device, requires_grad=ctx.requires_grad) - if ctx.requires_grad and not Tensor.no_grad: - ret._ctx = ctx # used by autograd engine - return ret + def dropout(self, p=0.5) -> Tensor: + if not Tensor.training: return self + _mask : np.ndarray = np.asarray(Tensor._rng.binomial(1, 1.0-p, size=self.shape), dtype=self.dtype) + return self * Tensor(_mask, requires_grad=False, device=self.device) * (1/(1.0 - p)) # register functions to move between devices -for device in [device for device in Device.__dict__.keys() if device[0] != "_"]: - setattr(Tensor, f"{device.lower()}", functools.partialmethod(Tensor.to, Device.__dict__[device])) - setattr(Tensor, f"{device.lower()}_", functools.partialmethod(Tensor.to_, Device.__dict__[device])) - -# register all the mlops "math" operations -def register(name:str, fxn:Function): - setattr(Tensor, "_"+name if hasattr(Tensor, name) else name, functools.partialmethod(fxn.apply)) -for name, cls in inspect.getmembers(importlib.import_module('tinygrad.mlops'), inspect.isclass): - if name[0] != "_" and name != "Function" and not name.endswith("Ops"): - register(name.lower(), cls) - -# register the operators -def register_op(name, fxn): - setattr(Tensor, f"__{name}__", fxn) - setattr(Tensor, f"__i{name}__", lambda self,x: self.assign(fxn(self,x))) - setattr(Tensor, f"__r{name}__", lambda self,x: fxn(x,self)) -for name in ['add', 'sub', 'mul', 'pow', 'matmul', 'truediv']: - register_op(name, getattr(Tensor, name if name != 'truediv' else 'div')) +for device in [device for device in Device._buffers.keys() if device[0] != "_"]: + setattr(Tensor, f"{device.lower()}", functools.partialmethod(Tensor.to, device)) + setattr(Tensor, f"{device.lower()}_", functools.partialmethod(Tensor.to_, device)) diff --git a/tools/lib/README.md b/tools/lib/README.md index 3cf239d2d..d77eef5ac 100644 --- a/tools/lib/README.md +++ b/tools/lib/README.md @@ -1,6 +1,6 @@ ## LogReader -Route is a class for conveniently accessing all the [logs](/selfdrive/loggerd/) from your routes. The LogReader class reads the non-video logs, i.e. rlog.bz2 and qlog.bz2. There's also a matching FrameReader class for reading the videos. +Route is a class for conveniently accessing all the [logs](/system/loggerd/) from your routes. The LogReader class reads the non-video logs, i.e. rlog.bz2 and qlog.bz2. There's also a matching FrameReader class for reading the videos. ```python from tools.lib.route import Route diff --git a/tools/replay/consoleui.cc b/tools/replay/consoleui.cc index 5ad702590..077861ff9 100644 --- a/tools/replay/consoleui.cc +++ b/tools/replay/consoleui.cc @@ -165,7 +165,10 @@ void ConsoleUI::updateStatus() { sm.update(0); if (status != Status::Paused) { - status = (sm.updated("carState") || sm.updated("liveParameters")) ? Status::Playing : Status::Waiting; + auto events = replay->events(); + uint64_t current_mono_time = replay->routeStartTime() + replay->currentSeconds() * 1e9; + bool playing = !events->empty() && events->back()->mono_time > current_mono_time; + status = playing ? Status::Playing : Status::Waiting; } auto [status_str, status_color] = status_text[status]; write_item(0, 0, "STATUS: ", status_str, " ", false, status_color); diff --git a/tools/replay/framereader.cc b/tools/replay/framereader.cc index a1ff7b9f8..ed276c627 100644 --- a/tools/replay/framereader.cc +++ b/tools/replay/framereader.cc @@ -68,14 +68,20 @@ FrameReader::~FrameReader() { bool FrameReader::load(const std::string &url, bool no_hw_decoder, std::atomic *abort, bool local_cache, int chunk_size, int retries) { FileReader f(local_cache, chunk_size, retries); std::string data = f.read(url, abort); - if (data.empty()) return false; + if (data.empty()) { + rWarning("URL %s returned no data", url.c_str()); + return false; + } return load((std::byte *)data.data(), data.size(), no_hw_decoder, abort); } bool FrameReader::load(const std::byte *data, size_t size, bool no_hw_decoder, std::atomic *abort) { input_ctx = avformat_alloc_context(); - if (!input_ctx) return false; + if (!input_ctx) { + rError("Error calling avformat_alloc_context"); + return false; + } struct buffer_data bd = { .data = (const uint8_t*)data, @@ -121,7 +127,10 @@ bool FrameReader::load(const std::byte *data, size_t size, bool no_hw_decoder, s } ret = avcodec_open2(decoder_ctx, decoder, nullptr); - if (ret < 0) return false; + if (ret < 0) { + rError("avcodec_open2 failed %d", ret); + return false; + } packets.reserve(60 * 20); // 20fps, one minute while (!(abort && *abort)) { diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 178b116a8..b68330f68 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -193,7 +193,7 @@ std::optional Replay::find(FindFlag flag) { void Replay::pause(bool pause) { updateEvents([=]() { - rWarning("%s at %d s", pause ? "paused..." : "resuming", currentSeconds()); + rWarning("%s at %.2f s", pause ? "paused..." : "resuming", currentSeconds()); paused_ = pause; return true; }); diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 6788a97d0..2bb426361 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -71,6 +71,7 @@ public: inline void setSpeed(float speed) { speed_ = speed; } inline float getSpeed() const { return speed_; } inline const std::vector *events() const { return events_.get(); } + inline const std::map> &segments() const { return segments_; }; inline const std::string &carFingerprint() const { return car_fingerprint_; } inline const std::vector> getTimeline() { std::lock_guard lk(timeline_lock);