diff --git a/.gitignore b/.gitignore index 6aee0ed8e..31cef9422 100644 --- a/.gitignore +++ b/.gitignore @@ -36,6 +36,7 @@ a.out config.json clcache compile_commands.json +compare_runtime*.html persist board/obj/ @@ -82,3 +83,5 @@ selfdrive/modeld/models/*.thneed build/ !**/.gitkeep + +poetry.toml diff --git a/Jenkinsfile b/Jenkinsfile index 6b05e81d7..c34d25358 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -59,7 +59,7 @@ pipeline { branch 'devel-staging' } steps { - phone_steps("tici", [ + phone_steps("tici-needs-can", [ ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"], ]) } @@ -111,7 +111,7 @@ pipeline { R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}" } steps { - phone_steps("tici", [ + phone_steps("tici-needs-can", [ ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR EXTRA_FILES='tools/' ./build_devel.sh"], ["build openpilot", "cd selfdrive/manager && ./build.py"], ["check dirty", "release/check-dirty.sh"], @@ -122,24 +122,33 @@ pipeline { } } - stage('HW + Unit Tests') { + stage('loopback-tests') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } steps { - phone_steps("tici2", [ - ["build", "cd selfdrive/manager && ./build.py"], - //["test power draw", "python system/hardware/tici/test_power_draw.py"], + phone_steps("tici-loopback", [ + ["build openpilot", "cd selfdrive/manager && ./build.py"], ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.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 sensord", "python selfdrive/sensord/test/test_sensord.py"], ]) } } - stage('camerad') { + stage('HW + Unit Tests') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } steps { - phone_steps("tici-party", [ + 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"], + ]) + } + } + + stage('camerad-ar') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici-ar0321", [ ["build", "cd selfdrive/manager && ./build.py"], ["test camerad", "python system/camerad/test/test_camerad.py"], ["test exposure", "python system/camerad/test/test_exposure.py"], @@ -147,17 +156,43 @@ pipeline { } } + 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"], + ["test exposure", "python system/camerad/test/test_exposure.py"], + ]) + } + } + + stage('sensord') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici-lsmc", [ + ["build", "cd selfdrive/manager && ./build.py"], + ["test sensord", "cd selfdrive/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"], + ]) + } + } + stage('replay') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } steps { - phone_steps("tici3", [ + phone_steps("tici-common", [ ["build", "cd selfdrive/manager && ./build.py"], ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"], ]) } } - } + } } + } } diff --git a/README.md b/README.md index 0b6fc2010..cfeb625bf 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 150+ 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 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. * 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). @@ -50,7 +50,7 @@ We have detailed instructions for [how to mount the device in a car](https://com Running on PC ------ -All of openpilot's services can run as normal on a PC, even without special hardware or a car. To develop or experiment with openpilot you can run openpilot on recorded or simulated data. +All openpilot services can run as usual on a PC without requiring special hardware or a car. You can also run openpilot on recorded or simulated data to develop or experiment with openpilot. With openpilot's tools, you can plot logs, replay drives, and watch the full-res camera streams. See [the tools README](tools/README.md) for more information. @@ -119,7 +119,7 @@ Directory Structure ├── 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 # Deamon that starts/stops all other daemons as needed + ├── 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 diff --git a/RELEASES.md b/RELEASES.md index a6ba4558e..e15286c5f 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,36 @@ +Version 0.9.0 (2022-11-21) +======================== +* New driving model + * Internal feature space information content increased tenfold during training to ~700 bits, which makes the model dramatically more accurate + * Less reliance on previous frames makes model more reactive and snappy + * Trained in new reprojective simulator + * Trained in 36 hours from scratch, compared to one week for previous releases + * Training now simulates both lateral and longitudinal behavior, which allows openpilot to slow down for turns, stop at traffic lights, and more in experimental mode +* Experimental driving mode + * End-to-end longitudinal control + * Stops for traffic lights and stop signs + * Slows down for turns + * openpilot defaults to chill mode, enable experimental mode in settings +* Driver monitoring updates + * New bigger model with added end-to-end distracted trigger + * Reduced false positives during driver calibration +* Self-tuning torque controller: learns parameters live for each car +* Torque controller used on all Toyota, Lexus, Hyundai, Kia, and Genesis models +* UI updates + * Matched speeds shown on car's dash + * Multi-language in navigation + * Improved update experience + * Border turns grey while overriding steering + * Bookmark events while driving; view them in comma connect + * New onroad visualization for experimental mode +* tools: new and improved cabana thanks to deanlee! +* Experimental longitudinal support for Volkswagen, CAN-FD Hyundai, and new GM models +* Genesis GV70 2022-23 support thanks to zunichky and sunnyhaibin! +* Hyundai Santa Cruz 2021-22 support thanks to sunnyhaibin! +* Kia Sportage 2023 support thanks to sunnyhaibin! +* Kia Sportage Hybrid 2023 support thanks to sunnyhaibin! +* Kia Stinger 2022 support thanks to sunnyhaibin! + Version 0.8.16 (2022-08-26) ======================== * New driving model @@ -73,7 +106,7 @@ Version 0.8.14 (2022-06-01) Version 0.8.13 (2022-02-18) ======================== * Improved driver monitoring - * Retuned driver pose learner for relaxed driving positions + * Re-tuned driver pose learner for relaxed driving positions * Added reliance on driving model to be more scene adaptive * Matched strictness between comma two and comma three * Improved performance in turns by compensating for the road bank angle @@ -219,7 +252,7 @@ Version 0.8.4 (2021-05-17) * Delay controls start until system is ready * Fuzzy car identification, enabled with Community Features toggle * Localizer optimized for increased precision and less CPU usage - * Retuned lateral control to be more aggressive when model is confident + * Re-tuned lateral control to be more aggressive when model is confident * Toyota Mirai 2021 support * Lexus NX 300 2020 support thanks to goesreallyfast! * Volkswagen Atlas 2018-19 support thanks to jyoung8607! @@ -384,7 +417,7 @@ Version 0.7 (2019-12-13) * Improve GM longitudinal control: proper computations for 15Hz radar * Move GM port, Toyota with DSU removed, comma pedal in community features; toggle switch required * Remove upload over cellular toggle: only upload qlog and qcamera files if not on wifi - * Refactor Panda code towards ISO26262 and SIL2 compliancy + * Refactor Panda code towards ISO26262 and SIL2 compliance * Forward stock FCW for Honda Nidec * Volkswagen port now standard: comma Harness intercepts stock camera diff --git a/SConstruct b/SConstruct index 49685b19b..74680c059 100644 --- a/SConstruct +++ b/SConstruct @@ -10,10 +10,6 @@ AGNOS = TICI Decider('MD5-timestamp') -AddOption('--test', - action='store_true', - help='build test files') - AddOption('--extras', action='store_true', help='build misc extras, like setup and installer files') @@ -53,6 +49,17 @@ AddOption('--no-thneed', dest='no_thneed', help='avoid using thneed') +AddOption('--pc-thneed', + action='store_true', + dest='pc_thneed', + help='use thneed on pc') + +AddOption('--no-test', + action='store_false', + dest='test', + default=os.path.islink(Dir('#laika/').abspath), + help='skip building test files') + real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() if platform.system() == "Darwin": arch = "Darwin" @@ -60,7 +67,6 @@ if platform.system() == "Darwin": if arch == "aarch64" and AGNOS: arch = "larch64" -USE_WEBCAM = os.getenv("USE_WEBCAM") is not None lenv = { "PATH": os.environ['PATH'], @@ -69,7 +75,7 @@ lenv = { "ACADOS_SOURCE_DIR": Dir("#third_party/acados/include/acados").abspath, "ACADOS_PYTHON_INTERFACE_PATH": Dir("#pyextra/acados_template").abspath, - "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer", + "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer" } rpath = lenv["LD_LIBRARY_PATH"].copy() @@ -93,9 +99,6 @@ if arch == "larch64": "#third_party/libyuv/larch64/lib", "/usr/lib/aarch64-linux-gnu" ] - cpppath += [ - "#system/camerad/include", - ] cflags = ["-DQCOM2", "-mcpu=cortex-a57"] cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"] rpath += ["/usr/local/lib"] @@ -106,6 +109,9 @@ else: # MacOS if arch == "Darwin": + if real_arch == "x86_64": + lenv["TERA_PATH"] = Dir("#").abspath + f"/third_party/acados/Darwin_x86_64/t_renderer" + brew_prefix = subprocess.check_output(['brew', '--prefix'], encoding='utf8').strip() yuv_dir = "mac" if real_arch != "arm64" else "mac_arm64" libpath = [ @@ -114,9 +120,13 @@ else: f"{brew_prefix}/Library", f"{brew_prefix}/opt/openssl/lib", f"{brew_prefix}/Cellar", - f"#third_party/acados/{arch}/lib", "/System/Library/Frameworks/OpenGL.framework/Libraries", ] + if real_arch == "x86_64": + libpath.append(f"#third_party/acados/Darwin_x86_64/lib") + else: + libpath.append(f"#third_party/acados/{arch}/lib") + cflags += ["-DGL_SILENCE_DEPRECATION"] cxxflags += ["-DGL_SILENCE_DEPRECATION"] cpppath += [ @@ -253,7 +263,7 @@ def abspath(x): # rpath works elsewhere return x[0].path.rsplit("/", 1)[1][:-3] -# Cython build enviroment +# Cython build environment py_include = sysconfig.get_paths()['include'] envCython = env.Clone() envCython["CPPPATH"] += [py_include, np.get_include()] @@ -317,6 +327,7 @@ qt_flags = [ qt_env['CXXFLAGS'] += qt_flags qt_env['LIBPATH'] += ['#selfdrive/ui'] qt_env['LIBS'] = qt_libs +qt_env['QT_MOCHPREFIX'] = cache_dir + '/moc_files/moc_' if GetOption("clazy"): checks = [ @@ -329,7 +340,7 @@ if GetOption("clazy"): qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0] qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks) -Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM') +Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED') SConscript(['common/SConscript']) Import('_common', '_gpucommon') @@ -421,7 +432,12 @@ SConscript(['selfdrive/sensord/SConscript']) SConscript(['selfdrive/ui/SConscript']) SConscript(['selfdrive/navd/SConscript']) -SConscript(['tools/replay/SConscript']) +if arch in ['x86_64', 'Darwin'] or GetOption('extras'): + SConscript(['tools/replay/SConscript']) + + opendbc = abspath([File('opendbc/can/libdbc.so')]) + Export('opendbc') + SConscript(['tools/cabana/SConscript']) if GetOption('test'): SConscript('panda/tests/safety/SConscript') diff --git a/cereal/car.capnp b/cereal/car.capnp index 39bb496c6..1b68892d1 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -17,7 +17,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { immediateDisable @6 :Bool; preEnable @7 :Bool; permanent @8 :Bool; # alerts presented regardless of openpilot state - override @9 :Bool; + overrideLateral @10 :Bool; + overrideLongitudinal @9 :Bool; enum EventName @0xbaa8c5d505f727de { canError @0; @@ -35,6 +36,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { pedalPressed @13; # exits active state pedalPressedPreEnable @73; # added during pre-enable state for either pedal gasPressedOverride @108; # added when user is pressing gas with no disengage on gas + steerOverride @114; cruiseDisabled @14; speedTooLow @17; outOfSpace @18; @@ -64,6 +66,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { lowBattery @48; vehicleModelInvalid @50; accFaulted @51; + accFaultedTemp @115; sensorDataInvalid @52; commIssue @53; commIssueAvgFreq @109; @@ -165,6 +168,7 @@ struct CarState { # brake pedal, 0.0-1.0 brake @5 :Float32; # this is user pedal only brakePressed @6 :Bool; # this is user pedal only + regenBraking @45 :Bool; # this is user pedal only parkingBrake @39 :Bool; brakeHoldActive @38 :Bool; @@ -340,8 +344,7 @@ struct CarControl { off @0; pid @1; stopping @2; - - startingDEPRECATED @3; + starting @3; } } @@ -349,6 +352,7 @@ struct CarControl { struct CruiseControl { cancel @0: Bool; resume @1: Bool; + override @4: Bool; speedOverrideDEPRECATED @2: Float32; accelOverrideDEPRECATED @3: Float32; } @@ -414,23 +418,18 @@ struct CarParams { enableGasInterceptor @2 :Bool; pcmCruise @3 :Bool; # is openpilot's state tied to the PCM's cruise state? enableDsu @5 :Bool; # driving support unit - enableApgs @6 :Bool; # advanced parking guidance system enableBsm @56 :Bool; # blind spot monitoring flags @64 :UInt32; # flags for car specific quirks + experimentalLongitudinalAvailable @71 :Bool; minEnableSpeed @7 :Float32; minSteerSpeed @8 :Float32; - maxSteeringAngleDeg @54 :Float32; safetyConfigs @62 :List(SafetyConfig); alternativeExperience @65 :Int16; # panda flag for features like no disengage on gas - maxLateralAccel @68 :Float32; - steerMaxBPDEPRECATED @11 :List(Float32); - steerMaxVDEPRECATED @12 :List(Float32); - gasMaxBPDEPRECATED @13 :List(Float32); - gasMaxVDEPRECATED @14 :List(Float32); - brakeMaxBPDEPRECATED @15 :List(Float32); - brakeMaxVDEPRECATED @16 :List(Float32); + # Car docs fields + maxLateralAccel @68 :Float32; + autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically # things about the car in the manual mass @17 :Float32; # [kg] curb weight: all fluids no cargo @@ -458,12 +457,13 @@ struct CarParams { vEgoStopping @29 :Float32; # Speed at which the car goes into stopping state vEgoStarting @59 :Float32; # Speed at which the car goes into starting state - directAccelControl @30 :Bool; # Does the car have direct accel control or just gas/brake - stoppingControl @31 :Bool; # Does the car allows full control even at lows speeds when stopping - stopAccel @60 :Float32; # Required acceleraton to keep vehicle stationary + stoppingControl @31 :Bool; # Does the car allow full control even at lows speeds when stopping steerControlType @34 :SteerControlType; radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN + stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary stoppingDecelRate @52 :Float32; # m/s^2/s while trying to stop + startAccel @32 :Float32; # Required acceleration to get car moving + startingState @70 :Bool; # Does this car make use of special starting state steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds longitudinalActuatorDelayLowerBound @61 :Float32; # Gas/Brake actuator delay in seconds, lower bound @@ -507,6 +507,8 @@ struct CarParams { friction @3 :Float32; kf @4 :Float32; steeringAngleDeadzoneDeg @5 :Float32; + latAccelFactor @6 :Float32; + latAccelOffset @7 :Float32; } struct LongitudinalPIDTuning { @@ -575,8 +577,8 @@ struct CarParams { subaruLegacy @22; # pre-Global platform hyundaiLegacy @23; hyundaiCommunity @24; - stellantis @25; - faw @26; + stellantisDEPRECATED @25; # Consolidated with Chrysler; may be recycled for the next new model + hongqi @26; body @27; hyundaiCanfd @28; } @@ -607,7 +609,7 @@ struct CarParams { enum Ecu { eps @0; - esp @1; + abs @1; fwdRadar @2; fwdCamera @3; engine @4; @@ -617,6 +619,10 @@ struct CarParams { gateway @10; # can gateway hud @11; # heads up display combinationMeter @12; # instrument cluster + electricBrakeBooster @15; + shiftByWire @16; + adas @19; + cornerRadar @21; # Toyota only dsu @6; @@ -625,8 +631,12 @@ struct CarParams { # Honda only vsa @13; # Vehicle Stability Assist programmedFuelInjection @14; - electricBrakeBooster @15; - shiftByWire @16; + + # Chrysler only + hcp @18; # Hybrid Control Processor + + # Hyundai only + vcu @20; # Vehicle (Motor) Control Unit debug @17; } @@ -643,6 +653,7 @@ struct CarParams { } enableCameraDEPRECATED @4 :Bool; + enableApgsDEPRECATED @6 :Bool; steerRateCostDEPRECATED @33 :Float32; isPandaBlackDEPRECATED @39 :Bool; hasStockCameraDEPRECATED @57 :Bool; @@ -650,7 +661,14 @@ struct CarParams { safetyModelDEPRECATED @9 :SafetyModel; safetyModelPassiveDEPRECATED @42 :SafetyModel = silent; minSpeedCanDEPRECATED @51 :Float32; - startAccelDEPRECATED @32 :Float32; communityFeatureDEPRECATED @46: Bool; startingAccelRateDEPRECATED @53 :Float32; + steerMaxBPDEPRECATED @11 :List(Float32); + steerMaxVDEPRECATED @12 :List(Float32); + gasMaxBPDEPRECATED @13 :List(Float32); + gasMaxVDEPRECATED @14 :List(Float32); + brakeMaxBPDEPRECATED @15 :List(Float32); + brakeMaxVDEPRECATED @16 :List(Float32); + directAccelControlDEPRECATED @30 :Bool; + maxSteeringAngleDegDEPRECATED @54 :Float32; } diff --git a/cereal/log.capnp b/cereal/log.capnp index 8e0fd9dd6..72dcef700 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -137,6 +137,7 @@ struct FrameData { gain @15 :Float32; # This includes highConversionGain if enabled measuredGreyFraction @21 :Float32; targetGreyFraction @22 :Float32; + exposureValPercent @27 :Float32; # Focus lensPos @11 :Int32; @@ -150,10 +151,7 @@ struct FrameData { transform @10 :List(Float32); - androidCaptureResult @9 :AndroidCaptureResult; - image @6 :Data; - globalGainDEPRECATED @5 :Int32; temperaturesC @24 :List(Float32); @@ -164,6 +162,15 @@ struct FrameData { front @3; } + sensor @26 :ImageSensor; + enum ImageSensor { + unknown @0; + ar0321 @1; + ox03c10 @2; + } + + globalGainDEPRECATED @5 :Int32; + androidCaptureResultDEPRECATED @9 :AndroidCaptureResult; struct AndroidCaptureResult { sensitivity @0 :Int32; frameDuration @1 :Int64; @@ -220,9 +227,9 @@ struct SensorEventData { fiber @2; velodyne @3; # Velodyne IMU bno055 @4; # Bosch accelerometer - lsm6ds3 @5; # accelerometer (c2) - bmp280 @6; # barometer (c2) - mmc3416x @7; # magnetometer (c2) + lsm6ds3 @5; # includes LSM6DS3 and LSM6DS3TR, TR = tape reel + bmp280 @6; # barometer + mmc3416x @7; # magnetometer bmx055 @8; rpr0521 @9; lsm6ds3trc @10; @@ -290,7 +297,6 @@ struct CanData { } struct DeviceState @0xa4d8b5af2aa492eb { - usbOnline @12 :Bool; networkType @22 :NetworkType; networkInfo @31 :NetworkInfo; networkStrength @24 :NetworkStrength; @@ -308,10 +314,6 @@ struct DeviceState @0xa4d8b5af2aa492eb { cpuUsagePercent @34 :List(Int8); # per-core cpu usage # power - batteryPercent @8 :Int16; - batteryCurrent @15 :Int32; - chargingError @17 :Bool; - chargingDisabled @18 :Bool; offroadPowerUsageUwh @23 :UInt32; carBatteryCapacityUwh @25 :UInt32; powerDrawW @40 :Float32; @@ -388,31 +390,44 @@ struct DeviceState @0xa4d8b5af2aa492eb { batteryStatusDEPRECATED @9 :Text; batteryVoltageDEPRECATED @16 :Int32; batteryTempCDEPRECATED @29 :Float32; + batteryPercentDEPRECATED @8 :Int16; + batteryCurrentDEPRECATED @15 :Int32; + chargingErrorDEPRECATED @17 :Bool; + chargingDisabledDEPRECATED @18 :Bool; + usbOnlineDEPRECATED @12 :Bool; } struct PandaState @0xa7649e2575e4591e { ignitionLine @2 :Bool; - controlsAllowed @3 :Bool; gasInterceptorDetected @4 :Bool; - canSendErrs @7 :UInt32; - canFwdErrs @8 :UInt32; - canRxErrs @19 :UInt32; + rxBufferOverflow @7 :UInt32; + txBufferOverflow @8 :UInt32; gmlanSendErrs @9 :UInt32; pandaType @10 :PandaType; ignitionCan @13 :Bool; - safetyModel @14 :Car.CarParams.SafetyModel; - safetyParam @27 :UInt16; - alternativeExperience @23 :Int16; faultStatus @15 :FaultStatus; powerSaveEnabled @16 :Bool; uptime @17 :UInt32; faults @18 :List(FaultType); harnessStatus @21 :HarnessStatus; heartbeatLost @22 :Bool; - blockedCnt @24 :UInt32; interruptLoad @25 :Float32; fanPower @28 :UInt8; + # can health + canState0 @29 :PandaCanState; + canState1 @30 :PandaCanState; + canState2 @31 :PandaCanState; + + # safety stuff + controlsAllowed @3 :Bool; + safetyRxInvalid @19 :UInt32; + safetyTxBlocked @24 :UInt32; + safetyModel @14 :Car.CarParams.SafetyModel; + safetyParam @27 :UInt16; + alternativeExperience @23 :Int16; + safetyRxChecksInvalid @32 :Bool; + enum FaultStatus { none @0; faultTemp @1; @@ -443,6 +458,8 @@ struct PandaState @0xa7649e2575e4591e { interruptRateClockSource @20; interruptRateTick @21; interruptRateExti @22; + interruptRateSpi @23; + interruptRateUart7 @24; # Update max fault type in boardd when adding faults } @@ -455,6 +472,8 @@ struct PandaState @0xa7649e2575e4591e { uno @5; dos @6; redPanda @7; + redPandaV2 @8; + tres @9; } enum HarnessStatus { @@ -468,9 +487,44 @@ struct PandaState @0xa7649e2575e4591e { currentDEPRECATED @1 :UInt32; hasGpsDEPRECATED @6 :Bool; fanSpeedRpmDEPRECATED @11 :UInt16; - usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerMode; + usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerModeDEPRECATED; safetyParamDEPRECATED @20 :Int16; safetyParam2DEPRECATED @26 :UInt32; + + struct PandaCanState { + busOff @0 :Bool; + busOffCnt @1 :UInt32; + errorWarning @2 :Bool; + errorPassive @3 :Bool; + lastError @4 :LecErrorCode; + lastStoredError @5 :LecErrorCode; + lastDataError @6 :LecErrorCode; + lastDataStoredError @7 :LecErrorCode; + receiveErrorCnt @8 :UInt8; + transmitErrorCnt @9 :UInt8; + totalErrorCnt @10 :UInt32; + totalTxLostCnt @11 :UInt32; + totalRxLostCnt @12 :UInt32; + totalTxCnt @13 :UInt32; + totalRxCnt @14 :UInt32; + totalFwdCnt @15 :UInt32; + canSpeed @16 :UInt16; + canDataSpeed @17 :UInt16; + canfdEnabled @18 :Bool; + brsEnabled @19 :Bool; + canfdNonIso @20 :Bool; + + enum LecErrorCode { + noError @0; + stuffError @1; + formError @2; + ackError @3; + bit1Error @4; + bit0Error @5; + crcError @6; + noChange @7; + } + } } struct PeripheralState { @@ -478,9 +532,9 @@ struct PeripheralState { voltage @1 :UInt32; current @2 :UInt32; fanSpeedRpm @3 :UInt16; - usbPowerMode @4 :UsbPowerMode; - enum UsbPowerMode @0xa8883583b32c9877 { + usbPowerModeDEPRECATED @4 :UsbPowerModeDEPRECATED; + enum UsbPowerModeDEPRECATED @0xa8883583b32c9877 { none @0; client @1; cdp @2; @@ -538,6 +592,7 @@ struct LiveCalibrationData { # the direction of travel vector in device frame rpyCalib @7 :List(Float32); rpyCalibSpread @8 :List(Float32); + wideFromDeviceEuler @10 :List(Float32); warpMatrixDEPRECATED @0 :List(Float32); warpMatrix2DEPRECATED @5 :List(Float32); @@ -567,6 +622,8 @@ struct ControlsState @0x97ff69c53601abf1 { enabled @19 :Bool; active @36 :Bool; + experimentalMode @64 :Bool; + longControlState @30 :Car.CarControl.Actuators.LongControlState; vPid @2 :Float32; vTargetLead @3 :Float32; @@ -608,7 +665,7 @@ struct ControlsState @0x97ff69c53601abf1 { preEnabled @1; enabled @2; softDisabling @3; - overriding @4; + overriding @4; # superset of overriding with steering or accelerator } enum AlertStatus { @@ -753,6 +810,9 @@ struct ModelDataV2 { meta @12 :MetaData; + # Model perceived motion + temporalPose @21 :Pose; + # All SI units and in device frame struct XYZTData { x @0 :List(Float32); @@ -816,6 +876,13 @@ struct ModelDataV2 { brake4MetersPerSecondSquaredProbs @5 :List(Float32); brake5MetersPerSecondSquaredProbs @6 :List(Float32); } + + struct Pose { + trans @0 :List(Float32); # m/s in device frame + rot @1 :List(Float32); # rad/s in device frame + transStd @2 :List(Float32); # std m/s in device frame + rotStd @3 :List(Float32); # std rad/s in device frame + } } struct EncodeIndex { @@ -917,11 +984,11 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { struct LateralPlan @0xe1e9318e2ae8b51e { modelMonoTime @31 :UInt64; - laneWidth @0 :Float32; - lProb @5 :Float32; - rProb @7 :Float32; + laneWidthDEPRECATED @0 :Float32; + lProbDEPRECATED @5 :Float32; + rProbDEPRECATED @7 :Float32; dPathPoints @20 :List(Float32); - dProb @21 :Float32; + dProbDEPRECATED @21 :Float32; mpcSolutionValid @9 :Bool; desire @17 :Desire; @@ -1136,6 +1203,7 @@ struct GnssMeasurements { # Different ultra-rapid files: nasaUltraRapid @1; glonassIacUltraRapid @2; + qcom @3; } } @@ -1198,7 +1266,7 @@ struct UbloxGnss { carrierPhaseValid @1 :Bool; # half cycle valid halfCycleValid @2 :Bool; - # half sycle subtracted from phase + # half cycle subtracted from phase halfCycleSubtracted @3 :Bool; } } @@ -1329,7 +1397,7 @@ struct QcomGnss @0xde94674b07ae51c1 { unknown3 @3; unknown4 @4; unknown5 @5; - unknown6 @6; + sbas @6; } enum SVObservationState @0xe81e829a0d6c83e9 { @@ -1752,6 +1820,22 @@ struct LiveParametersData { roll @14 :Float32; } +struct LiveTorqueParametersData { + liveValid @0 :Bool; + latAccelFactorRaw @1 :Float32; + latAccelOffsetRaw @2 :Float32; + frictionCoefficientRaw @3 :Float32; + latAccelFactorFiltered @4 :Float32; + latAccelOffsetFiltered @5 :Float32; + frictionCoefficientFiltered @6 :Float32; + totalBucketPoints @7 :Float32; + decay @8 :Float32; + maxResets @9 :Float32; + points @10 :List(List(Float32)); + version @11 :Int32; + useParams @12 :Bool; +} + struct LiveMapDataDEPRECATED { speedLimitValid @0 :Bool; speedLimit @1 :Float32; @@ -1779,6 +1863,8 @@ struct CameraOdometry { rot @1 :List(Float32); # rad/s in device frame transStd @2 :List(Float32); # std m/s in device frame rotStd @3 :List(Float32); # std rad/s in device frame + wideFromDeviceEuler @6 :List(Float32); + wideFromDeviceEulerStd @7 :List(Float32); } struct Sentinel { @@ -1792,6 +1878,10 @@ struct Sentinel { signal @1 :Int32; } +struct UIDebug { + drawTimeMillis @0 :Float32; +} + struct ManagerState { processes @0 :List(ProcessState); @@ -1887,7 +1977,13 @@ struct Event { gpsNMEA @3 :GPSNMEAData; can @5 :List(CanData); controlsState @7 :ControlsState; - sensorEvents @11 :List(SensorEventData); + gyroscope @99 :SensorEventData; + gyroscope2 @100 :SensorEventData; + accelerometer @98 :SensorEventData; + accelerometer2 @101 :SensorEventData; + magnetometer @95 :SensorEventData; + lightSensor @96 :SensorEventData; + temperatureSensor @97 :SensorEventData; pandaStates @81 :List(PandaState); peripheralState @80 :PeripheralState; radarState @13 :RadarState; @@ -1905,6 +2001,7 @@ struct Event { gpsLocation @21 :GpsLocationData; gnssMeasurements @91 :GnssMeasurements; liveParameters @61 :LiveParametersData; + liveTorqueParameters @94 :LiveTorqueParametersData; cameraOdometry @63 :CameraOdometry; thumbnail @66: Thumbnail; carEvents @68: List(Car.CarEvent); @@ -1938,8 +2035,9 @@ struct Event { navRoute @83 :NavRoute; navThumbnail @84: Thumbnail; - # user flags + # UI services userFlag @93 :UserFlag; + uiDebug @102 :UIDebug; # *********** debug *********** testJoystick @52 :Joystick; @@ -1985,5 +2083,6 @@ struct Event { uiLayoutStateDEPRECATED @57 :Legacy.UiLayoutState; pandaStateDEPRECATED @12 :PandaState; driverStateDEPRECATED @59 :DriverStateDEPRECATED; + sensorEventsDEPRECATED @11 :List(SensorEventData); } } diff --git a/cereal/messaging/msgq.h b/cereal/messaging/msgq.h index 90a20acdd..8af069d9c 100644 --- a/cereal/messaging/msgq.h +++ b/cereal/messaging/msgq.h @@ -5,7 +5,7 @@ #include #define DEFAULT_SEGMENT_SIZE (10 * 1024 * 1024) -#define NUM_READERS 10 +#define NUM_READERS 12 #define ALIGN(n) ((n + (8 - 1)) & -8) #define UNPACK64(higher, lower, input) do {uint64_t tmp = input; higher = tmp >> 32; lower = tmp & 0xFFFFFFFF;} while (0) diff --git a/cereal/services.py b/cereal/services.py index a64d8ca8b..4a2f83dcf 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -21,7 +21,13 @@ class Service: services = { # service: (should_log, frequency, qlog decimation (optional)) # note: the "EncodeIdx" packets will still be in the log - "sensorEvents": (True, 100., 100), + "gyroscope": (True, 104., 104), + "gyroscope2": (True, 100., 100), + "accelerometer": (True, 104., 104), + "accelerometer2": (True, 100., 100), + "magnetometer": (True, 100., 100), + "lightSensor": (True, 100., 100), + "temperatureSensor": (True, 100., 100), "gpsNMEA": (True, 9.), "deviceState": (True, 2., 1), "can": (True, 100.), @@ -35,6 +41,7 @@ services = { "logMessage": (True, 0.), "errorLogMessage": (True, 0., 1), "liveCalibration": (True, 4., 4), + "liveTorqueParameters": (True, 4., 1), "androidLog": (True, 0.), "carState": (True, 100., 10), "carControl": (True, 100., 10), @@ -71,6 +78,7 @@ services = { "userFlag": (True, 0., 1), # debug + "uiDebug": (True, 0., 1), "testJoystick": (True, 0.), "roadEncodeData": (False, 20.), "driverEncodeData": (False, 20.), diff --git a/common/clutil.cc b/common/clutil.cc index b8f9fde4c..9d3447d80 100644 --- a/common/clutil.cc +++ b/common/clutil.cc @@ -78,7 +78,8 @@ cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const ch } cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args) { - cl_program prg = CL_CHECK_ERR(clCreateProgramWithSource(ctx, 1, (const char*[]){src.c_str()}, NULL, &err)); + const char *csrc = src.c_str(); + cl_program prg = CL_CHECK_ERR(clCreateProgramWithSource(ctx, 1, &csrc, NULL, &err)); if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) { cl_print_build_errors(prg, device_id); assert(0); @@ -87,7 +88,7 @@ cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const } cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args) { - cl_program prg = CL_CHECK_ERR(clCreateProgramWithBinary(ctx, 1, &device_id, &length, (const uint8_t*[]){binary}, NULL, &err)); + cl_program prg = CL_CHECK_ERR(clCreateProgramWithBinary(ctx, 1, &device_id, &length, &binary, NULL, &err)); if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) { cl_print_build_errors(prg, device_id); assert(0); diff --git a/common/gpio.cc b/common/gpio.cc index 73ff1b3f5..9f5c211a4 100644 --- a/common/gpio.cc +++ b/common/gpio.cc @@ -4,11 +4,11 @@ #include #include +#include +#include #include "common/util.h" - -// We assume that all pins have already been exported on boot, -// and that we have permission to write to them. +#include "common/swaglog.h" int gpio_init(int pin_nr, bool output) { char pin_dir_path[50]; @@ -30,3 +30,36 @@ int gpio_set(int pin_nr, bool high) { } return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1); } + +int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { + + // Assumed that all interrupt pins are unexported and rights are given to + // read from gpiochip0. + std::string gpiochip_path = "/dev/gpiochip" + std::to_string(gpiochiop_id); + int fd = open(gpiochip_path.c_str(), O_RDONLY); + if (fd < 0) { + LOGE("Error opening gpiochip0 fd") + return -1; + } + + // Setup event + struct gpioevent_request rq; + rq.lineoffset = pin_nr; + rq.handleflags = GPIOHANDLE_REQUEST_INPUT; + + /* Requesting both edges as the data ready pulse from the lsm6ds sensor is + very short(75us) and is mostly detected as falling edge instead of rising. + So if it is detected as rising the following falling edge is skipped. */ + rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES; + + strncpy(rq.consumer_label, consumer_label, std::size(rq.consumer_label) - 1); + int ret = ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq); + if (ret == -1) { + LOGE("Unable to get line event from ioctl : %s", strerror(errno)); + close(fd); + return -1; + } + + close(fd); + return rq.fd; +} diff --git a/common/gpio.h b/common/gpio.h index e03001987..b2f67f8ba 100644 --- a/common/gpio.h +++ b/common/gpio.h @@ -8,6 +8,11 @@ #define GPIO_UBLOX_PWR_EN 34 #define GPIO_STM_RST_N 124 #define GPIO_STM_BOOT0 134 + #define GPIO_BMX_ACCEL_INT 21 + #define GPIO_BMX_GYRO_INT 23 + #define GPIO_BMX_MAGN_INT 87 + #define GPIO_LSM_INT 84 + #define GPIOCHIP_INT 0 #else #define GPIO_HUB_RST_N 0 #define GPIO_UBLOX_RST_N 0 @@ -15,7 +20,14 @@ #define GPIO_UBLOX_PWR_EN 0 #define GPIO_STM_RST_N 0 #define GPIO_STM_BOOT0 0 + #define GPIO_BMX_ACCEL_INT 0 + #define GPIO_BMX_GYRO_INT 0 + #define GPIO_BMX_MAGN_INT 0 + #define GPIO_LSM_INT 0 + #define GPIOCHIP_INT 0 #endif int gpio_init(int pin_nr, bool output); int gpio_set(int pin_nr, bool high); + +int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr); diff --git a/common/gpio.py b/common/gpio.py index 32e5ca86c..260f8898a 100644 --- a/common/gpio.py +++ b/common/gpio.py @@ -1,3 +1,5 @@ +from typing import Optional + def gpio_init(pin: int, output: bool) -> None: try: with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f: @@ -5,10 +7,19 @@ def gpio_init(pin: int, output: bool) -> None: except Exception as e: print(f"Failed to set gpio {pin} direction: {e}") - def gpio_set(pin: int, high: bool) -> None: try: with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f: f.write(b"1" if high else b"0") except Exception as e: print(f"Failed to set gpio {pin} value: {e}") + +def gpio_read(pin: int) -> Optional[bool]: + val = None + try: + with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f: + val = bool(int(f.read().strip())) + except Exception as e: + print(f"Failed to set gpio {pin} value: {e}") + + return val diff --git a/common/modeldata.h b/common/modeldata.h index e13840d53..a00d3d49d 100644 --- a/common/modeldata.h +++ b/common/modeldata.h @@ -4,7 +4,7 @@ #include "common/mat.h" #include "system/hardware/hw.h" -const int TRAJECTORY_SIZE = 33; +const int TRAJECTORY_SIZE = 33; const int LAT_MPC_N = 16; const int LON_MPC_N = 32; const float MIN_DRAW_DISTANCE = 10.0; diff --git a/common/params.cc b/common/params.cc index f89e7ff00..9e3e32d58 100644 --- a/common/params.cc +++ b/common/params.cc @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -84,12 +85,16 @@ private: std::unordered_map keys = { {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, + {"AssistNowToken", PERSISTENT}, {"AthenadPid", PERSISTENT}, {"AthenadUploadQueue", PERSISTENT}, {"CalibrationParams", PERSISTENT}, + {"CameraDebugExpGain", CLEAR_ON_MANAGER_START}, + {"CameraDebugExpTime", CLEAR_ON_MANAGER_START}, {"CarBatteryCapacity", PERSISTENT}, {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CarParamsCache", CLEAR_ON_MANAGER_START}, + {"CarParamsPersistent", PERSISTENT}, {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CompletedTrainingVersion", PERSISTENT}, {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, @@ -97,8 +102,9 @@ std::unordered_map keys = { {"DashcamOverride", PERSISTENT}, {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"DisablePowerDown", PERSISTENT}, - {"DisableRadar_Allow", PERSISTENT}, - {"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB + {"ExperimentalMode", PERSISTENT}, + {"ExperimentalModeConfirmed", PERSISTENT}, + {"ExperimentalLongitudinalEnabled", PERSISTENT}, // WARNING: THIS MAY DISABLE AEB {"DisableUpdates", PERSISTENT}, {"DisengageOnAccelerator", PERSISTENT}, {"DongleId", PERSISTENT}, @@ -113,6 +119,7 @@ std::unordered_map keys = { {"GithubUsername", PERSISTENT}, {"GitRemote", PERSISTENT}, {"GsmApn", PERSISTENT}, + {"GsmMetered", PERSISTENT}, {"GsmRoaming", PERSISTENT}, {"HardwareSerial", PERSISTENT}, {"HasAcceptedTerms", PERSISTENT}, @@ -139,6 +146,8 @@ std::unordered_map keys = { {"LastUpdateException", CLEAR_ON_MANAGER_START}, {"LastUpdateTime", PERSISTENT}, {"LiveParameters", PERSISTENT}, + {"LiveTorqueCarParams", PERSISTENT}, + {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"NavSettingTime24h", PERSISTENT}, {"NavSettingLeftSide", PERSISTENT}, @@ -150,18 +159,25 @@ std::unordered_map keys = { {"PrimeType", PERSISTENT}, {"RecordFront", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet - {"ReleaseNotes", PERSISTENT}, {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"ShouldDoUpdate", CLEAR_ON_MANAGER_START}, {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"SshEnabled", PERSISTENT}, {"SubscriberInfo", PERSISTENT}, - {"SwitchToBranch", CLEAR_ON_MANAGER_START}, {"TermsVersion", PERSISTENT}, {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, + {"UbloxAvailable", PERSISTENT}, {"UpdateAvailable", CLEAR_ON_MANAGER_START}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, + {"UpdaterState", CLEAR_ON_MANAGER_START}, + {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, + {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, + {"UpdaterAvailableBranches", CLEAR_ON_MANAGER_START}, + {"UpdaterCurrentDescription", CLEAR_ON_MANAGER_START}, + {"UpdaterCurrentReleaseNotes", CLEAR_ON_MANAGER_START}, + {"UpdaterNewDescription", CLEAR_ON_MANAGER_START}, + {"UpdaterNewReleaseNotes", CLEAR_ON_MANAGER_START}, {"Version", PERSISTENT}, {"VisionRadarToggle", PERSISTENT}, {"WideCameraOnly", PERSISTENT}, @@ -187,10 +203,16 @@ std::unordered_map keys = { Params::Params(const std::string &path) { - const char* env = std::getenv("OPENPILOT_PREFIX"); - prefix = env ? "/" + std::string(env) : "/d"; - std::string default_param_path = ensure_params_path(prefix); - params_path = path.empty() ? default_param_path : ensure_params_path(prefix, path); + prefix = "/" + util::getenv("OPENPILOT_PREFIX", "d"); + params_path = ensure_params_path(prefix, path); +} + +std::vector Params::allKeys() const { + std::vector ret; + for (auto &p : keys) { + ret.push_back(p.first); + } + return ret; } bool Params::checkKey(const std::string &key) { @@ -278,10 +300,13 @@ std::map Params::readAll() { void Params::clearAll(ParamKeyType key_type) { FileLock file_lock(params_path + "/.lock"); - std::string path; - for (auto &[key, type] : keys) { - if (type & key_type) { - unlink(getParamPath(key).c_str()); + 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()); + } } } diff --git a/common/params.h b/common/params.h index aa4b1d7af..aecb3ee47 100644 --- a/common/params.h +++ b/common/params.h @@ -2,6 +2,7 @@ #include #include +#include enum ParamKeyType { PERSISTENT = 0x02, @@ -15,6 +16,7 @@ enum ParamKeyType { class Params { public: Params(const std::string &path = {}); + std::vector allKeys() const; bool checkKey(const std::string &key); ParamKeyType getKeyType(const std::string &key); inline std::string getParamPath(const std::string &key = {}) { @@ -27,8 +29,8 @@ public: // helpers for reading values std::string get(const std::string &key, bool block = false); - inline bool getBool(const std::string &key) { - return get(key) == "1"; + inline bool getBool(const std::string &key, bool block = false) { + return get(key, block) == "1"; } std::map readAll(); diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 36c8cf777..9d8933609 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -2,6 +2,7 @@ # cython: language_level = 3 from libcpp cimport bool from libcpp.string cimport string +from libcpp.vector cimport vector import threading cdef extern from "common/params.h": @@ -15,13 +16,14 @@ cdef extern from "common/params.h": cdef cppclass c_Params "Params": c_Params(string) nogil string get(string, bool) nogil - bool getBool(string) nogil + bool getBool(string, bool) nogil int remove(string) nogil int put(string, string) nogil int putBool(string, bool) nogil bool checkKey(string) nogil string getParamPath(string) nogil void clearAll(ParamKeyType) + vector[string] allKeys() def ensure_bytes(v): @@ -66,11 +68,11 @@ cdef class Params: return val if encoding is None else val.decode(encoding) - def get_bool(self, key): + def get_bool(self, key, bool block=False): cdef string k = self.check_key(key) cdef bool r with nogil: - r = self.p.getBool(k) + r = self.p.getBool(k, block) return r def put(self, key, dat): @@ -90,7 +92,7 @@ cdef class Params: with nogil: self.p.putBool(k, val) - def delete(self, key): + def remove(self, key): cdef string k = self.check_key(key) with nogil: self.p.remove(k) @@ -99,6 +101,9 @@ cdef class Params: cdef string key_bytes = ensure_bytes(key) return self.p.getParamPath(key_bytes).decode("utf-8") + def all_keys(self): + return self.p.allKeys() + def put_nonblocking(key, val, d=""): threading.Thread(target=lambda: Params(d).put(key, val)).start() diff --git a/common/realtime.py b/common/realtime.py index 8a79d8d39..7dd2eb98a 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -31,7 +31,7 @@ class Priority: def set_realtime_priority(level: int) -> None: if not PC: - os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) # type: ignore[attr-defined] # pylint: disable=no-member + os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) # pylint: disable=no-member def set_core_affinity(cores: List[int]) -> None: diff --git a/common/util.cc b/common/util.cc index 92add6399..010fe8a11 100644 --- a/common/util.cc +++ b/common/util.cc @@ -1,5 +1,6 @@ #include "common/util.h" +#include #include #include @@ -97,6 +98,22 @@ 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) { @@ -133,6 +150,14 @@ int safe_fflush(FILE *stream) { return ret; } +int safe_ioctl(int fd, unsigned long request, void *argp) { + int ret; + do { + ret = ioctl(fd, request, argp); + } while ((ret == -1) && (errno == EINTR)); + return ret; +} + std::string readlink(const std::string &path) { char buff[4096]; ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1); diff --git a/common/util.h b/common/util.h index f3a24723b..b46f7bde4 100644 --- a/common/util.h +++ b/common/util.h @@ -80,11 +80,13 @@ 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); size_t safe_fwrite(const void * ptr, size_t size, size_t count, FILE * stream); int safe_fflush(FILE *stream); +int safe_ioctl(int fd, unsigned long request, void *argp); std::string readlink(const std::string& path); bool file_exists(const std::string& fn); diff --git a/common/version.h b/common/version.h index bf1c58df1..74a56a7c1 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.8.16" +#define COMMA_VERSION "0.9.0" diff --git a/docs/CARS.md b/docs/CARS.md index 24be43e4f..59b814555 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -2,227 +2,238 @@ # Supported Cars -A supported vehicle is one that just works when you install a comma device. Every car performs differently with openpilot, but all supported cars should 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. -# 205 Supported Cars +# 215 Supported Cars -|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Harness| -|---|---|---|:---:|:---:|:---:|:---:|:---:| -|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Acura|RDX 2019-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Audi|A3 2014-19|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Audi|Q2 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Audi|Q3 2020-21|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Audi|RS3 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Audi|S3 2015-17|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Cadillac|Escalade ESV 2016[1](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|OBD-II| -|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|Stock|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|GM| -|Chevrolet|Silverado 1500 2020-21|Safety Package II|Stock|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|GM| -|Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|OBD-II| -|Chrysler|Pacifica 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|FCA| -|Chrysler|Pacifica 2019-20|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|FCA| -|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|FCA| -|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|FCA| -|Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|FCA| -|comma|body|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|None| -|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai F| -|Genesis|G70 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai F| -|Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Genesis|G90 2017-18|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|OBD-II| -|GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|GM| -|Honda|Accord 2018-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|Accord Hybrid 2018-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Honda|Civic 2019-21|All|Stock|0 mph|2 mph[2](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|Civic 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B| -|Honda|Civic Hatchback 2017-21|Honda Sensing|Stock|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|Civic Hatchback 2022|All|Stock|0 mph|0 mph|[![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)](##)|Honda Nidec| -|Honda|CR-V 2017-22|Honda Sensing|Stock|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|CR-V Hybrid 2017-19|Honda Sensing|Stock|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|e 2020|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Honda|Insight 2019-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|Inspire 2018|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Bosch A| -|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|Honda Nidec| -|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai B| -|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai K| -|Hyundai|Elantra Hybrid 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai K| -|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai J| -|Hyundai|Ioniq 5 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai Q| -|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Hyundai|Ioniq Electric 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC) & LFA|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Hyundai|Ioniq Plug-in Hybrid 2020-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Kona 2020|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai B| -|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![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)](##)|Hyundai O| -|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai I| -|Hyundai|Palisade 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Santa Fe 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai D| -|Hyundai|Santa Fe 2021-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Santa Fe Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Santa Fe Plug-in Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai E| -|Hyundai|Sonata 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Hyundai|Sonata Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Tucson Hybrid 2022|All|Stock|0 mph|0 mph|[![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)](##)|Hyundai E| -|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|FCA| -|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|FCA| -|Kia|Ceed 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai E| -|Kia|EV6 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai P| -|Kia|Forte 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai B| -|Kia|Forte 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai G| -|Kia|K5 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Kia|Niro Electric 2019|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Kia|Niro Electric 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai F| -|Kia|Niro Electric 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Kia|Niro Electric 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai F| -|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Kia|Niro Plug-in Hybrid 2018-19|Smart Cruise Control (SCC) & LKAS|openpilot|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Kia|Optima 2017|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai B| -|Kia|Optima 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai G| -|Kia|Seltos 2021|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Kia|Sorento 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Kia|Sorento 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai E| -|Kia|Stinger 2018-20|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Kia|Telluride 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Lexus|CT Hybrid 2017-18|Lexus Safety System+|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|ES Hybrid 2017-18|Lexus Safety System+|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|ES Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|NX 2018-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|NX Hybrid 2018-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|RX 2016-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|RX Hybrid 2016-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Mazda| -|Mazda|CX-9 2021-22|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|Mazda| -|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Nissan B| -|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Nissan A| -|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Nissan A| -|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Nissan A| -|Ram|1500 2019-22|Adaptive Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|Ram| -|SEAT|Ateca 2018|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|SEAT|Leon 2014-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Subaru A| -|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|Subaru A| -|Subaru|Crosstrek 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Subaru A| -|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Subaru A| -|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![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)](##)|Subaru A| -|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Subaru B| -|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Subaru B| -|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![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)](##)|Subaru A| -|Škoda|Kamiq 2021[5](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Škoda|Karoq 2019-21[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Škoda|Kodiaq 2018-19|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Škoda|Octavia 2015, 2018-19|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Škoda|Octavia RS 2016|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Škoda|Scala 2020|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Škoda|Superb 2015-18|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Avalon 2016|Toyota Safety Sense P|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Avalon 2017-18|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Avalon 2019-21|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Avalon Hybrid 2019-21|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|C-HR 2017-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Camry 2018-20|All|Stock|0 mph[4](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Camry 2021-22|All|openpilot|0 mph[4](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Camry Hybrid 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Corolla 2017-19|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Corolla Cross (Non-US only) 2020-21|All|openpilot|17 mph|0 mph|[![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)](##)|Toyota| -|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Highlander 2017-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Highlander 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Highlander Hybrid 2017-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Highlander Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Prius 2016|Toyota Safety Sense P|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Prius 2017-20|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Prius Prime 2017-20|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Prius v 2017|Toyota Safety Sense P|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 2016|Toyota Safety Sense P|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 2017-18|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 Hybrid 2017-18|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Sienna 2018-20|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Volkswagen|Arteon 2018-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Arteon eHybrid 2020-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Arteon R 2020-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Atlas 2018-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Atlas Cross Sport 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|California 2021[7](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Caravelle 2020[7](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|CC 2018-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|e-Golf 2014-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Golf 2015-20[8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Golf Alltrack 2015-19|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Golf GTD 2015-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Golf GTE 2015-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Golf GTI 2015-21|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Golf R 2015-19[8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Golf SportsVan 2015-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| -|Volkswagen|Jetta 2018-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Jetta GLI 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat 2015-22[6,7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat Alltrack 2015-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat GTE 2015-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Polo 2020-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Polo GTI 2020-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|T-Cross 2021[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|T-Roc 2021[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Taos 2022[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont 2018-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont Cross Sport 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont X 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Tiguan 2019-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Touran 2017|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|VW| +|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness| +|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| +|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,8](#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,8](#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,8](#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,8](#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,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 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|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|6 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| +|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-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|GV70 2022-23|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|6 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-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 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 2019|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 (with HDA II) 2022-23|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|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|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-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|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 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|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 (with HDA II) 2022|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|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|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 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|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|Sportage 2023|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|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)|Stock|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-22|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| +|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-22|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,8](#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,8](#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-21|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[6](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| +|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#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,8](#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,8](#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,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| +|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#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-21|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|Camry 2018-20|All|Stock|0 mph[5](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Camry 2021-22|All|openpilot|0 mph[5](#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-22|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|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-22|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-22|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,8](#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,8](#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,8](#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,8](#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,8](#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,8](#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,8](#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,8](#footnotes)|0 mph|0 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,8](#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,8](#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,8](#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,8](#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,8](#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,8](#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,8](#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,8](#footnotes)|0 mph|0 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,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Passat 2015-22[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#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,8](#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,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| +|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| +|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| +|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| +|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#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,8](#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,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -1Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
-22019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
-3When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
-4openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-5Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
-6Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
-7Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). In the interim, if your car has a J533 connector CAN gateway inside the dashboard, choose "VW J533 Development" from the vehicle drop-down for a suitable harness. (Some newer models are also observed to not have a J533 connector.)
-8Includes versions with extra rear cargo space (may be called Variant, Estate, SportWagen, Shooting Brake, etc.)
+1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `master-ci`. Using openpilot longitudinal may disable Automatic Emergency Braking (AEB).
+2When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
+3Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
+42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
+5openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
+6Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
+7Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
+8Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
+9Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store.
## Community Maintained Cars Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). @@ -230,13 +241,13 @@ Although they're not upstream, the community has openpilot running on other make # Don't see your car here? **openpilot can support many more cars than it currently does.** There are a few reasons your car may not be supported. -If your car doesn't fit into any of the incompatibility criteria here, then there's a good chance it can be supported! We're adding support for new cars all the time. We don't have a roadmap for car support, and in fact, most car support comes from users like you! +If your car doesn't fit into any of the incompatibility criteria here, then there's a good chance it can be supported! We're adding support for new cars all the time. **We don't have a roadmap for car support**, and in fact, most car support comes from users like you! ### Which cars are able to be supported? -openpilot uses the existing steering, gas, and brake interfaces in your car. If your car lacks any one of these interfaces, openpilot will not be able to control the car. If your car has any form of [LKAS](https://en.wikipedia.org/wiki/Automated_Lane_Keeping_Systems)/[LCA](https://en.wikipedia.org/wiki/Lane_centering) and [ACC](https://en.wikipedia.org/wiki/Adaptive_cruise_control), then it almost certainly has these interfaces. These interfaces generally started shipping on cars around 2016. +openpilot uses the existing steering, gas, and brake interfaces in your car. If your car lacks any one of these interfaces, openpilot will not be able to control the car. If your car has [ACC](https://en.wikipedia.org/wiki/Adaptive_cruise_control) and any form of [LKAS](https://en.wikipedia.org/wiki/Automated_Lane_Keeping_Systems)/[LCA](https://en.wikipedia.org/wiki/Lane_centering), then it almost certainly has these interfaces. These features generally started shipping on cars around 2016. Note that manufacturers will often make their own [marketing terms](https://en.wikipedia.org/wiki/Adaptive_cruise_control#Vehicle_models_supporting_adaptive_cruise_control) for these features, such as Hyundai's "Smart Cruise Control" branding of Adaptive Cruise Control. -If your car has the following packages or features, then it's a good candidate for support. If it does not, then it's unlikely able to be supported. +If your car has the following packages or features, then it's a good candidate for support. | Make | Required Package/Features | | ---- | ------------------------- | @@ -254,8 +265,9 @@ All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wi ### Toyota Security -Specific new Toyota models are shipping with a new message authentication method that openpilot does not yet support. -So far, this list includes: +openpilot does not yet support these Toyota models due to a new message authentication method. +[Vote](https://comma.ai/shop/products/vote) if you'd like to see openpilot support on these models. + * Toyota RAV4 Prime 2021+ * Toyota Sienna 2021+ * Toyota Venza 2021+ @@ -264,4 +276,4 @@ So far, this list includes: * Toyota Corolla Cross 2022+ (only US model) * Lexus NX 2022+ * Toyota bZ4x 2023+ -* Subaru Solterra 2023+ \ No newline at end of file +* Subaru Solterra 2023+ diff --git a/laika/ephemeris.py b/laika/ephemeris.py index 91caef2a4..12f9700f1 100644 --- a/laika/ephemeris.py +++ b/laika/ephemeris.py @@ -11,8 +11,9 @@ from datetime import datetime from math import sin, cos, sqrt, fabs, atan2 from .gps_time import GPSTime, utc_to_gpst -from .constants import SPEED_OF_LIGHT, SECS_IN_MIN, SECS_IN_HR, SECS_IN_DAY, EARTH_ROTATION_RATE, EARTH_GM -from .helpers import get_constellation +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 def read4(f, rinex_ver): @@ -74,7 +75,7 @@ class EphemerisType(IntEnum): FINAL_ORBIT = 1 RAPID_ORBIT = 2 ULTRA_RAPID_ORBIT = 3 - QCOM_POLY = 4 # Currently not supported + QCOM_POLY = 4 @staticmethod def all_orbits(): @@ -225,7 +226,8 @@ class GLONASSEphemeris(Ephemeris): class PolyEphemeris(Ephemeris): - def __init__(self, prn: str, data, epoch: GPSTime, ephem_type: EphemerisType, file_epoch: GPSTime, file_name: str, healthy=True, tgd=0): + def __init__(self, prn: str, data, epoch: GPSTime, ephem_type: EphemerisType, + file_epoch: GPSTime=None, file_name: str=None, healthy=True, tgd=0): super().__init__(prn, data, epoch, ephem_type, healthy, max_time_diff=SECS_IN_HR, file_epoch=file_epoch, file_name=file_name) self.tgd = tgd self.to_json() @@ -530,28 +532,25 @@ def parse_rinex_nav_msg_glonass(file_name): return ephems -''' +def parse_qcom_ephem(qcom_poly, current_week): + svId = qcom_poly.svId + data = qcom_poly + t0 = data.t0 + # fix glonass time + prn = get_prn_from_nmea_id(svId) + if prn == 'GLONASS': + # TODO should handle leap seconds better + epoch = GPSTime(current_week, (t0 + 3*SECS_IN_WEEK) % (SECS_IN_WEEK) + 18) + else: + epoch = GPSTime(current_week, t0) + poly_data = {} + poly_data['t0'] = epoch + poly_data['xyz'] = np.array([ + [data.xyz0[0], data.xyzN[0], data.xyzN[1], data.xyzN[2]], + [data.xyz0[1], data.xyzN[3], data.xyzN[4], data.xyzN[5]], + [data.xyz0[2], data.xyzN[6], data.xyzN[7], data.xyzN[8]] ]).T -def parse_qcom_ephems(qcom_polys, current_week): - ephems = [] - for qcom_poly in qcom_polys: - svId = qcom_poly.qcomGnss.drSvPoly.svId - data = qcom_poly.qcomGnss.drSvPoly - t0 = data.t0 - # fix glonass time - if get_constellation(svId) == 'GLONASS': - # TODO should handle leap seconds better - epoch = GPSTime(current_week, (t0 + 3*SECS_IN_WEEK) % (SECS_IN_WEEK) + 18) - else: - epoch = GPSTime(current_week, t0) - poly_data = {} - poly_data['t0'] = epoch - poly_data['x'] = [data.xyzN[2], data.xyzN[1], data.xyzN[0], data.xyz0[0]] - poly_data['y'] = [data.xyzN[5], data.xyzN[4], data.xyzN[3], data.xyz0[1]] - poly_data['z'] = [data.xyzN[8], data.xyzN[7], data.xyzN[6], data.xyz0[2]] - 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 - ephems.append(PolyEphemeris(svId, poly_data, epoch, eph_type=EphemerisType.QCOM_POLY)) - return ephems -''' + 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) diff --git a/laika/helpers.py b/laika/helpers.py index 4ebba76c1..a0480d633 100644 --- a/laika/helpers.py +++ b/laika/helpers.py @@ -32,11 +32,13 @@ class ConstellationId(IntEnum): def from_qcom_source(cls, report_source: int): if report_source == 0: return ConstellationId.GPS + if report_source == 1: + return ConstellationId.GLONASS + if report_source == 2: + return ConstellationId.BEIDOU if report_source == 6: return ConstellationId.SBAS - elif report_source == 1: - return ConstellationId.GLONASS - raise NotImplementedError('Only GPS (0), SBAS (1) and GLONASS (6) are supported from qcom, not:', {report_source}) + raise NotImplementedError('Only GPS (0), GLONASS (1), BEIDOU (2) and SBAS (6) are supported from qcom, not:', {report_source}) # From https://gpsd.gitlab.io/gpsd/NMEA.html#_satellite_ids diff --git a/launch_env.sh b/launch_env.sh index ac84d6dcb..3059ec268 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="5.2" + export AGNOS_VERSION="6.2" fi if [ -z "$PASSIVE" ]; then diff --git a/opendbc/can/common_dbc.h b/opendbc/can/common_dbc.h index e5321b927..4e2fa32fc 100644 --- a/opendbc/can/common_dbc.h +++ b/opendbc/can/common_dbc.h @@ -72,6 +72,17 @@ struct DBC { std::vector vals; }; +typedef struct ChecksumState { + int checksum_size; + int counter_size; + int checksum_start_bit; + int counter_start_bit; + bool little_endian; + SignalType checksum_type; + unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const std::vector &d); +} 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); 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 39ef16d32..16ca328e8 100644 --- a/opendbc/can/dbc.cc +++ b/opendbc/can/dbc.cc @@ -47,16 +47,6 @@ inline std::string& trim(std::string& s, const char* t = " \t\n\r\f\v") { return s.erase(0, s.find_first_not_of(t)); } -typedef struct ChecksumState { - int checksum_size; - int counter_size; - int checksum_start_bit; - int counter_start_bit; - bool little_endian; - SignalType checksum_type; - unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const std::vector &d); -} ChecksumState; - ChecksumState* get_checksum(const std::string& dbc_name) { ChecksumState* s = nullptr; if (startswith(dbc_name, {"honda_", "acura_"})) { @@ -107,14 +97,7 @@ void set_signal_type(Signal& s, ChecksumState* chk, const std::string& dbc_name, } } -DBC* dbc_parse(const std::string& dbc_path) { - std::ifstream infile(dbc_path); - if (!infile) return nullptr; - - const std::string dbc_name = std::filesystem::path(dbc_path).filename(); - - std::unique_ptr checksum(get_checksum(dbc_name)); - +DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, ChecksumState *checksum) { uint32_t address = 0; std::set address_set; std::set msg_name_set; @@ -134,7 +117,7 @@ DBC* dbc_parse(const std::string& dbc_path) { int line_num = 0; std::smatch match; // TODO: see if we can speed up the regex statements in this loop, SG_ is specifically the slowest - while (std::getline(infile, line)) { + while (std::getline(stream, line)) { line = trim(line); line_num += 1; if (startswith(line, "BO_ ")) { @@ -168,7 +151,7 @@ DBC* dbc_parse(const std::string& dbc_path) { sig.is_signed = match[offset + 5].str() == "-"; sig.factor = std::stod(match[offset + 6].str()); sig.offset = std::stod(match[offset + 7].str()); - set_signal_type(sig, checksum.get(), dbc_name, line_num); + set_signal_type(sig, checksum, dbc_name, line_num); if (sig.is_little_endian) { sig.lsb = sig.start_bit; sig.msb = sig.start_bit + sig.size - 1; @@ -213,6 +196,16 @@ DBC* dbc_parse(const std::string& dbc_path) { return dbc; } +DBC* dbc_parse(const std::string& dbc_path) { + std::ifstream infile(dbc_path); + if (!infile) return nullptr; + + const std::string dbc_name = std::filesystem::path(dbc_path).filename(); + + std::unique_ptr checksum(get_checksum(dbc_name)); + return dbc_parse_from_stream(dbc_name, infile, checksum.get()); +} + const std::string get_dbc_root_path() { char *basedir = std::getenv("BASEDIR"); if (basedir != NULL) { diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc index 8ea4e5ef9..984df1115 100644 --- a/opendbc/can/parser.cc +++ b/opendbc/can/parser.cc @@ -21,7 +21,7 @@ int64_t get_raw_value(const std::vector &msg, const Signal &sig) { int msb = (int)(sig.msb / 8) == i ? sig.msb : (i+1)*8 - 1; int size = msb - lsb + 1; - uint8_t d = (msg[i] >> (lsb - (i*8))) & ((1ULL << size) - 1); + uint64_t d = (msg[i] >> (lsb - (i*8))) & ((1ULL << size) - 1); ret |= d << (bits - size); bits -= size; @@ -127,7 +127,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, } state.size = msg->size; - assert(state.size < 64); // max signal size is 64 bytes + assert(state.size <= 64); // max signal size is 64 bytes // track checksums and counters for this message for (const auto& sig : msg->sigs) { @@ -287,10 +287,12 @@ void CANParser::UpdateValid(uint64_t sec) { const bool missing = state.last_seen_nanos == 0; const bool timed_out = (sec - state.last_seen_nanos) > state.check_threshold; if (state.check_threshold > 0 && (missing || timed_out)) { - if (missing) { - LOGE("0x%X MISSING", state.address); - } else if (show_missing) { - LOGE("0x%X TIMEOUT", state.address); + if (show_missing && !bus_timeout) { + if (missing) { + LOGE("0x%X NOT SEEN", state.address); + } else if (timed_out) { + LOGE("0x%X TIMED OUT", state.address); + } } _valid = false; } diff --git a/opendbc/ford_lincoln_base_pt.dbc b/opendbc/ford_lincoln_base_pt.dbc index 38fa005fe..1f2560179 100644 --- a/opendbc/ford_lincoln_base_pt.dbc +++ b/opendbc/ford_lincoln_base_pt.dbc @@ -1126,7 +1126,8 @@ BO_ 984 IPMA_Data: 8 IPMA_ADAS SG_ DasStats_D_Dsply : 47|2@0+ (1,0) [0|3] "SED" GWM SG_ DasWarn_D_Dsply : 44|2@0+ (1,0) [0|3] "SED" GWM SG_ AhbHiBeam_D_Rq : 59|2@0+ (1,0) [0|3] "SED" GWM - SG_ Set_Me_X1 : 63|4@0+ (1,0) [0|15] "" XXX + SG_ Passthru_63 : 63|4@0+ (1,0) [0|15] "" XXX + SG_ Passthru_48 : 48|1@0+ (1,0) [0|1] "" XXX BO_ 985 IPMA_Data2: 8 IPMA_ADAS SG_ LdwChime_B_Rq : 34|1@0+ (1,0) [0|1] "SED" GWM @@ -3645,6 +3646,258 @@ BO_TX_BU_ 357 : ECM_Diesel,PCM,PCM_HEV; BO_TX_BU_ 342 : ECM_Diesel,PCM,PCM_HEV; BO_TX_BU_ 71 : ECM_Diesel,PCM,PCM_HEV; +CM_ "New CAN FD network"; +CM_ BU_ CMR_DSMC "Driver Status Monitor Camera"; +CM_ BU_ IPMA_ADAS "Assist Driving Alert System"; +CM_ BU_ PSCM "Passenger Front Climate Control Seat Module"; +CM_ BU_ ABS_ESC "Anti-lock Brake / Traction Control Module"; +CM_ BU_ TCCM "Transfer Case Control Module, 4x4 Control Module"; +CM_ BU_ TCM_DSL "Transmission Control Module"; +CM_ BU_ PCM "Powertrain Control Module"; +CM_ BU_ ECM_Diesel "Diesel"; +CM_ BU_ GWM "Gateway Module CGEA1.3(Star Architecture)"; +CM_ BU_ TSTR "Diagnostic Tester"; + +CM_ SG_ 524 PrplWhlTotTqRqMxAwd_No_Cs "Signal not transmitted on gas variants."; +CM_ SG_ 524 PrplWhlTotTqRqMxAwd_No_Cnt "Signal not transmitted on gas variants."; +CM_ SG_ 1150 LocationServices_1 "Tx by HS3 APIM"; +CM_ SG_ 542 LocationServices_3 "Tx by HS3 APIM"; +CM_ SG_ 810 NtfyDrvTrgtDist_L_Rq "Object.SetTripRangeAlert_Rq This signal is meant to provide the customer settings for the target distance remote notification"; +CM_ SG_ 810 NtfyDrvSocLvl1_Pc_Rq "Object.SetCustomerLevelAlert_Rq This signal is meant to provide the customer settings for the SOC remote notification."; +CM_ SG_ 810 PtRmtRprt_D_Stat "Object.RemoteDataReport_St"; +CM_ SG_ 550 PtWakeupActv1_B_Rq "ECG Application signal"; +CM_ SG_ 982 LatCtlPath_No_Cnt "Signal not transmitted on gas variants."; +CM_ SG_ 982 LatCtlPath_No_Cs "Signal not transmitted on gas variants."; +CM_ SG_ 1104 DrvEngageLevel_No_Cs "Signal not transmitted on gas variants."; +CM_ SG_ 1104 DrvEngageLevel_No_Cnt "Signal not transmitted on gas variants."; +CM_ SG_ 1085 ULoRgenTestMde_B_Stat "Signal not transmitted on HEV variants."; +CM_ SG_ 1085 PwSrcULoComm_B_Falt "Signal not transmitted on HEV variants."; +CM_ SG_ 1445 SOBDMC_AutoSarNMReserved4 "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 1445 SOBDMC_AutoSarNMReserved3 "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 1445 SOBDMC_AutoSarNMReserved2 "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 1445 SOBDMC_AutoSarNMReserved1 "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 1445 SOBDMC_AutoSarNMNodeId "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 1445 SOBDMC_AutoSarNMControl "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 1088 HvacPrecondMode2_D_Rq "Signal not transmitted on vehicles that do not have the Fuel operated heater feature."; +CM_ SG_ 1088 HvacPrecondBlwr2_D_Rq "Signal not transmitted on vehicles that do not have the Fuel operated heater feature."; +CM_ SG_ 786 ChrgStat_D_Dsply "Signal not transmitted by PCM_HEV"; +CM_ SG_ 66 immoControlData_T2 "Not transmitted on all vehicle variants"; +CM_ SG_ 66 immoControlCmd_T2 "Not transmitted on all vehicle variants"; +CM_ SG_ 549 BeltminderAudioMute "Object.MyKey.BeltMinderAudioMute.St"; +CM_ SG_ 549 Power_Up_Chime_Modules "Object.AlertMethod.IPC _Infotainment"; +CM_ SG_ 549 Chime_Source "Object.AlertMethod.IPC_infotainment"; +CM_ SG_ 611 AwdStat_D_RqDsply "Signal not transmitted on programs that do not have AWD."; +CM_ SG_ 611 AwdSrvcRqd_B_Rq "Signal not transmitted on programs that do not have AWD."; +CM_ SG_ 611 AwdLck_Tq_Rq "Signal not transmitted on programs that do not have AWD."; +CM_ SG_ 130 VehVTrlrAid_B_Rq "VR session start- req. limit blower speed no more than X volts. CC restriction on time msg. will be honored.Double Event msg1st event to request limit, 2nd event to release limit, No msg. CC Grammar v7.xls"; +CM_ SG_ 130 SteMdule_U_Meas "DCR 1745 to update Tx from GWM to EP100ms. SteMdule_I_Est & SteMdule_U_Meas are used by BMS system & filtered. The filter uses standard periodic rate & changing to EP would adversely affect the filter.(jweinfur)"; +CM_ SG_ 130 SteMdule_I_Est "DCR 1745 to update Tx from GWM to EP100ms. SteMdule_I_Est & SteMdule_U_Meas are used by BMS system & filtered. The filter uses standard periodic rate & changing to EP would adversely affect the filter.(jweinfur)"; +CM_ SG_ 126 StePinRelInit_An_Sns "SASM will transmit these signals on vehicles with SASM and without PSCM. SCCM will not Tx this signal when SASM is present."; +CM_ SG_ 126 StePinAn_No_Cs "Signal not transmitted on gas variants."; +CM_ SG_ 1200 BrkTot_Tq_RqDrv "DCR 1836 to update Tx from GWM to EP100ms rejected due MPS6 TCM requires BrkTot_Tq_RqDrv at 20ms (bshu1)."; +CM_ SG_ 1046 TCMode "Signal data set to 0x0 for ABS only vehicle option content"; +CM_ SG_ 1046 DrvAntiLckLamp_D_Rq "update value table v8.34, not align w/GSDB, need etracker. ABS & IPC implemented as updated."; +CM_ SG_ 1046 BpedMove_D_Actl "CCMGenSigTimeoutTime=1000"; +CM_ SG_ 1044 StePinOffst_An_Est "SASM will transmit these signals on vehicles with SASM and without PSCM. SCCM will not Tx this signal when SASM is present."; +CM_ SG_ 1044 StePinOffst_No_Cs "Signal not transmitted on gas variants."; +CM_ SG_ 534 BrkObdData_No_Actl "Signal not transmitted on gas, diesel and HEV. Only supported on PHEV."; +CM_ SG_ 534 BrkObdIndex_No_Actl "Signal not transmitted on gas, diesel and HEV. Only supported on PHEV."; +CM_ SG_ 534 WhlRotatRr_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; +CM_ SG_ 534 WhlRotatRl_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; +CM_ SG_ 534 WhlRotatFr_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; +CM_ SG_ 534 WhlRotatFl_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; +CM_ SG_ 531 VehLongOvrGnd_A_Est "CCMGenSigTimeoutTime=1000"; +CM_ SG_ 1034 VehicleGGCCData "RCMGenSigTimeout=6000"; +CM_ SG_ 1090 Mtr2State_D_ActlMntr "Signal Not transmitted on PCM_HEV variant"; +CM_ SG_ 1090 Inv1Ain_I_ActlMntr "Signal Not transmitted on PCM_HEV variant"; +CM_ SG_ 1090 VehElRnge_L_Dsply "Signal Not transmitted on HPCM variant"; +CM_ SG_ 374 TrnRng_D_Rq "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 374 TrnPrkSys_D_Actl "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 374 GearLvr_D_ActlDrv "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 374 GearPos_No_Cs "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 374 GearPos_D_Trg "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 374 GearPos_No_Cnt "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 374 GearPos_D_Actl "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 603 WhlDirAvgDrv_D_Actl "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; +CM_ SG_ 603 PrplTqMnRgen_B_Actl "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; +CM_ SG_ 603 BattTracCnnct_D_Rq "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; +CM_ SG_ 872 HybMdeStat_D_Dsply "Signal not transmitted by HPCM"; +CM_ SG_ 560 SelDrvMdeSwtch_D_Stat3 "ECM_HEV, PCM & PCM_HEV do not support or Transmit signal."; +CM_ SG_ 560 TrnSrvcRqd_B_Rq "Signal not transmitted on Diesel & HEV variants. MHT(U611/625) HPCM Rx 0x230 from PCM_HEV. Powersplit FWD HEVs(Cx482/3 430) HPCM Tx 0x230."; +CM_ SG_ 560 TrnShifActv_B_Actl "Signal not transmitted on HEV variants. MHT(U611/625) HPCM Rx 0x230 from PCM_HEV. Powersplit FWD HEVs(Cx482/3 430) HPCM Tx 0x230."; +CM_ SG_ 560 GboxOil_Te_Actl "Signal not transmitted on HEV variants. MHT(U611/625) HPCM Rx 0x230 from PCM_HEV. Powersplit FWD HEVs(Cx482/3 430) HPCM Tx 0x230."; +CM_ SG_ 369 SeatWorkSrfc_B_Falt "Signal Not transmitted by TCM_DSL"; +CM_ SG_ 369 TrnIpcDsplyRng2_D_Actl "Signal not transmitted on HEV variants."; +CM_ SG_ 369 TrnIpcDsplyRng_D_Stat "Signal not transmitted on HEV variants."; +CM_ SG_ 92 TrnLvrV_D_Rq "Signal not transmitted on vehicles with PCM_HEV"; +CM_ SG_ 92 TrnSbwSysHlth_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnGearNtmAllow_B_Stat "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnDtpCmd_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnGearCmd_No_Cs "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnValidGear_D_Cnfm "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnNtrlTowCmd_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnGearCmd_Pc_ActlPt "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnGear_D_RqPt "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnCmdState_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnCmdCnt_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 PrkBrkActv_D_RqTrnGear "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnGearMsgTxt_D_Rq "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 92 TrnGearCmd_No_Cnt "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 1087 BattRgenLoChrg_D_RqEng "Signal not transmitted on diesel variant"; +CM_ SG_ 1087 BattRgenLoDChrg_D_RqEng "Signal not transmitted on diesel variant"; +CM_ SG_ 1087 FapLcInhbt_B_Rq "Signal not transmitted on diesel variant"; +CM_ SG_ 1100 EngExhMdeQuiet_D2_Stat "Signal not transmitted on HEV & Diesel programs"; +CM_ SG_ 1100 HvacCmprLim_D_Stat "Signal not transmitted on Diesel variants"; +CM_ SG_ 1100 WakeAlarm1_T_Rq "Signal not transmitted on Gas variants"; +CM_ SG_ 1100 WakeAlarm1_B_Typ "Signal not transmitted on Gas variants"; +CM_ SG_ 1100 Veh_V_DsplyTrailCtlSet "Signal not transmitted on HEV & Diesel programs"; +CM_ SG_ 1100 TrailCtlPt_B_Falt "Signal not transmitted on HEV & Diesel programs"; +CM_ SG_ 1100 AutoTowAllw_D_StatMnu "Signal not transmitted on Gas & Diesel variants"; +CM_ SG_ 1100 AutoTowActv_B_Stat "Signal not transmitted on Gas & Diesel variants"; +CM_ SG_ 1086 EngMnfld_P_Actl "Signal not transmitted on HEV & Diesel programs"; +CM_ SG_ 1086 ElLoadCtl_D_Rq "Signal not transmitted on HEV variants."; +CM_ SG_ 1071 CluPdl_PcRate_Actl "Signal not transmitted on HEV and Gas variants."; +CM_ SG_ 1071 CluPdlPosPcMeas_D_Qf "Signal not transmitted on HEV and Gas variants."; +CM_ SG_ 1071 CluPdlPos_Pc_Meas "Signal not transmitted on HEV and Gas variants."; +CM_ SG_ 1071 EngAoutIdl_N_Ntrl "Signal not transmitted on HEV variants and diesel."; +CM_ SG_ 1069 SlMde_D_Stat "IPC and HUD_ADV Receve from HS3 FGW 0x42D, not HS3 0x38A, MSG DCR 506. Signal not transmitted on Gas & HEV variants"; +CM_ SG_ 1069 SlMde_D_RqDsply "IPC and HUD_ADV Receve from HS3 FGW 0x42D, not HS3 0x38A, MSG DCR 506. Signal not transmitted on Gas & HEV variants"; +CM_ SG_ 1069 IsaOffst_D_Stat "Signal not transmitted on Gas & HEV variants"; +CM_ SG_ 1069 ObdWarmUp_B_Complt "Signal not transmitted on diesel variants."; +CM_ SG_ 1069 EngMsgTxt_D_Rq "Signal not transmitted on diesel variants."; +CM_ SG_ 1069 EngClnt_Te_ActlDiag "Signal not transmitted on gas variants."; +CM_ SG_ 1069 EngLoad_Pc_CalcDiag "Signal not transmitted on gas variants."; +CM_ SG_ 1069 ApedPos_Pc_ActlDiag "Signal not transmitted on gas variants."; +CM_ SG_ 1060 RearDiffOilTeWarn_B_Rq "Signal not transmitted on HEV & Diesel programs"; +CM_ SG_ 1060 RearDiffOil_Te_Actl "Signal not transmitted on HEV & Diesel programs"; +CM_ SG_ 1057 FohEng_D_Rq "Signal not transmitted on vehicles that do not have the fuel operated heater feature. Signal not transmitted on HEV programs"; +CM_ SG_ 1057 EngIdlShutDown_D_Stat "Signal not transmitted on diesel variants."; +CM_ SG_ 1057 FUEL_ALCOHOL_PERCNT "signal not transmitted on diesel and HEV variants."; +CM_ SG_ 1057 TrnTotTq_Rt_Est "Signal not transmitted on gas and HEV variants."; +CM_ SG_ 1057 TrnTotLss_Tq_Est "Signal not transmitted on gas and HEV variants."; +CM_ SG_ 1057 VehVLimStat_D_Actl "Signal not transmitted on gas and HEV variants."; +CM_ SG_ 1057 VehVLimActv_B_Actl "Signal not transmitted on gas and HEV variants."; +CM_ SG_ 562 GearNtrl_No_Cs "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 562 GearNtrl_No_Cnt "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; +CM_ SG_ 516 EngAoutNActl_D_QF "Signal not transmitted on Gas & HEV variants"; +CM_ SG_ 516 EngAout3_N_Actl "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 516 ApedPos_Pc_ActlArb "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 516 ApedPosPcActl_No_Cnt "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 516 ApedPosPcActl_No_Cs "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 514 VehVTrlrAid_B_Avail "Signal not transmitted on HEV variants."; +CM_ SG_ 514 GearRvrse_D_Actl "Do not implement GearRvrse_D_Actl until eTracker 8804151 is approved, 11/24/09"; +CM_ SG_ 512 PrplWhlTotTqRq_No_Cs "Signal not transmitted on gas & Diesel variants."; +CM_ SG_ 512 PrplWhlTotTqRq_No_Cnt "Signal not transmitted on gas & Diesel variants."; +CM_ SG_ 512 PtDrvMde_D_Stat "Signal not transmitted on Gas & HEV variants."; +CM_ SG_ 380 EngExhBrkOnLamp_B_Rq "Signal not transmitted on Gas variants"; +CM_ SG_ 380 EngExhBrkAutoLamp_B_Rq "Signal not transmitted on Gas variants"; +CM_ SG_ 380 EngAout_N_MxAllw "Signal not transmitted on Gas variants"; +CM_ SG_ 380 EngExhBrkMde_D_Actl "Signal not transmitted on Gas variants"; +CM_ SG_ 377 HvacAirFullOut_B_Rq "Signal not transmitted on diesel variant"; +CM_ SG_ 377 FuelFlw_Vl_Dsply "Signal not transmitted on diesel variants."; +CM_ SG_ 377 FuelFillInlet_B_Dsply "Signal not transmitted on diesel variants."; +CM_ SG_ 377 OdoCount "Signal not transmitted on diesel variants."; +CM_ SG_ 377 EngOilLife_Pc_Actl "Signal not transmitted on diesel variants."; +CM_ SG_ 377 FuelFilterLamp_B_Dsply "Signal not transmitted on Gas & HEV variants"; +CM_ SG_ 377 AirCondRec_B_Rq "Signal not transmitted on diesel variants."; +CM_ SG_ 377 AirCondClutch_B_Stats "Signal not transmitted on diesel variants."; +CM_ SG_ 376 GasPrtc_D_RqDsply "Signal not transmitted on HEV & Diesel variants."; +CM_ SG_ 376 EngAout_Aa_Actl "Signal not transmitted on HEV variants."; +CM_ SG_ 376 AslIconDsply_D_Rq "Signal not transmitted on HEV variants."; +CM_ SG_ 376 AslChime_B_Rq "Signal not transmitted on HEV variants."; +CM_ SG_ 376 HvacHtrCore2_Te_Actl "Signal not transmitted on Gas & Diesel variants"; +CM_ SG_ 376 EcssLamp_D_RqDsply "Signal not transmitted on Gas & HEV variants"; +CM_ SG_ 376 AirAmb_Te_Actl "Signal not transmitted on Gas variants"; +CM_ SG_ 359 PrplWhlRgenMn_Tq_Actl "Signal not transmitted on Gas & Diesel variants"; +CM_ SG_ 359 ElPw_D_StatStrtStop "Signal not transmitted on HEV variants."; +CM_ SG_ 359 TrnAin_Tq_Actl "Signal not transmitted on HEV variances."; +CM_ SG_ 359 PlgActvArb_B_Actl "Only supported on PHEV programs. PlgActvArb_B_Actl HPCM will Tx on Powersplit PHEVs, On MHT PHEVs the PCM_HEV will Tx and HPCM will Rx."; +CM_ SG_ 359 TrnAinTq_D_Qf "Signal not transmitted on HEV variances."; +CM_ SG_ 358 StopStrtStdby_D_Indic "Signal not transmitted on HEV variants."; +CM_ SG_ 358 StopStrtIODTxt_D_Rq "Signal not transmitted on HEV variants."; +CM_ SG_ 358 StopStrtDrvMde_D_Indic "Signal not transmitted on HEV variants."; +CM_ SG_ 358 StopStrtMsgTxt_D_Rq "Signal not transmitted on HEV variants."; +CM_ SG_ 358 HiElPwInhbt_B_Stat "Signal not transmitted on HEV programs"; +CM_ SG_ 358 AutoStopPtDelta_I_Est "Signal not transmitted on HEV variants. "; +CM_ SG_ 357 BpedDrvAppl_D_Actl "CCMGenSigTimeoutTime=1000"; +CM_ SG_ 357 CcStat_D_Actl "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 357 EngAout_N_MnAllw "Signal not transmitted on Gas & Diesel variants."; +CM_ SG_ 357 AccStopMde_D_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 342 EngOvrhtMitgActv_D_Ind "Signal not transmitted on diesel."; +CM_ SG_ 342 EngGoutLss_Tq_Est "Signal not transmitted on gas and diesel variants."; +CM_ SG_ 342 EngOil_Te_Actl "Signal not transmitted on Gas & HEV variants. "; +CM_ SG_ 954 GenericSwtch2_No_Actl "U625 Police Wig Wag feature"; +CM_ SG_ 935 SodRight_D_Stat "IPCGenSigTimeoutTime=1600"; +CM_ SG_ 935 Side_Detect_R_Illum "DDMGenSigTimeoutTime=2000"; +CM_ SG_ 935 SodSnsRight_D_Stat "IPCGenSigTimeoutTime=1600"; +CM_ SG_ 935 SodAlrtRight_D_Stat "DDMGenSigTimeoutTime=2000"; +CM_ SG_ 1112 BattTracClntPmp_D_Stat "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; +CM_ SG_ 1050 Outside_Air_Temp_Stat "Filtered value, i.e. same as disply for customer"; +CM_ SG_ 1009 CenterStackRing_D_Actl "The Center Stack sends the CPLR a signal to turn On or Off the light Ring, This is a customer selectable preference switch"; +CM_ SG_ 1009 APIM_NumberofTracks_St "Object NumberofTracks"; +CM_ SG_ 1009 APIM_ActiveTrackNum_St "Object.ActiveTrackNum"; +CM_ SG_ 360 GsmSrvcRqd_B_Rq "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 360 TrnGearPwmFalt_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 360 GearButtnStuck_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGsmNtmState_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 DrQltyDrv_D_StatGsm "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnBtsiOvrrd_B_Stat "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnValidGearRq_D_Stat "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearRqCnt_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_U_Actl "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlR2 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlR1 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlR0 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlP2 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlP1 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlP0 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlN2 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlN1 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlN0 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlM2 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlM1 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlM0 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlD2 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlD1 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGearButtn_B_ActlD0 "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGear_No_Cs "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGear_No_Cnt "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 TrnGear_D_RqDrv "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 90 BrkSwtchPos_B_ActlGsm "Signal not transmitted on vehicles that do not have shift by wire."; +CM_ SG_ 138 SteWhlRelCalib_An_Sns "Signal is not transmitted on vehicles where PSCM is transmitting the message that contains this signal. Signal data set to 0x0 for ABS only vehicle option content"; +CM_ SG_ 1093 StopLamp_B_RqTrlrBrk "Signal not transmitted on all variants."; +CM_ SG_ 792 ElTrip_L_Dsply "Key cycle trip distance driven with the engine off (Trip EV Miles)"; +CM_ SG_ 792 ElLongTerm_L_Dsply "Long term distance distance driven with the engine off since the last long term reset (Long Term EV Miles)."; +CM_ SG_ 765 Mc_VehTimeFrmtUsrSel_St "Object.BodyInterface.Settings.VehicleTimeFormatUserSelection"; +CM_ SG_ 765 Mc_VehUntTrpCoUsrSel_St "Object.VehicleSettings.Disp_Miles_Kilometers.Rq"; +CM_ SG_ 765 Mc_VehUnitTempUsrSel_St "Object.VehicleSettings.Disp_Temperature.Rq"; +CM_ SG_ 765 Mc_VehLangUsrSel_St "Object.BodyInterface.Settings.VehicleLanguageUserSelection Object Vehicle Settings.Disp_LangSel"; +CM_ SG_ 934 Side_Detect_L_Illum "DDMGenSigTimeoutTime=2000"; +CM_ SG_ 934 SodLeft_D_Stat "IPCGenSigTimeoutTime=1600"; +CM_ SG_ 934 SodSnsLeft_D_Stat "IPCGenSigTimeoutTime=1600"; +CM_ SG_ 934 SodAlrtLeft_D_Stat "DDMGenSigTimeoutTime=2000"; +CM_ SG_ 1072 MetricActv_B_Actl "0 =Inactive(English), 1=Active(Metric)"; +CM_ SG_ 145 VehYaw_W_Actl "CCMGenSigTimeoutTime=1000"; +CM_ SG_ 931 CrnkInhbt_No_Cs "Signal not transmitted on gas and HEV variants."; +CM_ SG_ 931 CrnkInhbt_No_Cnt "Signal not transmitted on gas and HEV variants."; +CM_ SG_ 947 Dimming_Lvl "Refer to Vehicle Interior Illum Dimming Ctrl Sys Eng Spec, Table 2"; +CM_ SG_ 947 PrkBrkActv_B_Actl "Signal not transmitted on vehicles with electric park brake. "; +CM_ SG_ 394 HaDsply_No_Cs "Signal not transmitted on gas variants."; +CM_ SG_ 394 HaDsply_No_Cnt "Signal not transmitted on gas variants."; +CM_ SG_ 391 CmbbBrkDecel_A_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 391 CmbbBrkPrchg_D_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 391 CmbbBrkDecel_B_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 391 CmbbBaSens_D_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 390 AccBrkTot_A_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 390 AccBrkPrkEl_B_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 390 AccPrpl_A_Rq "PCMGenSigTimeoutTime=1000"; +CM_ SG_ 390 AccBrkPrchg_B_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 390 AccBrkDecel_B_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 390 AccStopStat_B_Rq "ABSGenSigTimeoutTime=1000"; +CM_ SG_ 942 PrkAidRdiusRight_L_Dsply "MSGDCR 679, set all signals to OnChange"; +CM_ SG_ 942 PrkAidRdiusLeft_L_Dsply "MSGDCR 679, set all signals to OnChange"; +CM_ SG_ 942 PrkAidDrvDir_D_Stat "MSGDCR 679, set all signals to OnChange"; + BA_DEF_ BO_ "OvtpPushApplication" ENUM "FALSE","TRUE"; BA_DEF_ BU_ "OvtpOperationMode" ENUM "SERVER","CLIENT"; BA_DEF_ BU_ "OvtpEcuAddress" INT 0 1022; @@ -12440,255 +12693,3 @@ VAL_ 922 DcacRdy_D_Stat 7 "NotUsed_2" 6 "NotUsed_1" 5 "Faulted" 4 "ProtectionTem VAL_ 922 DcacPlugPrsnt_B_Stat 1 "Yes" 0 "No"; VAL_ 922 DcacClntFlw_D_Rq 3 "High" 2 "Medium" 1 "Low" 0 "Off"; VAL_ 922 CoolFanDcac_D_Rq 3 "High" 2 "Medium" 1 "Low" 0 "Off"; - -CM_ "New CAN FD network"; -CM_ BU_ CMR_DSMC "Driver Status Monitor Camera"; -CM_ BU_ IPMA_ADAS "Assist Driving Alert System"; -CM_ BU_ PSCM "Passenger Front Climate Control Seat Module"; -CM_ BU_ ABS_ESC "Anti-lock Brake / Traction Control Module"; -CM_ BU_ TCCM "Transfer Case Control Module, 4x4 Control Module"; -CM_ BU_ TCM_DSL "Transmission Control Module"; -CM_ BU_ PCM "Powertrain Control Module"; -CM_ BU_ ECM_Diesel "Diesel"; -CM_ BU_ GWM "Gateway Module CGEA1.3(Star Architecture)"; -CM_ BU_ TSTR "Diagnostic Tester"; - -CM_ SG_ 524 PrplWhlTotTqRqMxAwd_No_Cs "Signal not transmitted on gas variants."; -CM_ SG_ 524 PrplWhlTotTqRqMxAwd_No_Cnt "Signal not transmitted on gas variants."; -CM_ SG_ 1150 LocationServices_1 "Tx by HS3 APIM"; -CM_ SG_ 542 LocationServices_3 "Tx by HS3 APIM"; -CM_ SG_ 810 NtfyDrvTrgtDist_L_Rq "Object.SetTripRangeAlert_Rq This signal is meant to provide the customer settings for the target distance remote notification"; -CM_ SG_ 810 NtfyDrvSocLvl1_Pc_Rq "Object.SetCustomerLevelAlert_Rq This signal is meant to provide the customer settings for the SOC remote notification."; -CM_ SG_ 810 PtRmtRprt_D_Stat "Object.RemoteDataReport_St"; -CM_ SG_ 550 PtWakeupActv1_B_Rq "ECG Application signal"; -CM_ SG_ 982 LatCtlPath_No_Cnt "Signal not transmitted on gas variants."; -CM_ SG_ 982 LatCtlPath_No_Cs "Signal not transmitted on gas variants."; -CM_ SG_ 1104 DrvEngageLevel_No_Cs "Signal not transmitted on gas variants."; -CM_ SG_ 1104 DrvEngageLevel_No_Cnt "Signal not transmitted on gas variants."; -CM_ SG_ 1085 ULoRgenTestMde_B_Stat "Signal not transmitted on HEV variants."; -CM_ SG_ 1085 PwSrcULoComm_B_Falt "Signal not transmitted on HEV variants."; -CM_ SG_ 1445 SOBDMC_AutoSarNMReserved4 "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 1445 SOBDMC_AutoSarNMReserved3 "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 1445 SOBDMC_AutoSarNMReserved2 "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 1445 SOBDMC_AutoSarNMReserved1 "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 1445 SOBDMC_AutoSarNMNodeId "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 1445 SOBDMC_AutoSarNMControl "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 1088 HvacPrecondMode2_D_Rq "Signal not transmitted on vehicles that do not have the Fuel operated heater feature."; -CM_ SG_ 1088 HvacPrecondBlwr2_D_Rq "Signal not transmitted on vehicles that do not have the Fuel operated heater feature."; -CM_ SG_ 786 ChrgStat_D_Dsply "Signal not transmitted by PCM_HEV"; -CM_ SG_ 66 immoControlData_T2 "Not transmitted on all vehicle variants"; -CM_ SG_ 66 immoControlCmd_T2 "Not transmitted on all vehicle variants"; -CM_ SG_ 549 BeltminderAudioMute "Object.MyKey.BeltMinderAudioMute.St"; -CM_ SG_ 549 Power_Up_Chime_Modules "Object.AlertMethod.IPC _Infotainment"; -CM_ SG_ 549 Chime_Source "Object.AlertMethod.IPC_infotainment"; -CM_ SG_ 611 AwdStat_D_RqDsply "Signal not transmitted on programs that do not have AWD."; -CM_ SG_ 611 AwdSrvcRqd_B_Rq "Signal not transmitted on programs that do not have AWD."; -CM_ SG_ 611 AwdLck_Tq_Rq "Signal not transmitted on programs that do not have AWD."; -CM_ SG_ 130 VehVTrlrAid_B_Rq "VR session start- req. limit blower speed no more than X volts. CC restriction on time msg. will be honored.Double Event msg1st event to request limit, 2nd event to release limit, No msg. CC Grammar v7.xls"; -CM_ SG_ 130 SteMdule_U_Meas "DCR 1745 to update Tx from GWM to EP100ms. SteMdule_I_Est & SteMdule_U_Meas are used by BMS system & filtered. The filter uses standard periodic rate & changing to EP would adversely affect the filter.(jweinfur)"; -CM_ SG_ 130 SteMdule_I_Est "DCR 1745 to update Tx from GWM to EP100ms. SteMdule_I_Est & SteMdule_U_Meas are used by BMS system & filtered. The filter uses standard periodic rate & changing to EP would adversely affect the filter.(jweinfur)"; -CM_ SG_ 126 StePinRelInit_An_Sns "SASM will transmit these signals on vehicles with SASM and without PSCM. SCCM will not Tx this signal when SASM is present."; -CM_ SG_ 126 StePinAn_No_Cs "Signal not transmitted on gas variants."; -CM_ SG_ 1200 BrkTot_Tq_RqDrv "DCR 1836 to update Tx from GWM to EP100ms rejected due MPS6 TCM requires BrkTot_Tq_RqDrv at 20ms (bshu1)."; -CM_ SG_ 1046 TCMode "Signal data set to 0x0 for ABS only vehicle option content"; -CM_ SG_ 1046 DrvAntiLckLamp_D_Rq "update value table v8.34, not align w/GSDB, need etracker. ABS & IPC implemented as updated."; -CM_ SG_ 1046 BpedMove_D_Actl "CCMGenSigTimeoutTime=1000"; -CM_ SG_ 1044 StePinOffst_An_Est "SASM will transmit these signals on vehicles with SASM and without PSCM. SCCM will not Tx this signal when SASM is present."; -CM_ SG_ 1044 StePinOffst_No_Cs "Signal not transmitted on gas variants."; -CM_ SG_ 534 BrkObdData_No_Actl "Signal not transmitted on gas, diesel and HEV. Only supported on PHEV."; -CM_ SG_ 534 BrkObdIndex_No_Actl "Signal not transmitted on gas, diesel and HEV. Only supported on PHEV."; -CM_ SG_ 534 WhlRotatRr_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; -CM_ SG_ 534 WhlRotatRl_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; -CM_ SG_ 534 WhlRotatFr_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; -CM_ SG_ 534 WhlRotatFl_No_Cnt "DCR 1833 to update Tx from GWM to EP100ms. Wheel Count signals are used by complex algorithm to determine Tire Pressure by Location & can't change from 20msec. (jweinfur)"; -CM_ SG_ 531 VehLongOvrGnd_A_Est "CCMGenSigTimeoutTime=1000"; -CM_ SG_ 1034 VehicleGGCCData "RCMGenSigTimeout=6000"; -CM_ SG_ 1090 Mtr2State_D_ActlMntr "Signal Not transmitted on PCM_HEV variant"; -CM_ SG_ 1090 Inv1Ain_I_ActlMntr "Signal Not transmitted on PCM_HEV variant"; -CM_ SG_ 1090 VehElRnge_L_Dsply "Signal Not transmitted on HPCM variant"; -CM_ SG_ 374 TrnRng_D_Rq "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 374 TrnPrkSys_D_Actl "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 374 GearLvr_D_ActlDrv "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 374 GearPos_No_Cs "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 374 GearPos_D_Trg "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 374 GearPos_No_Cnt "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 374 GearPos_D_Actl "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 603 WhlDirAvgDrv_D_Actl "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; -CM_ SG_ 603 PrplTqMnRgen_B_Actl "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; -CM_ SG_ 603 BattTracCnnct_D_Rq "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; -CM_ SG_ 872 HybMdeStat_D_Dsply "Signal not transmitted by HPCM"; -CM_ SG_ 560 SelDrvMdeSwtch_D_Stat3 "ECM_HEV, PCM & PCM_HEV do not support or Transmit signal."; -CM_ SG_ 560 TrnSrvcRqd_B_Rq "Signal not transmitted on Diesel & HEV variants. MHT(U611/625) HPCM Rx 0x230 from PCM_HEV. Powersplit FWD HEVs(Cx482/3 430) HPCM Tx 0x230."; -CM_ SG_ 560 TrnShifActv_B_Actl "Signal not transmitted on HEV variants. MHT(U611/625) HPCM Rx 0x230 from PCM_HEV. Powersplit FWD HEVs(Cx482/3 430) HPCM Tx 0x230."; -CM_ SG_ 560 GboxOil_Te_Actl "Signal not transmitted on HEV variants. MHT(U611/625) HPCM Rx 0x230 from PCM_HEV. Powersplit FWD HEVs(Cx482/3 430) HPCM Tx 0x230."; -CM_ SG_ 369 SeatWorkSrfc_B_Falt "Signal Not transmitted by TCM_DSL"; -CM_ SG_ 369 TrnIpcDsplyRng2_D_Actl "Signal not transmitted on HEV variants."; -CM_ SG_ 369 TrnIpcDsplyRng_D_Stat "Signal not transmitted on HEV variants."; -CM_ SG_ 92 TrnLvrV_D_Rq "Signal not transmitted on vehicles with PCM_HEV"; -CM_ SG_ 92 TrnSbwSysHlth_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnGearNtmAllow_B_Stat "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnDtpCmd_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnGearCmd_No_Cs "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnValidGear_D_Cnfm "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnNtrlTowCmd_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnGearCmd_Pc_ActlPt "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnGear_D_RqPt "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnCmdState_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnCmdCnt_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 PrkBrkActv_D_RqTrnGear "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnGearMsgTxt_D_Rq "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 92 TrnGearCmd_No_Cnt "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 1087 BattRgenLoChrg_D_RqEng "Signal not transmitted on diesel variant"; -CM_ SG_ 1087 BattRgenLoDChrg_D_RqEng "Signal not transmitted on diesel variant"; -CM_ SG_ 1087 FapLcInhbt_B_Rq "Signal not transmitted on diesel variant"; -CM_ SG_ 1100 EngExhMdeQuiet_D2_Stat "Signal not transmitted on HEV & Diesel programs"; -CM_ SG_ 1100 HvacCmprLim_D_Stat "Signal not transmitted on Diesel variants"; -CM_ SG_ 1100 WakeAlarm1_T_Rq "Signal not transmitted on Gas variants"; -CM_ SG_ 1100 WakeAlarm1_B_Typ "Signal not transmitted on Gas variants"; -CM_ SG_ 1100 Veh_V_DsplyTrailCtlSet "Signal not transmitted on HEV & Diesel programs"; -CM_ SG_ 1100 TrailCtlPt_B_Falt "Signal not transmitted on HEV & Diesel programs"; -CM_ SG_ 1100 AutoTowAllw_D_StatMnu "Signal not transmitted on Gas & Diesel variants"; -CM_ SG_ 1100 AutoTowActv_B_Stat "Signal not transmitted on Gas & Diesel variants"; -CM_ SG_ 1086 EngMnfld_P_Actl "Signal not transmitted on HEV & Diesel programs"; -CM_ SG_ 1086 ElLoadCtl_D_Rq "Signal not transmitted on HEV variants."; -CM_ SG_ 1071 CluPdl_PcRate_Actl "Signal not transmitted on HEV and Gas variants."; -CM_ SG_ 1071 CluPdlPosPcMeas_D_Qf "Signal not transmitted on HEV and Gas variants."; -CM_ SG_ 1071 CluPdlPos_Pc_Meas "Signal not transmitted on HEV and Gas variants."; -CM_ SG_ 1071 EngAoutIdl_N_Ntrl "Signal not transmitted on HEV variants and diesel."; -CM_ SG_ 1069 SlMde_D_Stat "IPC and HUD_ADV Receve from HS3 FGW 0x42D, not HS3 0x38A, MSG DCR 506. Signal not transmitted on Gas & HEV variants"; -CM_ SG_ 1069 SlMde_D_RqDsply "IPC and HUD_ADV Receve from HS3 FGW 0x42D, not HS3 0x38A, MSG DCR 506. Signal not transmitted on Gas & HEV variants"; -CM_ SG_ 1069 IsaOffst_D_Stat "Signal not transmitted on Gas & HEV variants"; -CM_ SG_ 1069 ObdWarmUp_B_Complt "Signal not transmitted on diesel variants."; -CM_ SG_ 1069 EngMsgTxt_D_Rq "Signal not transmitted on diesel variants."; -CM_ SG_ 1069 EngClnt_Te_ActlDiag "Signal not transmitted on gas variants."; -CM_ SG_ 1069 EngLoad_Pc_CalcDiag "Signal not transmitted on gas variants."; -CM_ SG_ 1069 ApedPos_Pc_ActlDiag "Signal not transmitted on gas variants."; -CM_ SG_ 1060 RearDiffOilTeWarn_B_Rq "Signal not transmitted on HEV & Diesel programs"; -CM_ SG_ 1060 RearDiffOil_Te_Actl "Signal not transmitted on HEV & Diesel programs"; -CM_ SG_ 1057 FohEng_D_Rq "Signal not transmitted on vehicles that do not have the fuel operated heater feature. Signal not transmitted on HEV programs"; -CM_ SG_ 1057 EngIdlShutDown_D_Stat "Signal not transmitted on diesel variants."; -CM_ SG_ 1057 FUEL_ALCOHOL_PERCNT "signal not transmitted on diesel and HEV variants."; -CM_ SG_ 1057 TrnTotTq_Rt_Est "Signal not transmitted on gas and HEV variants."; -CM_ SG_ 1057 TrnTotLss_Tq_Est "Signal not transmitted on gas and HEV variants."; -CM_ SG_ 1057 VehVLimStat_D_Actl "Signal not transmitted on gas and HEV variants."; -CM_ SG_ 1057 VehVLimActv_B_Actl "Signal not transmitted on gas and HEV variants."; -CM_ SG_ 562 GearNtrl_No_Cs "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 562 GearNtrl_No_Cnt "Powersplit HEVs with HF45, HPCM will Tx this message. For all other programs it will be Tx by PCM or TCM. "; -CM_ SG_ 516 EngAoutNActl_D_QF "Signal not transmitted on Gas & HEV variants"; -CM_ SG_ 516 EngAout3_N_Actl "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 516 ApedPos_Pc_ActlArb "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 516 ApedPosPcActl_No_Cnt "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 516 ApedPosPcActl_No_Cs "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 514 VehVTrlrAid_B_Avail "Signal not transmitted on HEV variants."; -CM_ SG_ 514 GearRvrse_D_Actl "Do not implement GearRvrse_D_Actl until eTracker 8804151 is approved, 11/24/09"; -CM_ SG_ 512 PrplWhlTotTqRq_No_Cs "Signal not transmitted on gas & Diesel variants."; -CM_ SG_ 512 PrplWhlTotTqRq_No_Cnt "Signal not transmitted on gas & Diesel variants."; -CM_ SG_ 512 PtDrvMde_D_Stat "Signal not transmitted on Gas & HEV variants."; -CM_ SG_ 380 EngExhBrkOnLamp_B_Rq "Signal not transmitted on Gas variants"; -CM_ SG_ 380 EngExhBrkAutoLamp_B_Rq "Signal not transmitted on Gas variants"; -CM_ SG_ 380 EngAout_N_MxAllw "Signal not transmitted on Gas variants"; -CM_ SG_ 380 EngExhBrkMde_D_Actl "Signal not transmitted on Gas variants"; -CM_ SG_ 377 HvacAirFullOut_B_Rq "Signal not transmitted on diesel variant"; -CM_ SG_ 377 FuelFlw_Vl_Dsply "Signal not transmitted on diesel variants."; -CM_ SG_ 377 FuelFillInlet_B_Dsply "Signal not transmitted on diesel variants."; -CM_ SG_ 377 OdoCount "Signal not transmitted on diesel variants."; -CM_ SG_ 377 EngOilLife_Pc_Actl "Signal not transmitted on diesel variants."; -CM_ SG_ 377 FuelFilterLamp_B_Dsply "Signal not transmitted on Gas & HEV variants"; -CM_ SG_ 377 AirCondRec_B_Rq "Signal not transmitted on diesel variants."; -CM_ SG_ 377 AirCondClutch_B_Stats "Signal not transmitted on diesel variants."; -CM_ SG_ 376 GasPrtc_D_RqDsply "Signal not transmitted on HEV & Diesel variants."; -CM_ SG_ 376 EngAout_Aa_Actl "Signal not transmitted on HEV variants."; -CM_ SG_ 376 AslIconDsply_D_Rq "Signal not transmitted on HEV variants."; -CM_ SG_ 376 AslChime_B_Rq "Signal not transmitted on HEV variants."; -CM_ SG_ 376 HvacHtrCore2_Te_Actl "Signal not transmitted on Gas & Diesel variants"; -CM_ SG_ 376 EcssLamp_D_RqDsply "Signal not transmitted on Gas & HEV variants"; -CM_ SG_ 376 AirAmb_Te_Actl "Signal not transmitted on Gas variants"; -CM_ SG_ 359 PrplWhlRgenMn_Tq_Actl "Signal not transmitted on Gas & Diesel variants"; -CM_ SG_ 359 ElPw_D_StatStrtStop "Signal not transmitted on HEV variants."; -CM_ SG_ 359 TrnAin_Tq_Actl "Signal not transmitted on HEV variances."; -CM_ SG_ 359 PlgActvArb_B_Actl "Only supported on PHEV programs. PlgActvArb_B_Actl HPCM will Tx on Powersplit PHEVs, On MHT PHEVs the PCM_HEV will Tx and HPCM will Rx."; -CM_ SG_ 359 TrnAinTq_D_Qf "Signal not transmitted on HEV variances."; -CM_ SG_ 358 StopStrtStdby_D_Indic "Signal not transmitted on HEV variants."; -CM_ SG_ 358 StopStrtIODTxt_D_Rq "Signal not transmitted on HEV variants."; -CM_ SG_ 358 StopStrtDrvMde_D_Indic "Signal not transmitted on HEV variants."; -CM_ SG_ 358 StopStrtMsgTxt_D_Rq "Signal not transmitted on HEV variants."; -CM_ SG_ 358 HiElPwInhbt_B_Stat "Signal not transmitted on HEV programs"; -CM_ SG_ 358 AutoStopPtDelta_I_Est "Signal not transmitted on HEV variants. "; -CM_ SG_ 357 BpedDrvAppl_D_Actl "CCMGenSigTimeoutTime=1000"; -CM_ SG_ 357 CcStat_D_Actl "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 357 EngAout_N_MnAllw "Signal not transmitted on Gas & Diesel variants."; -CM_ SG_ 357 AccStopMde_D_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 342 EngOvrhtMitgActv_D_Ind "Signal not transmitted on diesel."; -CM_ SG_ 342 EngGoutLss_Tq_Est "Signal not transmitted on gas and diesel variants."; -CM_ SG_ 342 EngOil_Te_Actl "Signal not transmitted on Gas & HEV variants. "; -CM_ SG_ 954 GenericSwtch2_No_Actl "U625 Police Wig Wag feature"; -CM_ SG_ 935 SodRight_D_Stat "IPCGenSigTimeoutTime=1600"; -CM_ SG_ 935 Side_Detect_R_Illum "DDMGenSigTimeoutTime=2000"; -CM_ SG_ 935 SodSnsRight_D_Stat "IPCGenSigTimeoutTime=1600"; -CM_ SG_ 935 SodAlrtRight_D_Stat "DDMGenSigTimeoutTime=2000"; -CM_ SG_ 1112 BattTracClntPmp_D_Stat "48V mHEV: ECM_HEVDSL is Tx, MHT HEV: PCM_HEV is Tx, Powersplit HEV: SOBDMC_HPCM is Tx"; -CM_ SG_ 1050 Outside_Air_Temp_Stat "Filtered value, i.e. same as disply for customer"; -CM_ SG_ 1009 CenterStackRing_D_Actl "The Center Stack sends the CPLR a signal to turn On or Off the light Ring, This is a customer selectable preference switch"; -CM_ SG_ 1009 APIM_NumberofTracks_St "Object NumberofTracks"; -CM_ SG_ 1009 APIM_ActiveTrackNum_St "Object.ActiveTrackNum"; -CM_ SG_ 360 GsmSrvcRqd_B_Rq "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 360 TrnGearPwmFalt_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 360 GearButtnStuck_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGsmNtmState_D_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 DrQltyDrv_D_StatGsm "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnBtsiOvrrd_B_Stat "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnValidGearRq_D_Stat "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearRqCnt_B_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_U_Actl "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlR2 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlR1 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlR0 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlP2 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlP1 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlP0 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlN2 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlN1 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlN0 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlM2 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlM1 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlM0 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlD2 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlD1 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGearButtn_B_ActlD0 "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGear_No_Cs "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGear_No_Cnt "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 TrnGear_D_RqDrv "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 90 BrkSwtchPos_B_ActlGsm "Signal not transmitted on vehicles that do not have shift by wire."; -CM_ SG_ 138 SteWhlRelCalib_An_Sns "Signal is not transmitted on vehicles where PSCM is transmitting the message that contains this signal. Signal data set to 0x0 for ABS only vehicle option content"; -CM_ SG_ 1093 StopLamp_B_RqTrlrBrk "Signal not transmitted on all variants."; -CM_ SG_ 792 ElTrip_L_Dsply "Key cycle trip distance driven with the engine off (Trip EV Miles)"; -CM_ SG_ 792 ElLongTerm_L_Dsply "Long term distance distance driven with the engine off since the last long term reset (Long Term EV Miles)."; -CM_ SG_ 765 Mc_VehTimeFrmtUsrSel_St "Object.BodyInterface.Settings.VehicleTimeFormatUserSelection"; -CM_ SG_ 765 Mc_VehUntTrpCoUsrSel_St "Object.VehicleSettings.Disp_Miles_Kilometers.Rq"; -CM_ SG_ 765 Mc_VehUnitTempUsrSel_St "Object.VehicleSettings.Disp_Temperature.Rq"; -CM_ SG_ 765 Mc_VehLangUsrSel_St "Object.BodyInterface.Settings.VehicleLanguageUserSelection Object Vehicle Settings.Disp_LangSel"; -CM_ SG_ 934 Side_Detect_L_Illum "DDMGenSigTimeoutTime=2000"; -CM_ SG_ 934 SodLeft_D_Stat "IPCGenSigTimeoutTime=1600"; -CM_ SG_ 934 SodSnsLeft_D_Stat "IPCGenSigTimeoutTime=1600"; -CM_ SG_ 934 SodAlrtLeft_D_Stat "DDMGenSigTimeoutTime=2000"; -CM_ SG_ 1072 MetricActv_B_Actl "0 =Inactive(English), 1=Active(Metric)"; -CM_ SG_ 145 VehYaw_W_Actl "CCMGenSigTimeoutTime=1000"; -CM_ SG_ 931 CrnkInhbt_No_Cs "Signal not transmitted on gas and HEV variants."; -CM_ SG_ 931 CrnkInhbt_No_Cnt "Signal not transmitted on gas and HEV variants."; -CM_ SG_ 947 Dimming_Lvl "Refer to Vehicle Interior Illum Dimming Ctrl Sys Eng Spec, Table 2"; -CM_ SG_ 947 PrkBrkActv_B_Actl "Signal not transmitted on vehicles with electric park brake. "; -CM_ SG_ 394 HaDsply_No_Cs "Signal not transmitted on gas variants."; -CM_ SG_ 394 HaDsply_No_Cnt "Signal not transmitted on gas variants."; -CM_ SG_ 391 CmbbBrkDecel_A_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 391 CmbbBrkPrchg_D_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 391 CmbbBrkDecel_B_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 391 CmbbBaSens_D_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 390 AccBrkTot_A_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 390 AccBrkPrkEl_B_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 390 AccPrpl_A_Rq "PCMGenSigTimeoutTime=1000"; -CM_ SG_ 390 AccBrkPrchg_B_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 390 AccBrkDecel_B_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 390 AccStopStat_B_Rq "ABSGenSigTimeoutTime=1000"; -CM_ SG_ 942 PrkAidRdiusRight_L_Dsply "MSGDCR 679, set all signals to OnChange"; -CM_ SG_ 942 PrkAidRdiusLeft_L_Dsply "MSGDCR 679, set all signals to OnChange"; -CM_ SG_ 942 PrkAidDrvDir_D_Stat "MSGDCR 679, set all signals to OnChange"; diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 55c10524b..a251bb96e 100644 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc/gm_global_a_powertrain_generated.dbc @@ -91,7 +91,7 @@ BO_ 201 ECMEngineStatus: 8 K20_ECM SG_ EngineTPS : 39|8@0+ (0.392156863,0) [0|100.000000065] "%" NEO SG_ EngineRPM : 15|16@0+ (0.25,0) [0|0] "RPM" NEO SG_ CruiseMainOn : 29|1@0+ (1,0) [0|1] "" NEO - SG_ Brake_Pressed : 40|1@0+ (1,0) [0|1] "" NEO + SG_ BrakePressed : 40|1@0+ (1,0) [0|1] "" NEO SG_ Standstill : 2|1@0+ (1,0) [0|1] "" NEO SG_ CruiseActive : 31|2@0+ (1,0) [0|3] "" NEO @@ -104,6 +104,7 @@ BO_ 209 EBCMBrakePedalSensors: 7 K17_EBCM SG_ BrakeNormalized2 : 47|8@0- (-1,0) [0|255] "" XXX BO_ 241 EBCMBrakePedalPosition: 6 K17_EBCM + SG_ BrakePressed : 1|1@0+ (1,0) [0|1] "" XXX SG_ BrakePedalPosition : 15|8@0+ (1,0) [0|255] "" NEO BO_ 298 BCMDoorBeltStatus: 8 K9_BCM @@ -143,6 +144,8 @@ BO_ 388 PSCMStatus: 8 K43_PSCM SG_ LKADriverAppldTrq : 50|11@0- (0.01,0) [-10.24|10.23] "Nm" NEO SG_ LKATorqueDelivered : 18|11@0- (0.01,0) [0|1] "" NEO SG_ LKATotalTorqueDelivered : 2|11@0- (0.01,0) [-10.24|10.23] "Nm" NEO + SG_ RollingCounter : 38|4@0+ (1,0) [0|15] "" XXX + SG_ PSCMStatusChecksum : 33|10@0+ (1,0) [0|1023] "" XXX BO_ 417 AcceleratorPedal: 7 XXX SG_ AcceleratorPedal : 55|8@0+ (1,0) [0|0] "" NEO @@ -184,6 +187,9 @@ BO_ 501 ECMPRDNL2: 8 K20_ECM SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO SG_ ManualMode : 41|1@0+ (1,0) [0|1] "" NEO +BO_ 532 BRAKE_RELATED: 6 XXX + SG_ UserBrakePressure : 0|9@0+ (1,0) [0|511] "" XXX + BO_ 560 EPBStatus: 8 EPB SG_ EPBClosed : 12|1@0+ (1,0) [0|1] "" NEO @@ -204,6 +210,9 @@ BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM BO_ 717 ASCM_2CD: 5 K124_ASCM +BO_ 761 BRAKE_RELATED_2: 7 XXX + SG_ UserBrakePressure2 : 47|9@0+ (1,0) [0|511] "" XXX + BO_ 789 EBCMFrictionBrakeCmd: 5 K124_ASCM SG_ RollingCounter : 33|2@0+ (1,0) [0|0] "" NEO SG_ FrictionBrakeMode : 7|4@0+ (1,0) [0|0] "" NEO @@ -221,13 +230,17 @@ BO_ 810 TCICOnStarGPSPosition: 8 K73_TCIC SG_ GPSLongitude : 39|32@0+ (1,-2147483648) [0|0] "milliarcsecond" NEO SG_ GPSLatitude : 7|32@0+ (1,0) [0|0] "milliarcsecond" NEO -BO_ 840 EBCMWheelSpdFront: 4 K17_EBCM +BO_ 840 EBCMWheelSpdFront: 5 K17_EBCM SG_ FLWheelSpd : 7|16@0+ (0.0311,0) [0|255] "km/h" NEO SG_ FRWheelSpd : 23|16@0+ (0.0311,0) [0|255] "km/h" NEO BO_ 842 EBCMWheelSpdRear: 5 K17_EBCM SG_ RLWheelSpd : 7|16@0+ (0.0311,0) [0|255] "km/h" NEO SG_ RRWheelSpd : 23|16@0+ (0.0311,0) [0|255] "km/h" NEO + SG_ MovingForward : 32|1@0+ (1,0) [0|1] "" XXX + SG_ MovingBackward : 33|1@0+ (1,0) [0|1] "" XXX + SG_ MovingForward2 : 35|1@1+ (1,0) [0|1] "" XXX + SG_ MovingBackward2 : 36|1@0+ (1,0) [0|1] "" XXX BO_ 869 ASCM_365: 4 K124_ASCM @@ -241,6 +254,10 @@ 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_ 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 + BO_ 1001 ECMVehicleSpeed: 8 K20_ECM SG_ VehicleSpeed : 7|16@0+ (0.01,0) [0|0] "mph" NEO SG_ VehicleSpeedLeft : 39|16@0+ (0.01,0) [0|0] "mph" NEO @@ -283,11 +300,14 @@ CM_ BU_ K114B_HPCM "Hybrid Powertrain Control Module"; CM_ BU_ NEO "Comma NEO"; CM_ BU_ K124_ASCM "Active Safety Control Module"; CM_ SG_ 381 MSG17D_AccPower "Need to investigate"; +CM_ BO_ 190 "Length varies from 6 to 8 bytes by car"; CM_ SG_ 190 GasPedalAndAcc "ACC baseline is 62"; CM_ SG_ 352 Ignition "Non-zero when ignition is on"; CM_ SG_ 451 GasPedalAndAcc2 "ACC baseline is 62"; CM_ SG_ 497 Ignition "Describes ignition + preconditioning mode, noisy"; CM_ SG_ 501 PRNDL2 "When ManualMode is Active, Value is 13=L1 12=L2 11=L3 ... 4=L10"; +CM_ SG_ 532 UserBrakePressure "can be lower than other brake position signals when the brakes are pre-filled from ACC braking and the user presses on the brakes. user-only pressure?"; +CM_ SG_ 761 UserBrakePressure2 "Similar to BRAKE_RELATED->UserBrakePressure"; CM_ SG_ 1001 VehicleSpeed "Spinouts show here on 2wd. Speed derived from right front wheel (drive tire)"; BA_DEF_ "UseGMParameterIDs" INT 0 0; BA_DEF_ "ProtocolType" STRING ; diff --git a/opendbc/honda_civic_ex_2022_can_generated.dbc b/opendbc/honda_civic_ex_2022_can_generated.dbc index 782316108..2b06a4a49 100644 --- a/opendbc/honda_civic_ex_2022_can_generated.dbc +++ b/opendbc/honda_civic_ex_2022_can_generated.dbc @@ -448,6 +448,7 @@ BO_ 432 STANDSTILL: 7 VSA BO_ 456 ACC_CONTROL: 8 XXX SG_ ACCEL_COMMAND : 7|12@0- (0.01,0) [0|0] "m/s^2" XXX SG_ CONTROL_OFF : 8|1@0+ (1,0) [0|1] "" XXX + SG_ STANDSTILL : 9|1@0+ (1,0) [0|1] "" XXX SG_ CONTROL_ON : 10|1@0+ (1,0) [0|1] "" XXX SG_ BOH : 23|1@0+ (1,0) [0|1] "" XXX SG_ AEB_STATUS : 33|1@1+ (1,0) [0|7] "" XXX @@ -495,9 +496,8 @@ 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_ BO 456 "not sure which bit enables cruise"; -CM_ SG_ 456 ACCEL_COMMAND "seems to be m/s^2? need to verify"; -CM_ SG_ 456 BOH "could be cruise standstill?"; +CM_ BO_ 456 "not sure which bit enables cruise"; +CM_ SG_ 456 STANDSTILL "set to 1 when camera requests -4.0 m/s^2"; VAL_ 401 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P"; VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P"; diff --git a/opendbc/hyundai_canfd.dbc b/opendbc/hyundai_canfd.dbc index 83c5f833f..0977f9aef 100644 --- a/opendbc/hyundai_canfd.dbc +++ b/opendbc/hyundai_canfd.dbc @@ -33,7 +33,7 @@ NS_ : BS_: -BU_: XXX CAMERA +BU_: XXX CAMERA FRONT_RADAR ADRV BO_ 53 ACCELERATOR: 32 XXX @@ -42,6 +42,11 @@ BO_ 53 ACCELERATOR: 32 XXX SG_ GEAR : 192|3@1+ (1,0) [0|7] "" XXX SG_ ACCELERATOR_PEDAL : 40|8@1+ (1,0) [0|255] "" XXX +BO_ 64 GEAR_ALT: 32 XXX + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ GEAR : 32|3@1+ (1,0) [0|7] "" XXX + BO_ 69 GEAR: 24 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX @@ -62,15 +67,23 @@ BO_ 80 LKAS: 16 XXX SG_ NEW_SIGNAL_2 : 70|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_1 : 80|1@0+ (1,0) [0|1] "" XXX SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX + SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX + +BO_ 81 ADRV_0x51: 32 ADRV + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX BO_ 96 ESP_STATUS: 32 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX SG_ ESP_DISABLED : 42|3@1+ (1,0) [0|63] "" XXX + SG_ BRAKE_PRESSURE : 128|10@1+ (1,0) [0|65535] "" XXX + SG_ BRAKE_PRESSED : 148|1@1+ (1,0) [0|3] "" XXX BO_ 101 BRAKE: 32 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ BRAKE_POSITION : 40|16@1- (1,0) [0|65535] "" XXX SG_ BRAKE_PRESSED : 57|1@1+ (1,0) [0|3] "" XXX BO_ 160 WHEEL_SPEEDS: 24 XXX @@ -85,11 +98,18 @@ BO_ 234 MDPS: 24 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX SG_ LKA_ACTIVE : 48|1@0+ (1,0) [0|16777215] "" XXX + SG_ LKA_FAULT : 54|1@0+ (1,0) [0|1] "" XXX SG_ STEERING_OUT_TORQUE : 64|12@1+ (0.1,-204.8) [0|65535] "" XXX SG_ STEERING_COL_TORQUE : 80|13@1+ (1,-4095) [0|4095] "" XXX SG_ STEERING_ANGLE : 96|16@1- (-0.1,0) [0|255] "deg" XXX SG_ STEERING_ANGLE_2 : 128|16@1- (-0.1,0) [0|65535] "deg" XXX +BO_ 256 ACCELERATOR_BRAKE_ALT: 32 XXX + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ BRAKE_PRESSED : 32|1@1+ (1,0) [0|1] "" XXX + SG_ ACCELERATOR_PEDAL_PRESSED : 176|1@1+ (1,0) [0|1] "" XXX + BO_ 261 ACCELERATOR_ALT: 32 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX @@ -98,10 +118,10 @@ BO_ 261 ACCELERATOR_ALT: 32 XXX BO_ 293 STEERING_SENSORS: 16 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX - SG_ STEERING_RATE : 40|8@1- (-0.1,0) [0|255] "deg/s" XXX + SG_ STEERING_RATE : 40|8@1+ (4,0) [0|1016] "deg/s" XXX SG_ STEERING_ANGLE : 24|16@1- (-0.1,0) [0|255] "deg" XXX -BO_ 298 LFA: 16 XXX +BO_ 298 LFA: 16 ADRV SG_ STEER_REQ : 52|1@1+ (1,0) [0|1] "" XXX SG_ TORQUE_REQUEST : 41|11@1+ (1,-1024) [0|4095] "" XXX SG_ LKA_ICON : 38|2@1+ (1,0) [0|255] "" XXX @@ -124,15 +144,33 @@ BO_ 304 GEAR_SHIFTER: 16 XXX SG_ GEAR : 64|3@1+ (1,0) [0|7] "" XXX SG_ KNOB_POSITION : 40|3@1+ (1,0) [0|3] "" XXX -BO_ 373 SCC1: 24 XXX +BO_ 373 TCS: 24 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX - SG_ ACCEL_REQ : 48|13@1- (1,0) [0|1023] "" XXX - SG_ CRUISE_ACTIVE : 68|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 80|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 74|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 76|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_4 : 24|7@1+ (1,0) [0|127] "" XXX + SG_ aBasis : 32|11@1+ (0.01,-10.23) [0|7] "m/s^2" XXX + SG_ NEW_SIGNAL_5 : 72|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_6 : 128|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_7 : 135|2@0+ (1,0) [0|3] "" XXX + SG_ PROBABLY_EQUIP : 136|2@1+ (1,0) [0|3] "" XXX + SG_ AEB_EQUIP_MAYBE : 96|1@0+ (1,0) [0|1] "" XXX + SG_ EQUIP_MAYBE : 64|1@0+ (1,0) [0|1] "" XXX + SG_ DriverBraking : 81|1@0+ (1,0) [0|1] "" XXX + SG_ DriverBrakingLowSens : 84|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_REQ : 68|1@0+ (1,0) [0|1] "" XXX + SG_ ACCEL_REF_ACC : 48|11@1- (1,0) [0|1023] "" XXX -BO_ 352 CAM_0x160: 16 CAMERA +BO_ 352 ADRV_0x160: 16 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_FF : 64|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_FC : 72|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_2 : 56|8@1+ (1,0) [0|1] "" XXX + SG_ AEB_SETTING : 24|2@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_9 : 80|8@1+ (1,0) [0|255] "" XXX BO_ 384 CAM_0x180: 32 CAMERA SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX @@ -158,43 +196,43 @@ BO_ 389 CAM_0x185: 8 CAMERA SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX -BO_ 416 CRUISE_INFO: 32 XXX +BO_ 416 SCC_CONTROL: 32 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX - SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE3 : 24|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE4 : 32|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE5 : 40|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE6 : 48|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE7 : 56|8@1+ (1,0) [0|255] "" XXX SG_ NEW_SIGNAL_1 : 64|2@1+ (1,0) [0|3] "" XXX - SG_ CRUISE_MAIN : 66|1@1+ (1,0) [0|1] "" XXX - SG_ CRUISE_STATUS : 67|3@1+ (1,0) [0|7] "" XXX - SG_ CRUISE_INACTIVE : 70|1@1+ (1,0) [0|1] "" XXX - SG_ NEW_SIGNAL_2 : 71|5@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_8 : 170|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_10 : 156|5@0+ (1,0) [0|3] "" XXX + SG_ ZEROS : 215|48@0+ (1,0) [0|281474976710655] "" XXX + SG_ ZEROS_3 : 191|7@0+ (1,0) [0|127] "" XXX + SG_ ZEROS_4 : 183|4@0+ (1,0) [0|63] "" XXX + SG_ ZEROS_6 : 119|16@0+ (1,0) [0|65535] "" XXX + SG_ ZEROS_8 : 95|5@0+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_3 : 109|2@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_TMP_64 : 55|8@0+ (1,0) [0|63] "" XXX + SG_ SET_ME_2 : 105|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_6 : 104|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ ZEROS_9 : 71|5@1+ (1,0) [0|15] "" XXX + SG_ ZEROS_10 : 111|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_3 : 45|2@0+ (1,0) [0|3] "" XXX + SG_ ObjValid : 46|1@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_2 : 168|2@1+ (1,0) [0|3] "" XXX + SG_ OBJ_STATUS : 176|3@1+ (1,0) [0|7] "" XXX + SG_ ACC_ObjDist : 24|11@1+ (0.1,0) [0|204.7] "m" XXX + SG_ ZEROS_5 : 77|11@1+ (1,0) [0|2047] "" XXX + SG_ DISTANCE_SETTING : 88|3@1+ (1,0) [0|3] "" XXX + SG_ ZEROS_2 : 207|5@0+ (1,0) [0|63] "" XXX SG_ CRUISE_STANDSTILL : 76|1@1+ (1,0) [0|1] "" XXX - SG_ NEW_SIGNAL_3 : 77|11@1+ (1,0) [0|2047] "" XXX - SG_ BYTE11 : 88|8@1+ (1,0) [0|127] "" XXX - SG_ SET_SPEED : 102|7@0+ (1,0) [0|127] "km/h or mph" XXX - SG_ NEW_SIGNAL_4 : 103|1@1+ (1,0) [0|1] "" XXX - SG_ BYTE13 : 104|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE14 : 112|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE15 : 120|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE16 : 128|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE17 : 136|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE18 : 144|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE19 : 152|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE20 : 160|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE21 : 168|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE22 : 176|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE23 : 184|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE24 : 192|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE25 : 200|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE26 : 208|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE27 : 216|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE28 : 224|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE29 : 232|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE30 : 240|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE31 : 248|8@1+ (1,0) [0|255] "" XXX + SG_ aReqRaw : 140|11@1+ (0.01,-10.23) [-10.23|10.24] "m/s^2" XXX + SG_ aReqValue : 128|11@1+ (0.01,-10.23) [-10.23|10.24] "m/s^2" XXX + SG_ ZEROS_7 : 63|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 158|2@0+ (1,0) [0|3] "" XXX + SG_ ACCMode : 68|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_12 : 35|9@1+ (0.1,0) [0|255] "" XXX + SG_ JerkLowerLimit : 166|7@0+ (0.1,0) [0|12.7] "m/s^3" XXX + SG_ StopReq : 184|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_15 : 192|11@1+ (0.1,0) [0|204.7] "m" XXX + SG_ VSetDis : 103|8@0+ (1,0) [0|255] "km/h or mph" XXX + SG_ MainMode_ACC : 66|1@1+ (1,0) [0|1] "" XXX BO_ 426 CRUISE_BUTTONS_ALT: 16 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX @@ -247,16 +285,136 @@ BO_ 463 CRUISE_BUTTONS: 8 XXX SG_ COUNTER : 12|4@1+ (1,0) [0|255] "" XXX SG_ CRUISE_BUTTONS : 16|3@1+ (1,0) [0|3] "" XXX -BO_ 480 LFAHDA_CLUSTER: 16 XXX +BO_ 474 ADRV_0x1da: 32 ADRV + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_22 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_41 : 47|8@0+ (1,0) [0|255] "" XXX + +BO_ 480 LFAHDA_CLUSTER: 16 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX SG_ HDA_ICON : 31|1@1+ (1,0) [0|1] "" XXX SG_ LFA_ICON : 47|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_1 : 32|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_2 : 30|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 49|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_4 : 24|1@0+ (1,0) [0|1] "" XXX + +BO_ 490 ADRV_0x1ea: 32 ADRV + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_1C : 31|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_1 : 32|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_2 : 47|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_3 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 64|6@1+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_5 : 72|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_6 : 75|5@1+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_7 : 80|5@1+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_8 : 88|7@1+ (1,0) [0|127] "" XXX + SG_ NEW_SIGNAL_9 : 96|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_10 : 143|5@0+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_11 : 144|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_12 : 152|6@1+ (1,0) [0|63] "" XXX + SG_ NEW_SIGNAL_13 : 160|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_14 : 163|5@1+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_15 : 175|4@0+ (1,0) [0|63] "" XXX + SG_ NEW_SIGNAL_16 : 168|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_17 : 176|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_18 : 184|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_19 : 208|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_20 : 212|1@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_FF : 120|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_TMP_F : 232|5@1+ (1,0) [0|31] "" XXX + SG_ SET_ME_TMP_F_2 : 240|5@1+ (1,0) [0|31] "" XXX BO_ 507 CAM_0x1fb: 32 CAMERA SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX +BO_ 512 ADRV_0x200: 8 ADRV + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_E1 : 24|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_3A : 32|8@1+ (1,0) [0|255] "" XXX + +BO_ 513 RADAR_0x201: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 528 RADAR_0x210: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 529 RADAR_0x211: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 530 RADAR_0x212: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 531 RADAR_0x213: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 532 RADAR_0x214: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 533 RADAR_0x215: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 534 RADAR_0x216: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 535 RADAR_0x217: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 536 RADAR_0x218: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 537 RADAR_0x219: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 538 RADAR_0x21a: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 539 RADAR_0x21b: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 540 RADAR_0x21c: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 541 RADAR_0x21d: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 542 RADAR_0x21e: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 543 RADAR_0x21f: 32 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 576 RADAR_0x240: 16 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + +BO_ 593 RADAR_0x251: 16 FRONT_RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + BO_ 674 CAM_0x2a2: 32 CAMERA SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX @@ -306,6 +464,11 @@ BO_ 702 CAM_0x2be: 32 CAMERA SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX +BO_ 837 ADRV_0x345: 8 ADRV + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_15 : 24|8@1+ (1,0) [0|255] "" XXX + BO_ 961 BLINKER_STALKS: 8 XXX SG_ COUNTER_ALT : 15|4@0+ (1,0) [0|15] "" XXX SG_ CHECKSUM_MAYBE : 7|8@0+ (1,0) [0|255] "" XXX @@ -329,19 +492,70 @@ BO_ 1043 BLINKERS: 8 XXX BO_ 1240 CLUSTER_INFO: 8 XXX SG_ DISTANCE_UNIT : 0|1@1+ (1,0) [0|1] "" XXX +BO_ 442 BLINDSPOTS_REAR_CORNERS: 24 XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_2 : 96|1@0+ (1,0) [0|1] "" XXX + SG_ COLLISION_AVOIDANCE_ACTIVE : 68|1@0+ (1,0) [0|1] "" XXX + SG_ LEFT_MB : 30|1@0+ (1,0) [0|3] "" XXX + SG_ LEFT_BLOCKED : 24|1@0+ (1,0) [0|1] "" XXX + SG_ MORE_LEFT_PROB : 32|1@1+ (1,0) [0|3] "" XXX + SG_ FL_INDICATOR : 46|6@0+ (1,0) [0|1] "" XXX + SG_ FR_INDICATOR : 54|6@0+ (1,0) [0|63] "" XXX + SG_ RIGHT_BLOCKED : 64|1@0+ (1,0) [0|1] "" XXX + +BO_ 874 BLINDSPOTS_FRONT_CORNER_2: 16 XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + +BO_ 485 BLINDSPOTS_FRONT_CORNER_1: 16 XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_1 : 108|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_2 : 96|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 88|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_4 : 80|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_5 : 31|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_6 : 24|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_7 : 32|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_8 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_9 : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 506 CLUSTER_SPEED_LIMIT: 32 XXX + SG_ SPEED_LIMIT_3 : 119|8@0+ (1,0) [0|255] "" XXX + SG_ SPEED_LIMIT_2 : 47|7@0+ (1,0) [0|255] "" XXX + SG_ SPEED_LIMIT_1 : 39|7@0+ (1,0) [0|255] "" XXX + SG_ SPEED_CHANGE_BLINKING : 129|1@1+ (1,0) [0|3] "" XXX + SG_ CHIME_2 : 122|2@1+ (1,0) [0|7] "" XXX + SG_ CHIME_1 : 133|1@0+ (1,0) [0|1] "" XXX + SG_ ARROW_DOWN : 120|1@0+ (1,0) [0|1] "" XXX + SG_ ARROW_UP : 121|1@0+ (1,0) [0|1] "" XXX + SG_ SECONDARY_LIMIT_1 : 79|8@0+ (1,0) [0|127] "" XXX + SG_ SECONDARY_LIMIT_2 : 103|8@0+ (1,0) [0|127] "" XXX + SG_ SCHOOL_ZONE : 155|1@0+ (1,0) [0|1] "" XXX + +CM_ SG_ 96 BRAKE_PRESSURE "User applied brake pedal pressure. Ramps from computer applied pressure on falling edge of cruise. Cruise cancels if !=0"; +CM_ SG_ 101 BRAKE_POSITION "User applied brake pedal position, max is ~700. Signed on some vehicles"; +CM_ SG_ 373 PROBABLY_EQUIP "aeb equip?"; +CM_ SG_ 373 DriverBraking "Likely derived from BRAKE->BRAKE_POSITION"; +CM_ SG_ 373 DriverBrakingLowSens "Higher threshold version of DriverBraking"; +CM_ SG_ 352 SET_ME_9 "has something to do with AEB settings"; +CM_ SG_ 416 VSetDis "set speed in display units"; CM_ SG_ 961 COUNTER_ALT "only increments on change"; CM_ SG_ 1041 COUNTER_ALT "only increments on change"; CM_ SG_ 1043 COUNTER_ALT "only increments on change"; - VAL_ 53 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ; +VAL_ 64 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ; VAL_ 69 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ; -VAL_ 304 GEAR 1 "P" 2 "R" 3 "N" 4 "D"; VAL_ 80 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ; VAL_ 80 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ; +VAL_ 234 LKA_FAULT 0 "ok" 1 "lka fault" ; VAL_ 298 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ; VAL_ 298 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ; -VAL_ 416 CRUISE_STATUS 0 "cruise_inactive" 2 "cruise_active" 4 "driver_override" ; +VAL_ 304 GEAR 1 "P" 2 "R" 3 "N" 4 "D" ; +VAL_ 352 AEB_SETTING 1 "off" 2 "warning only" 3 "active assist" ; +VAL_ 416 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled" ; VAL_ 426 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ; VAL_ 463 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ; diff --git a/opendbc/hyundai_kia_generic.dbc b/opendbc/hyundai_kia_generic.dbc index c09b59f40..c14193440 100644 --- a/opendbc/hyundai_kia_generic.dbc +++ b/opendbc/hyundai_kia_generic.dbc @@ -1440,11 +1440,10 @@ BO_ 909 FCA11: 8 FCA SG_ FCA_DrvSetStatus : 22|3@1+ (1,0) [0|7] "" CLU,ESC SG_ CF_VSM_DecCmdAct : 31|1@1+ (1,0) [0|1] "" ESC SG_ FCA_Failinfo : 32|3@1+ (1,0) [0|7] "" ACU,CLU,ESC + SG_ CR_FCA_Alive : 35|4@1+ (1,0) [0|15] "" ESC SG_ FCA_RelativeVelocity : 39|9@1+ (0.1,-25.5) [-25.5|25.5] "m/s" iBAU SG_ FCA_TimetoCollision : 48|8@1+ (10,0) [0|2540] "ms" iBAU - SG_ CR_FCA_Alive : 56|4@1+ (1,0) [0|15] "" ESC - SG_ CR_FCA_ChkSum : 60|4@1+ (1,0) [0|15] "" ESC - SG_ Supplemental_Counter : 35|4@1+ (1,0) [0|15] "" XXX + SG_ CR_FCA_ChkSum : 56|8@1+ (1,0) [0|255] "" ESC SG_ PAINT1_Status : 16|2@1+ (1,0) [0|1] "" XXX BO_ 1156 HDA11_MFC: 8 XXX @@ -1645,10 +1644,11 @@ CM_ "BO_ E_EMS11: All (plug-in) hybrids use this gas signal: CR_Vcu_AccPedDep_Po CM_ SG_ 1348 SpeedLim_Nav_Clu "Speed limit displayed on Nav, Cluster and HUD"; VAL_ 274 CUR_GR 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D" 7 "D" 8 "D" 14 "R" 0 "P"; -VAL_ 871 CF_Lvr_Gear 5 "D" 8 "S" 6 "N" 7 "R" 0 "P"; +VAL_ 871 CF_Lvr_Gear 12 "T" 5 "D" 8 "S" 6 "N" 7 "R" 0 "P"; VAL_ 882 Elect_Gear_Shifter 5 "D" 8 "S" 6 "N" 7 "R" 0 "P"; VAL_ 905 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled"; VAL_ 909 CF_VSM_Warn 2 "FCW" 3 "AEB"; +VAL_ 916 ACCEnable 0 "SCC ready" 1 "SCC temp fault" 2 "SCC permanent fault" 3 "SCC permanent fault, communication issue"; VAL_ 1057 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault"; VAL_ 1157 HDA_Icon_State 0 "no_hda" 1 "white_hda" 2 "green_hda"; VAL_ 1157 LFA_SysWarning 0 "no_message" 1 "switching_to_hda" 2 "switching_to_scc" 3 "lfa_error" 4 "check_hda" 5 "keep_hands_on_wheel_orange" 6 "keep_hands_on_wheel_red"; diff --git a/opendbc/hyundai_kia_mando_front_radar.dbc b/opendbc/hyundai_kia_mando_front_radar_generated.dbc similarity index 98% rename from opendbc/hyundai_kia_mando_front_radar.dbc rename to opendbc/hyundai_kia_mando_front_radar_generated.dbc index 552cdddea..4abd25331 100644 --- a/opendbc/hyundai_kia_mando_front_radar.dbc +++ b/opendbc/hyundai_kia_mando_front_radar_generated.dbc @@ -1,3 +1,6 @@ +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; + +CM_ "hyundai_kia_mando_front_radar.dbc starts here"; VERSION "" @@ -35,8 +38,7 @@ NS_ : BS_: BU_: XXX - - + BO_ 1280 RADAR_TRACK_500: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -48,8 +50,7 @@ BO_ 1280 RADAR_TRACK_500: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1281 RADAR_TRACK_501: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -61,8 +62,7 @@ BO_ 1281 RADAR_TRACK_501: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1282 RADAR_TRACK_502: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -74,8 +74,7 @@ BO_ 1282 RADAR_TRACK_502: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1283 RADAR_TRACK_503: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -87,8 +86,7 @@ BO_ 1283 RADAR_TRACK_503: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1284 RADAR_TRACK_504: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -100,8 +98,7 @@ BO_ 1284 RADAR_TRACK_504: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1285 RADAR_TRACK_505: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -113,8 +110,7 @@ BO_ 1285 RADAR_TRACK_505: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1286 RADAR_TRACK_506: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -126,8 +122,7 @@ BO_ 1286 RADAR_TRACK_506: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1287 RADAR_TRACK_507: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -139,8 +134,7 @@ BO_ 1287 RADAR_TRACK_507: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1288 RADAR_TRACK_508: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -152,8 +146,7 @@ BO_ 1288 RADAR_TRACK_508: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1289 RADAR_TRACK_509: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -165,8 +158,7 @@ BO_ 1289 RADAR_TRACK_509: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1290 RADAR_TRACK_50a: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -178,8 +170,7 @@ BO_ 1290 RADAR_TRACK_50a: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1291 RADAR_TRACK_50b: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -191,8 +182,7 @@ BO_ 1291 RADAR_TRACK_50b: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1292 RADAR_TRACK_50c: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -204,8 +194,7 @@ BO_ 1292 RADAR_TRACK_50c: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1293 RADAR_TRACK_50d: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -217,8 +206,7 @@ BO_ 1293 RADAR_TRACK_50d: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1294 RADAR_TRACK_50e: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -230,8 +218,7 @@ BO_ 1294 RADAR_TRACK_50e: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1295 RADAR_TRACK_50f: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -243,8 +230,7 @@ BO_ 1295 RADAR_TRACK_50f: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1296 RADAR_TRACK_510: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -256,8 +242,7 @@ BO_ 1296 RADAR_TRACK_510: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1297 RADAR_TRACK_511: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -269,8 +254,7 @@ BO_ 1297 RADAR_TRACK_511: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1298 RADAR_TRACK_512: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -282,8 +266,7 @@ BO_ 1298 RADAR_TRACK_512: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1299 RADAR_TRACK_513: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -295,8 +278,7 @@ BO_ 1299 RADAR_TRACK_513: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1300 RADAR_TRACK_514: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -308,8 +290,7 @@ BO_ 1300 RADAR_TRACK_514: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1301 RADAR_TRACK_515: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -321,8 +302,7 @@ BO_ 1301 RADAR_TRACK_515: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1302 RADAR_TRACK_516: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -334,8 +314,7 @@ BO_ 1302 RADAR_TRACK_516: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1303 RADAR_TRACK_517: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -347,8 +326,7 @@ BO_ 1303 RADAR_TRACK_517: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1304 RADAR_TRACK_518: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -360,8 +338,7 @@ BO_ 1304 RADAR_TRACK_518: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1305 RADAR_TRACK_519: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -373,8 +350,7 @@ BO_ 1305 RADAR_TRACK_519: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1306 RADAR_TRACK_51a: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -386,8 +362,7 @@ BO_ 1306 RADAR_TRACK_51a: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1307 RADAR_TRACK_51b: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -399,8 +374,7 @@ BO_ 1307 RADAR_TRACK_51b: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1308 RADAR_TRACK_51c: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -412,8 +386,7 @@ BO_ 1308 RADAR_TRACK_51c: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1309 RADAR_TRACK_51d: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -425,8 +398,7 @@ BO_ 1309 RADAR_TRACK_51d: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1310 RADAR_TRACK_51e: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -438,8 +410,7 @@ BO_ 1310 RADAR_TRACK_51e: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - - + BO_ 1311 RADAR_TRACK_51f: 8 RADAR SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX @@ -451,4 +422,4 @@ BO_ 1311 RADAR_TRACK_51f: 8 RADAR SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX - + \ No newline at end of file diff --git a/opendbc/toyota_new_mc_pt_generated.dbc b/opendbc/toyota_new_mc_pt_generated.dbc index e28893e4c..708560f64 100644 --- a/opendbc/toyota_new_mc_pt_generated.dbc +++ b/opendbc/toyota_new_mc_pt_generated.dbc @@ -249,7 +249,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM BO_ 1009 PCM_CRUISE_ALT: 8 XXX SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX - SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX + SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX BO_ 1020 SOLAR_SENSOR: 8 XXX SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX @@ -373,6 +373,7 @@ BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX @@ -439,7 +440,7 @@ CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; CM_ SG_ 956 GEAR "on 6MT, only R shows."; -CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to"; +CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to"; CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator"; CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity"; CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle"; @@ -472,7 +473,7 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; -CM_ SG_ 1552 UI_SPEED "Vehicle's speedometer's display speed after Toyota's offset was applied, uses vehicle's unit"; +CM_ SG_ 1552 UI_SPEED "Does not appear to match dash"; CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state"; CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting"; CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state"; @@ -524,6 +525,7 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on"; VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed"; +VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)"; VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index c35e441df..0c6e1cf9d 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -249,7 +249,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM BO_ 1009 PCM_CRUISE_ALT: 8 XXX SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX - SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX + SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX BO_ 1020 SOLAR_SENSOR: 8 XXX SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX @@ -373,6 +373,7 @@ BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX @@ -439,7 +440,7 @@ CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; CM_ SG_ 956 GEAR "on 6MT, only R shows."; -CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to"; +CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to"; CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator"; CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity"; CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle"; @@ -472,7 +473,7 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; -CM_ SG_ 1552 UI_SPEED "Vehicle's speedometer's display speed after Toyota's offset was applied, uses vehicle's unit"; +CM_ SG_ 1552 UI_SPEED "Does not appear to match dash"; CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state"; CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting"; CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state"; @@ -524,6 +525,7 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on"; VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed"; +VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)"; VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked"; diff --git a/opendbc/toyota_tnga_k_pt_generated.dbc b/opendbc/toyota_tnga_k_pt_generated.dbc index f0e40dc4a..2eaef636e 100644 --- a/opendbc/toyota_tnga_k_pt_generated.dbc +++ b/opendbc/toyota_tnga_k_pt_generated.dbc @@ -249,7 +249,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM BO_ 1009 PCM_CRUISE_ALT: 8 XXX SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX - SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX + SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX BO_ 1020 SOLAR_SENSOR: 8 XXX SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX @@ -373,6 +373,7 @@ BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX @@ -439,7 +440,7 @@ CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; CM_ SG_ 956 GEAR "on 6MT, only R shows."; -CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to"; +CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to"; CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator"; CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity"; CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle"; @@ -472,7 +473,7 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; -CM_ SG_ 1552 UI_SPEED "Vehicle's speedometer's display speed after Toyota's offset was applied, uses vehicle's unit"; +CM_ SG_ 1552 UI_SPEED "Does not appear to match dash"; CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state"; CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting"; CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state"; @@ -524,6 +525,7 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on"; VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed"; +VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)"; VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked"; diff --git a/opendbc/vw_golf_mk4.dbc b/opendbc/vw_golf_mk4.dbc index 66681dbd5..b266a7f4c 100644 --- a/opendbc/vw_golf_mk4.dbc +++ b/opendbc/vw_golf_mk4.dbc @@ -296,7 +296,9 @@ BO_ 1408 Motor_Flexia: 8 XXX SG_ Verschleissindex : 16|8@1+ (1,0) [0|254] "" XXX SG_ Russindex : 8|8@1+ (1,0) [0|254] "" XXX SG_ Verbrennungsart : 7|1@1+ (1,0) [0|0] "" XXX - SG_ Frei_Motor_Flexia_1 : 4|3@1+ (1,0) [0|0] "" XXX + SG_ Frei_Motor_Flexia_1 : 6|1@1+ (1,0) [0|0] "" XXX + SG_ Warm_Up_Cycle : 5|1@1+ (1,0) [0|0] "" XXX + SG_ Driving_Cycle : 4|1@1+ (1,0) [0|0] "" XXX SG_ Zaehler_Motor_Flexia : 1|3@1+ (1,0) [0|15] "" XXX SG_ Multiplex_Schalter_Motor_Flexia M : 0|1@1+ (1,0) [0|0] "" XXX @@ -1121,7 +1123,7 @@ BO_ 208 Lenkhilfe_3: 6 XXX SG_ LH3_BLWValid : 45|1@1+ (1,0) [0|1] "" XXX SG_ LH3_Lenkungstyp : 46|2@1+ (1,0) [0|3] "" XXX -BO_ 978 Lenkhilfe_2: 8 XXX +BO_ 978 Lenkhilfe_2: 7 XXX SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX SG_ LH2_Geradeaus : 12|1@1+ (1,0) [0|1] "" XXX @@ -1457,19 +1459,28 @@ CM_ SG_ 640 Fahrerwunschmoment "Driver Requested Torque"; CM_ SG_ 640 mechanisches_Motor_Verlustmomen "Mechanical Torque Loss"; CM_ SG_ 640 Fahrpedalwert_oder_Drosselklapp "Accelerator Pedal or Throttle Position"; CM_ SG_ 640 Motordrehzahl "Engine Speed"; +CM_ SG_ 640 Momentenangaben_ungenau "Approximate Torque Values"; CM_ SG_ 644 MOB_CHECKSUM "Checksum MOB"; CM_ SG_ 644 MOB_COUNTER "Counter MOB"; +CM_ SG_ 648 Minimales_Motormoment_bei_Zuend "Minimum Torque Adjustment"; +CM_ SG_ 648 Begrenzungsmoment "Maximum Inner Torque"; CM_ SG_ 648 Bremstestschalter "Brake Test Switch"; +CM_ SG_ 648 Soll_Geschwindigkeit_bei_GRA_Be "Desired Vehicle Speed"; CM_ SG_ 648 Bremslichtschalter "Brake Light Switch"; CM_ SG_ 648 Leerlaufsolldrehzahl__Motor_2_ "Target Idle Speed"; CM_ SG_ 648 Fahrzeuggeschwindigkeit "Vehicle Speed"; CM_ SG_ 648 Kuehlmitteltemperatur__Motor_2_ "Coolant Temperature"; CM_ SG_ 896 Drosselklappenpoti "Throttle Position"; +CM_ SG_ 896 Motor_Wunschdrehzahl "Desired engine speed"; +CM_ SG_ 896 Motordrehzahlbeeinflussung "Shift Target Influence"; CM_ SG_ 896 Fahrpedal_Rohsignal "Accelerator Pedal Position"; CM_ SG_ 896 Ansauglufttemperatur "Intake Air Temperature"; +CM_ SG_ 896 Kein_E_Gas "ETB flag"; +CM_ SG_ 896 Kein_Start_Stop "Start/stop flag"; +CM_ SG_ 896 Rad_Wunschmoment "Desired wheel torque"; CM_ SG_ 912 GK1_Fa_Tuerkont "Status of the driver's door rotary latch"; CM_ SG_ 912 BSK_HL_geoeffnet "Status of the rear left door rotary latch"; @@ -1480,6 +1491,15 @@ CM_ SG_ 912 BSK_HD_Hauptraste "Status of trunk lid main detent"; CM_ SG_ 1088 Zaehler_Getriebe_1 "Counter Getriebe_1"; CM_ SG_ 1088 Waehlhebelposition__Getriebe_1_ "Gear Selector Position"; +CM_ SG_ 1056 Fehlerstatus_Aussentemp__4_1 "ambient temp error"; +CM_ SG_ 1056 Fehlerstatus_Oeltemperatur_4_1 "oil temp error"; +CM_ SG_ 1056 Fehlerst__Kuehlmitteltemp__4_1 "water temp error"; +CM_ SG_ 1056 Aussentemperatur_gefiltert "outside temp, filtered"; +CM_ SG_ 1056 Oeltemperatur_4_1 "kombi oil temperature"; +CM_ SG_ 1056 Kuehlmitteltemp__4_1__Kombi_2_ "kombi coolant temperature"; + +CM_ SG_ 1096 Zaehler_Waehlhebel_1 "Counter Waehlhebel_1"; + CM_ SG_ 1152 Checksumme_Motor_5 "Checksum Motor_5"; CM_ SG_ 1160 Zaehler_Motor_6 "Counter Motor_6"; @@ -1492,6 +1512,16 @@ CM_ SG_ 1344 Zahler_Getriebe_2 "Counter Getriebe_2"; CM_ SG_ 1386 ACA_V_Wunsch "255=unset"; +CM_ SG_ 1408 Zaehler_Motor_Flexia "Counter Motor_Flexia"; +CM_ SG_ 1408 Verbrennungsart "Type of combustion"; +CM_ SG_ 1408 Max_Drehmoment "Maximum torque"; +CM_ SG_ 1408 Drehzahl_MaxNorm "RPM of maximum torque"; +CM_ SG_ 1408 Hubraum "Displacement"; +CM_ SG_ 1408 Anzahl_Zylinder "Number of cylinders"; +CM_ SG_ 1408 Anzahl_Ventile "Number of valves"; +CM_ SG_ 1408 Ansaugsystem "Induction System"; +CM_ SG_ 1408 Motorleistung "Maximum engine power"; + CM_ SG_ 1416 Ladedruck "Boost Pressure"; CM_ SG_ 1470 LDW_Direction "0=right,1=left"; diff --git a/opendbc/vw_mqb_2010.dbc b/opendbc/vw_mqb_2010.dbc index 031f77e91..f1adeab84 100644 --- a/opendbc/vw_mqb_2010.dbc +++ b/opendbc/vw_mqb_2010.dbc @@ -36,39 +36,69 @@ BS_: BU_: Airbag_MQB BAP_Tester_MQB BMS_MQB Datenlogger_MQB Gateway_MQB Getriebe_DQ_Hybrid_MQB Getriebe_DQ_MQB LEH_MQB Motor_Diesel_MQB Motor_Hybrid_MQB Motor_Otto_MQB SAK_MQB Waehlhebel_MQB Vector__XXX l c i XXX -BO_ 290 ACC_06: 8 Gateway_MQB - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_limitierte_Anfahrdyn : 12|1@1+ (1,0) [0|1] "" Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_zul_Regelabw_unten : 16|6@1+ (0.024,0) [0|1.512] "Unit_MeterPerSeconSquar" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_StartStopp_Info : 22|2@1+ (1,0) [0|3] "" Motor_Diesel_MQB,Motor_Otto_MQB - SG_ ACC_Sollbeschleunigung_02 : 24|11@1+ (0.005,-7.22) [-7.22|3.005] "Unit_MeterPerSeconSquar" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_zul_Regelabw_oben : 35|5@1+ (0.0625,0) [0|1.9375] "Unit_MeterPerSeconSquar" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_neg_Sollbeschl_Grad_02 : 40|8@1+ (0.05,0) [0|12.75] "Unit_MeterPerCubicSecon" Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_pos_Sollbeschl_Grad_02 : 48|8@1+ (0.05,0) [0|12.75] "Unit_MeterPerCubicSecon" Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_Anfahren : 56|1@1+ (1,0) [0|1] "" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_Anhalten : 57|1@1+ (1,0) [0|1] "" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_Typ : 58|2@1+ (1,0) [0|3] "" Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_Status_ACC : 60|3@1+ (1,0) [0|7] "" Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB - SG_ ACC_Minimale_Bremsung : 63|1@1+ (1,0) [0|1] "" Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB +BO_ 290 ACC_06: 8 XXX + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX + SG_ ACC_limitierte_Anfahrdyn : 12|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_nachtr_Stopp_Anf : 13|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_DynErhoehung : 14|1@1+ (1.0,0.0) [0.0|1] "" XXX + SG_ ACC_Freilaufstrategie_TSK : 15|1@1+ (1.0,0.0) [0.0|1] "" XXX + SG_ ACC_zul_Regelabw_unten : 16|6@1+ (0.024,0) [0|1.512] "Unit_MeterPerSeconSquar" XXX + SG_ ACC_StartStopp_Info : 22|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Sollbeschleunigung_02 : 24|11@1+ (0.005,-7.22) [-7.22|3.005] "Unit_MeterPerSeconSquar" XXX + SG_ ACC_zul_Regelabw_oben : 35|5@1+ (0.0625,0) [0|1.9375] "Unit_MeterPerSeconSquar" XXX + SG_ ACC_neg_Sollbeschl_Grad_02 : 40|8@1+ (0.05,0) [0|12.75] "Unit_MeterPerCubicSecon" XXX + SG_ ACC_pos_Sollbeschl_Grad_02 : 48|8@1+ (0.05,0) [0|12.75] "Unit_MeterPerCubicSecon" XXX + SG_ ACC_Anfahren : 56|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Anhalten : 57|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Typ : 58|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Status_ACC : 60|3@1+ (1,0) [0|7] "" XXX + SG_ ACC_Minimale_Bremsung : 63|1@1+ (1,0) [0|1] "" XXX -BO_ 279 ACC_10: 8 Gateway_MQB - SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" Airbag_MQB - SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" Airbag_MQB - SG_ AWV1_Anf_Prefill : 16|1@1+ (1,0) [0|1] "" Airbag_MQB - SG_ ANB_CM_Info : 17|1@1+ (1,0) [0|1] "" Vector__XXX - SG_ AWV2_Freigabe : 18|1@1+ (1,0) [0|1] "" Airbag_MQB - SG_ AWV1_HBA_Param : 19|2@1+ (1,0) [0|3] "" Vector__XXX - SG_ AWV2_Ruckprofil : 21|3@1+ (1,0) [0|7] "" Vector__XXX - SG_ AWV2_Priowarnung : 24|1@1+ (1,0) [0|1] "" Vector__XXX - SG_ ANB_CM_Anforderung : 25|1@1+ (1,0) [0|1] "" Airbag_MQB - SG_ ANB_Info_Teilbremsung : 26|1@1+ (1,0) [0|1] "" Vector__XXX - SG_ ANB_Notfallblinken : 27|1@1+ (1,0) [0|1] "" Vector__XXX - SG_ ANB_Teilbremsung_Freigabe : 28|1@1+ (1,0) [0|1] "" Airbag_MQB - SG_ ANB_Zielbrems_Teilbrems_Verz_Anf : 29|10@1+ (0.024,-20.016) [-20.016|4.536] "Unit_MeterPerSeconSquar" Airbag_MQB - SG_ ANB_Zielbremsung_Freigabe : 39|1@1+ (1,0) [0|1] "" Airbag_MQB - SG_ AWV_Vorstufe : 40|1@1+ (1,0) [0|1] "" Vector__XXX - SG_ AWV_Halten : 41|1@1+ (1,0) [0|1] "" Vector__XXX +BO_ 279 ACC_10: 8 XXX + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX + SG_ AWV1_Anf_Prefill : 16|1@1+ (1,0) [0|1] "" XXX + SG_ ANB_CM_Info : 17|1@1+ (1,0) [0|1] "" XXX + SG_ AWV2_Freigabe : 18|1@1+ (1,0) [0|1] "" XXX + SG_ AWV1_HBA_Param : 19|2@1+ (1,0) [0|3] "" XXX + SG_ AWV2_Ruckprofil : 21|3@1+ (1,0) [0|7] "" XXX + SG_ AWV2_Priowarnung : 24|1@1+ (1,0) [0|1] "" XXX + SG_ ANB_CM_Anforderung : 25|1@1+ (1,0) [0|1] "" XXX + SG_ ANB_Info_Teilbremsung : 26|1@1+ (1,0) [0|1] "" XXX + SG_ ANB_Notfallblinken : 27|1@1+ (1,0) [0|1] "" XXX + SG_ ANB_Teilbremsung_Freigabe : 28|1@1+ (1,0) [0|1] "" XXX + SG_ ANB_Zielbrems_Teilbrems_Verz_Anf : 29|10@1+ (0.024,-20.016) [-20.016|4.536] "Unit_MeterPerSeconSquar" XXX + SG_ ANB_Zielbremsung_Freigabe : 39|1@1+ (1,0) [0|1] "" XXX + SG_ AWV_Vorstufe : 40|1@1+ (1,0) [0|1] "" XXX + SG_ AWV_Halten : 41|1@1+ (1,0) [0|1] "" XXX + SG_ AWV_CityANB_Auspraegung : 42|1@1+ (1,0) [0|1] "" XXX + SG_ PCF_Freigabe : 43|1@1+ (1,0) [0|1] "" XXX + SG_ AWV1_ECD_Anlauf : 44|1@1+ (1,0) [0|1] "" XXX + SG_ AWV_AWA_VZ_Anf_Lenkmomoffset : 46|1@1+ (1.0,0.0) [0.0|1] "" XXX + SG_ AWV_AWA_Anf_Lenkmomoffset : 47|9@1+ (0.01,0) [0.00|5.11] "Unit_NewtoMeter" XXX + SG_ PCF_Time_to_collision : 56|8@1+ (0.01,0) [0|2.5] "Unit_Secon" XXX + +BO_ 679 ACC_13: 8 XXX + SG_ ACC_Regelgeschw : 12|10@1+ (0.32,0) [0|327.04] "Unit_KiloMeterPerHour" XXX + SG_ ACC_Einheit_maxSetzgeschw : 22|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_maxSetzgeschw : 23|9@1+ (1,0) [0|510] "" XXX + SG_ ACC_minRegelgeschw : 32|8@1+ (0.32,0) [0|81.28] "Unit_KiloMeterPerHour" XXX + SG_ ACC_maxRegelgeschw : 40|8@1+ (0.32,0) [0|81.28] "Unit_KiloMeterPerHour" XXX + SG_ ACC_Tempolimitassistent : 48|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Kurvenassistent : 52|3@1+ (1,0) [0|7] "" XXX + SG_ ACC_RUV : 56|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Tachokranz : 58|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Typ_Tachokranz_unten : 59|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_ENG_Texte : 60|2@1+ (1,0) [0|3] "" XXX + +BO_ 681 ACC_15: 8 XXX + SG_ AWV_Warnung : 16|3@1+ (1,0) [0|7] "" XXX + SG_ AWV_Texte : 24|3@1+ (1,0) [0|7] "" XXX + SG_ AWV_Status_Anzeige : 32|2@1+ (1,0) [0|3] "" XXX + SG_ AWV_Einstellung_System_FSG : 34|1@1+ (1,0) [0|1] "" XXX + SG_ AWV_Einstellung_Warnung_FSG : 36|3@1+ (1,0) [0|7] "" XXX + SG_ AWV_Warnlevel : 58|6@1+ (1,0) [0|63] "" XXX BO_ 64 Airbag_01: 8 Airbag_MQB SG_ Airbag_01_CRC : 0|8@1+ (1,0) [0|255] "" BMS_MQB,Gateway_MQB,LEH_MQB,Motor_Diesel_MQB,Motor_Hybrid_MQB,Motor_Otto_MQB @@ -1237,33 +1267,35 @@ BO_ 919 LDW_02: 8 XXX BO_ 780 ACC_02: 8 XXX SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX - SG_ ACC_Wunschgeschw : 12|10@1+ (0.32,0) [0.00|326.72] "Unit_KiloMeterPerHour" XXX - SG_ ACC_Status_Prim_Anz : 22|2@1+ (1.0,0.0) [0.0|3] "" XXX + SG_ ACC_Wunschgeschw_02 : 12|10@1+ (0.32,0) [0|327.04] "Unit_KiloMeterPerHour" XXX + SG_ ACC_Status_Prim_Anz : 22|2@1+ (1,0) [0|3] "" XXX SG_ ACC_Abstandsindex : 24|10@1+ (1,0) [1|1021] "" XXX - SG_ ACC_Akustik : 34|3@1+ (1.0,0.0) [0.0|7] "" XXX - SG_ ACC_Gesetzte_Zeitluecke : 37|3@1+ (1.0,0.0) [0.0|7] "" XXX - SG_ ACC_Optischer_Fahrerhinweis : 40|1@1+ (1.0,0.0) [0.0|1] "" XXX - SG_ ACC_Typ_Tachokranz : 41|1@1+ (1.0,0.0) [0.0|1] "" XXX - SG_ ACC_Anzeige_Zeitluecke : 42|1@1+ (1.0,0.0) [0.0|1] "" XXX - SG_ ACC_Tachokranz : 43|1@1+ (1.0,0.0) [0.0|1] "" XXX - SG_ ACC_Display_Prio : 44|2@1+ (1.0,0.0) [0.0|3] "" XXX - SG_ ACC_Relevantes_Objekt : 46|2@1+ (1.0,0.0) [0.0|3] "" XXX - SG_ ACC_Texte_Primaeranz : 48|7@1+ (1.0,0.0) [0.0|127] "" XXX - SG_ ACC_Wunschgeschw_erreicht : 55|1@1+ (1.0,0.0) [0.0|1] "" XXX - SG_ ACC_Status_Anzeige : 61|3@1+ (1.0,0.0) [0.0|7] "" XXX + SG_ ACC_Akustik_02 : 34|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Warnung_Verkehrszeichen_1 : 36|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Gesetzte_Zeitluecke : 37|3@1+ (1,0) [0|7] "" XXX + SG_ ACC_Optischer_Fahrerhinweis : 40|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Typ_Tachokranz : 41|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Anzeige_Zeitluecke : 42|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Tachokranz : 43|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Display_Prio : 44|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Relevantes_Objekt : 46|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Texte_Primaeranz : 48|7@1+ (1,0) [0|127] "" XXX + SG_ ACC_Wunschgeschw_erreicht : 55|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Typ_Tachokranz_unten : 60|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Status_Anzeige : 61|3@1+ (1,0) [0|7] "" XXX BO_ 302 ACC_07: 8 XXX SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX - SG_ ACC_Distance_to_Stop : 12|11@1+ (0.01,0) [0|1] "m" XXX - SG_ ACC_Hold_Request : 23|1@1+ (1,0) [0|1] "x" XXX - SG_ ACC_Boost_Request : 24|1@1+ (1,0) [0|1] "" XXX - SG_ ACC_Freewheel_Request : 25|1@1+ (1,0) [0|1] "" XXX - SG_ ACC_Freewheel_Type : 26|2@1+ (1,0) [0|3] "enum" XXX - SG_ ACC_Hold_Type : 28|3@1+ (1,0) [0|15] "enum" XXX - SG_ ACC_Hold_Release : 31|1@1+ (1,0) [0|1] "" XXX - SG_ ACC_Accel_Secondary : 32|8@1+ (0.03,-4.6) [-4.6|2.99] "m/s2" XXX - SG_ ACC_Accel_TSK : 53|11@1+ (0.005,-7.22) [-7.22|3.005] "m/s2" XXX + SG_ ACC_Anhalteweg : 12|11@1+ (0.01,0) [0|20.45] "Unit_Meter" XXX + SG_ ACC_Anhalten : 23|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Boost_Anf : 24|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Freilauf_Anf : 25|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Freilauf_Info : 26|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Anforderung_HMS : 28|3@1+ (1,0) [0|7] "" XXX + SG_ ACC_Anfahren : 31|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Folgebeschl : 32|8@1+ (0.03,-4.6) [-4.6|2.99] "Unit_MeterPerSeconSquar" XXX + SG_ ACC_Sollbeschleunigung_02 : 53|11@1+ (0.005,-7.22) [-7.22|3.005] "Unit_MeterPerSeconSquar" XXX BO_ 264 Fahrwerk_01: 8 XXX SG_ Fahrwerk_01_BZ : 8|4@1+ (1,0) [0|15] "" XXX @@ -1297,15 +1329,18 @@ BO_ 783 SWA_01: 8 Gateway_MQB BO_ 804 ACC_04: 8 XXX SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX - SG_ ACC_Texte_Zusatzanz : 16|6@1+ (1.0,0.0) [0.0|63] "" XXX - SG_ ACC_Status_Zusatzanz : 22|5@1+ (1.0,0.0) [0.0|31] "" XXX - SG_ ACC_Texte : 27|5@1+ (1.0,0.0) [0.0|31] "" XXX - SG_ ACC_Texte_braking_guard : 32|3@1+ (1.0,0.0) [0.0|7] "" XXX - SG_ ACC_Warnhinweis : 35|1@1+ (1.0,0.0) [0.0|1] "" XXX - SG_ ACC_Geschw_Zielfahrzeug : 40|10@1+ (0.32,0) [0.00|326.72] "Unit_KiloMeterPerHour" XXX - SG_ ACC_Charisma_FahrPr : 56|3@1+ (1.0,0.0) [0.0|7] "" XXX - SG_ ACC_Charisma_Status : 59|2@1+ (1.0,0.0) [0.0|3] "" XXX - SG_ ACC_Charisma_Umschaltung : 61|2@1+ (1.0,0.0) [0.0|3] "" XXX + SG_ ACC_Texte_Sekundaeranz : 12|4@1+ (1,0) [0|15] "" XXX + SG_ ACC_Texte_Zusatzanz : 16|6@1+ (1,0) [0|63] "" XXX + SG_ ACC_Status_Zusatzanz : 22|5@1+ (1,0) [0|31] "" XXX + SG_ ACC_Texte : 27|5@1+ (1,0) [0|31] "" XXX + SG_ ACC_Texte_braking_guard : 32|3@1+ (1,0) [0|7] "" XXX + SG_ ACC_Warnhinweis : 35|1@1+ (1,0) [0|1] "" XXX + SG_ ACC_Zeitluecke_Abstandswarner : 36|6@1+ (0.1,0) [0|6] "Unit_Secon" XXX + SG_ ACC_Abstand_Abstandswarner : 42|9@1+ (1,0) [0|508] "" XXX + SG_ ACC_Tempolimit : 51|5@1+ (1,0) [0|31] "" XXX + SG_ ACC_Charisma_FahrPr : 56|4@1+ (1,0) [0|15] "" XXX + SG_ ACC_Charisma_Status : 60|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_Texte_Abstandswarner : 62|2@1+ (1,0) [0|3] "" XXX BO_ 917 LWR_AFS_01: 8 XXX @@ -1437,14 +1472,90 @@ CM_ SG_ 1720 KBI_Variante "Zeigt an ob es sich um ein konventionelles Zeiger-Kom CM_ SG_ 1720 KBI_Variante_USA "In diesem Signal wird die HW-Variante des Kombis ausgegeben, ACC plausibilisiert auf dieses Signal hin seine US-Codierung"; -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_ 302 ACC_Freewheel_Type 0 "freewheel_released" 1 "freewheel_not_permitted" 2 "freewheel_not_released" 3 "freewheel_requested" ; -VAL_ 302 ACC_Hold_Type 0 "no_request" 1 "hold" 2 "park" 3 "hold_standby" 4 "startup" 5 "loosen_over_ramp" ; -VAL_ 391 GearPosition 2 "P" 3 "R" 4 "N" 5 "D" 6 "D"; -VAL_ 391 RegenBrakingMode 0 "default" 1 "B1" 2 "B2" 3 "B3"; -VAL_ 870 Fast_Send_Rate_Active 0 "1 Hz" 1 "50 Hz"; +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_ 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" ; +VAL_ 780 ACC_Akustik_02 0 "keine_Akustik" 1 "hochpriore_Akustik" 2 "niederpriore_Akustik" 3 "hochpriore_Dauerakustik" ; +VAL_ 780 ACC_Warnung_Verkehrszeichen_1 0 "keine_Warnung_Initialwert" 1 "Warnung" ; +VAL_ 780 ACC_Gesetzte_Zeitluecke 0 "keine_Anzeige" 1 "Zeitluecke_1" 2 "Zeitluecke_2" 3 "Zeitluecke_3" 4 "Zeitluecke_4" 5 "Zeitluecke_5" 6 "nicht_definiert" 7 "nicht_definiert" ; +VAL_ 780 ACC_Optischer_Fahrerhinweis 0 "optischer_Fahrerhinweis_AUS" 1 "optischer_Fahrerhinweis_EIN" ; +VAL_ 780 ACC_Typ_Tachokranz 0 "Tachokranz_lang" 1 "Tachokranz_kurz" ; +VAL_ 780 ACC_Anzeige_Zeitluecke 0 "Anzeige_Zeitluecke_nicht_angefordert" 1 "Anzeige_Zeitluecke_angefordert" ; +VAL_ 780 ACC_Tachokranz 0 "Tachokranz_nicht_beleuchtet" 1 "Tachokranz_beleuchtet" ; +VAL_ 780 ACC_Display_Prio 0 "hoechste_Prio" 1 "mittlere_Prio" 2 "geringe_Prio" 3 "keine_Prio" ; +VAL_ 780 ACC_Relevantes_Objekt 0 "Symbol_nicht_beleuchtet" 1 "Farbe_1_typisch_gruen" 2 "Farbe_2_typisch_rot" 3 "Farbe_3_typisch_gelb" ; +VAL_ 780 ACC_Texte_Primaeranz 0 "keine Anzeige" 1 "ACC nicht verfuegbar !" 2 "Auto_Auto_ _ _" 3 "Auto_ _Auto_ _" 4 "Auto_ _ _Auto_" 5 "Auto_ _ _ _Auto" 6 "Auto_Auto_ _ _ Gong (durchgestrichen)" 7 "Auto_ _Auto_ _ Gong (durchgestrichen)" 8 "Auto_ _ _Auto_ Gong (durchgestrichen)" 9 "Auto_ _ _ _Auto Gong (durchgestrichen)" 10 "ACC bereit" 11 "keine Abstandsregelung" 12 "ACC Sensor Sicht !" 13 "ACC nicht verfuegbar" 14 "o o o" 15 "Hochschalten" 16 "ESP Eingriff" 17 "Herunterschalten" 18 "Parkbremse !" 19 "Geschwindigkeitsgrenze" 20 "Waehlhebelposition !" 21 "VDA ACC-Symbol YYY km/h / mph" 22 "Tempolimit XXX km/h / mph" 23 "Kurve XXX km/h / mph" 24 "ACC Abschaltung" 25 "Symbol 'Eieruhr'" 26 "!" 27 "--- km/h / mph" 28 "XXX km/h / mph (Schriftart 2)" 29 "Lenkradwinkel" 30 "Anfahren bestaetigen" 31 "Fahrzeug verloren" 32 "Im Stand nicht moeglich" 33 "Ungueltiger Anfahrbefehl" 34 "Tuer offen !" 35 "Fahrer Gurtschloss offen !" 36 "Schalthebelposition !" 37 "Drehzahl !" 38 "Kurvenassistent aus" 39 "Tempolimit aus" 40 "Abbiegeassistent" 41 "Ortsanfang XXX km/h / mph" 42 "Ortsende XXX km/h / mph" 43 "Tempolimit Ende XXX km/h / mph" 44 "HDC aktiv" 45 "braking guard Bremsruck" 46 "braking guard aus" 47 "braking guard aus" 48 "Uebernehmen !" 49 "Steigung zu gross" 50 "Stehendes Objekt voraus" 51 "SET / 'GRA Symbol'___xxx km/h / mph" 52 "SET / 'GRA Symbol' xxx km/h / mph" 53 "ACC aus" 54 "ACC startet" 55 "ACC reinigen" 56 "ACC Fehler" 57 "ACC haelt an !" 58 "Bremse betaetigen !" 59 "Kupplung betaetigt" 60 "LIM AUS" 61 "LIM AKTIV" 62 "LIM PASSIV" 63 "LIM FEHLER" 64 "Bremse ueberhitzt !" 65 "Bremse haelt !" 66 "ESP PASSIV !" 67 "ACC_anfahrbereit" 68 "Gang_einlegen" 69 "Rechtsueberholen_verhindert" 70 "Linksueberholen_verhindert" 71 "Achtung_Geschwindigkeitsueberschreitung" 72 "Tempolimit_und_Kurvenassistent_nicht_verfuegbar" ; +VAL_ 780 ACC_Wunschgeschw_erreicht 0 "Wunschgeschwindigkeit_nicht_erreicht" 1 "Wunschgeschwindigkeit_erreicht" ; +VAL_ 780 ACC_Typ_Tachokranz_unten 0 "LEDs_an" 1 "LEDs_aus" ; +VAL_ 780 ACC_Status_Anzeige 0 "ACC_GRA_Hauptschalter_aus" 1 "ACC_in_Init_nicht_bei_GRA" 2 "ACC_GRA_passiv" 3 "ACC_GRA_aktiv" 4 "ACC_GRA_im_Hintergrund_uebertreten" 5 "ACC_GRA_Abschaltreaktion" 6 "ACC_reversibel_aus_nicht_bei_GRA" 7 "ACC_GRA_irreversibel_aus" ; +VAL_ 804 ACC_Texte_Sekundaeranz 0 "keine_Anzeige" 1 "Zielfahrzeug_erkannt" 2 "Rechtskurve_voraus" 3 "Linkskurve_voraus" 4 "Tempolimit_voraus" 5 "Sensorsicht" 6 "Anfahrbereit" 7 "Tempolimit_erkannt" 8 "Kreuzung_voraus" 9 "Kreisverkehr_voraus" ; +VAL_ 804 ACC_Texte_Zusatzanz 0 "keine_Anzeige" 1 "ACC_AUS" 2 "ACC_BEREIT" 3 "UEBERTRETEN" 4 "ABSTAND" 5 "DISTANZ_1" 6 "DISTANZ_2" 7 "DISTANZ_3" 8 "DISTANZ_4" 9 "DISTANZ_1__dyn" 10 "DISTANZ_2__dyn" 11 "DISTANZ_3__dyn" 12 "DISTANZ_4__dyn" 13 "DISTANZ_1__stand" 14 "DISTANZ_2__stand" 15 "DISTANZ_3__stand" 16 "DISTANZ_4__stand" 17 "DISTANZ_1__comf" 18 "DISTANZ_2__comf" 19 "DISTANZ_3__comf" 20 "DISTANZ_4__comf" 21 "DISTANZ_1__efficiency" 22 "DISTANZ_2__efficiency" 23 "DISTANZ_3__efficiency" 24 "DISTANZ_4__efficiency" 25 "DISTANZ_5" 26 "DISTANZ_5__dyn" 27 "DISTANZ_5__stand" 28 "DISTANZ_5__comf" 29 "DISTANZ_5__efficiency" 30 "ACHTUNG" 31 "Abstandsanzeige" 32 "Abstandsanz_Warnung_aktiviert" ; +VAL_ 804 ACC_Status_Zusatzanz 0 "keine Anzeige" 1 "Bild 1 (Fzg. Silhouette, typ. farblos)" 2 "Bild 2 (Fzg. Farbe 1, typ. grau)" 3 "Bild 3 (Fzg. Farbe 2, typ. rot)" 4 "Bild 4 (Symbol 1 ACCplus, typ. Doppelfzg. gelb)" 5 "Bild 5 (Symbol 2 ACCplus, typ. Doppelfzg. grau)" 6 "Bild 6 (Fzg. Farbe 2, typ. rot) mit Priorität im Kombi (Pop-up)" ; +VAL_ 804 ACC_Texte 0 "keine_Anzeige" 1 "ACC_nicht_verfuegbar" 2 "ACC_und_AWV_nicht_verfuegbar" 3 "ACC_keine_Sensorsicht" 4 "ACC_und_AWV_keine_Sensorsicht" 5 "ACC_Steigung_zu_gross" 6 "ACC_nur_in_Fahrstufe_verfuegbar" 7 "ACC_Parkbremse_betaetigt" 8 "ACC_ESP_Eingriff" 9 "ACC_Bitte_uebernehmen" 10 "ACC_HDC_betaetigt" 11 "ACC_Geschwindigkeitsgrenze" 12 "ACC_Schalthebelposition" 13 "ACC_Drehzahl" 14 "ACC_Kupplung_betaetigt" 15 "ACC_Aktivierverhinderung" 16 "ACC_Abschaltung" 17 "ACC_Parkassistent_aktiv" 18 "ACC_ESC_aus" 19 "ACC_Charisma_Modus_inkompatibel" 20 "ACC_Stehendes_Objekt_voraus" 21 "ACA_Fahreruebernahme" 22 "ACA_Querfuehrung_nicht_verfuegbar" ; +VAL_ 804 ACC_Texte_braking_guard 0 "keine_Anzeige" 1 "AWV_aus" 2 "AWV_Warnung" 3 "AWV_Demomodus" 4 "AWV_Systemstoerung" 5 "AWV_Eingriff" 6 "AWV_Vorwarnung_aus" 7 "AWV_keine_Sensorsicht" ; +VAL_ 804 ACC_Warnhinweis 0 "kein_Warnhinweis" 1 "Warnhinweis" ; +VAL_ 804 ACC_Zeitluecke_Abstandswarner 61 "Freifahrt" 62 "nicht_definiert" 63 "keine_Anzeige" ; +VAL_ 804 ACC_Abstand_Abstandswarner 509 "Freifahrt" 510 "nicht_definiert" 511 "keine_Anzeige" ; +VAL_ 804 ACC_Tempolimit 0 "keine_Anzeige" 1 "5_zulHoechstgeschw" 2 "7_zulHoechstgeschw" 3 "10_zulHoechstgeschw" 4 "15_zulHoechstgeschw" 5 "20_zulHoechstgeschw" 6 "25_zulHoechstgeschw" 7 "30_zulHoechstgeschw" 8 "35_zulHoechstgeschw" 9 "40_zulHoechstgeschw" 10 "45_zulHoechstgeschw" 11 "50_zulHoechstgeschw" 12 "55_zulHoechstgeschw" 13 "60_zulHoechstgeschw" 14 "65_zulHoechstgeschw" 15 "70_zulHoechstgeschw" 16 "75_zulHoechstgeschw" 17 "80_zulHoechstgeschw" 18 "85_zulHoechstgeschw" 19 "90_zulHoechstgeschw" 20 "95_zulHoechstgeschw" 21 "100_zulHoechstgeschw" 22 "110_zulHoechstgeschw" 23 "120_zulHoechstgeschw" 24 "130_zulHoechstgeschw" 25 "140_zulHoechstgeschw" 26 "150_zulHoechstgeschw" 27 "160_zulHoechstgeschw" 28 "200_zulHoechstgeschw" 30 "250_zulHoechstgeschw" 31 "Ende_zulHoechstgeschw" ; +VAL_ 804 ACC_Charisma_FahrPr 0 "keine_Funktion" 1 "Programm_1" 2 "Programm_2" 3 "Programm_3" 4 "Programm_4" 5 "Programm_5" 6 "Programm_6" 7 "Programm_7" 8 "Programm_8" 9 "Programm_9" 10 "Programm_10" 11 "Programm_11" 12 "Programm_12" 13 "Programm_13" 14 "Programm_14" 15 "Programm_15" ; +VAL_ 804 ACC_Charisma_Status 0 "Init" 1 "verfuegbar" 2 "nicht_verfuegbar" 3 "asynchron_durch_Fahrerwunsch" ; +VAL_ 804 ACC_Texte_Abstandswarner 0 "keine_Anzeige" 1 "Systemstoerung" 2 "keine_Sensorsicht" 3 "zurzeit_eingeschraenkt" ; +VAL_ 290 ACC_limitierte_Anfahrdyn 0 "keine_Limitierung" 1 "Limitierung_Anfahrdynamik_angefordert" ; +VAL_ 290 ACC_nachtr_Stopp_Anf 0 "nicht_angefordert" 1 "angefordert" ; +VAL_ 290 ACC_StartStopp_Info 0 "Motorlauf_langfristig_nicht_notwendig_Stoppfreigabe" 1 "Motoranlauf_nicht_zwingend_notwendig_Stoppverbot_keine_Startanforderung" 2 "Motoranlauf_zwingend_notwendig_Startanforderung" 3 "Systemfehler" ; +VAL_ 290 ACC_Sollbeschleunigung_02 2046 "Neutralwert" 2047 "Fehler" ; +VAL_ 290 ACC_Anfahren 0 "keine_Anforderung_Anfahren" 1 "Anforderung_Anfahren" ; +VAL_ 290 ACC_Anhalten 0 "kein_Anhalten_gewuenscht" 1 "Anhalten_gewuenscht" ; +VAL_ 290 ACC_Typ 0 "Basis_ACC" 1 "ACC_mit_FollowToStop" 2 "ACC_mit_StopAndGo" 3 "ACC_nicht_codiert" ; +VAL_ 290 ACC_Status_ACC 0 "ACC_OFF_Hauptschalter_aus" 1 "ACC_INIT" 2 "ACC_STANDBY" 3 "ACC_AKTIV_regelt" 4 "ACC_OVERRIDE" 5 "ACC_Abschaltreaktion" 6 "reversibler_Fehler_im_ACC_System" 7 "irreversibler_Fehler_im_ACC_System" ; +VAL_ 290 ACC_Minimale_Bremsung 0 "Anforderung_Minimale_Bremsung_nicht_aktiv" 1 "Anforderung_Minimale_Bremsung_aktiv" ; +VAL_ 302 ACC_Anhalteweg 2046 "Neutralwert" 2047 "Fehler" ; +VAL_ 302 ACC_Anhalten 0 "kein_Anhalten_gewuenscht" 1 "Anhalten_gewuenscht" ; +VAL_ 302 ACC_Freilauf_Anf 0 "keine Freilauf-Anforderung" 1 "Freilauf-Anforderung" ; +VAL_ 302 ACC_Freilauf_Info 0 "Freilauf_freigegeben" 1 "kein_Uebergang_in_Freilauf_zulaessig" 2 "Freilauf_nicht_freigegeben" 3 "Freilauf_Anforderung" ; +VAL_ 302 ACC_Anforderung_HMS 0 "keine_Anforderung" 1 "halten" 2 "parken" 3 "halten_Standby" 4 "anfahren" 5 "Loesen_ueber_Rampe" ; +VAL_ 302 ACC_Anfahren 0 "keine_Anforderung_Anfahren" 1 "Anforderung_Anfahren" ; +VAL_ 302 ACC_Folgebeschl 254 "Neutralwert" ; +VAL_ 302 ACC_Sollbeschleunigung_02 2046 "Neutralwert" 2047 "Fehler" ; +VAL_ 279 AWV1_Anf_Prefill 0 "Prefill_nicht_aktivieren" 1 "Prefill_aktivieren" ; +VAL_ 279 ANB_CM_Info 0 "Standard" 1 "Erweitert" ; +VAL_ 279 AWV2_Freigabe 0 "keine_Freigabe" 1 "Freigabe" ; +VAL_ 279 AWV1_HBA_Param 0 "Defaultparametersatz" 1 "Parametersatz_mit_leicht_erhoehter_Empfindlichkeit" 2 "Parametersatz_mit_erhoehter_Empfindlichkeit" 3 "Parametersatz_mit_hoechster_Empfindlichkeit" ; +VAL_ 279 AWV2_Priowarnung 0 "Anzeige_Verlassen_der_Fahrspur_wird_nicht_unterdrueckt" 1 "Anzeige_Verlassen_der_Fahrspur_wird_unterdrueckt" ; +VAL_ 279 ANB_CM_Anforderung 0 "keine_Anforderung" 1 "Anforderung_aktiv" ; +VAL_ 279 ANB_Info_Teilbremsung 0 "Auspraegung_Standard" 1 "Auspraegung_Erweitert" ; +VAL_ 279 ANB_Notfallblinken 0 "kein_ANB_Notfallblinken" 1 "Notfallblinken_ANB_angefordert" ; +VAL_ 279 ANB_Teilbremsung_Freigabe 0 "Teilbremsung_nicht_freigegeben" 1 "Teilbremsung_freigegeben" ; +VAL_ 279 ANB_Zielbremsung_Freigabe 0 "Zielbremsung_nicht_freigegeben" 1 "Zielbremsung_freigegeben" ; +VAL_ 279 AWV_Vorstufe 0 "keine_Notbremsung_erwartet" 1 "Notbremsung_in_Kuerze" ; +VAL_ 279 AWV_Halten 0 "keine_Anforderung" 1 "Anforderung_das_Fzg_im_Stillstand_zu_halten" ; +VAL_ 279 AWV_CityANB_Auspraegung 0 "autom_Bremsung_im_ges_vBereich" 1 "autom_Bremsung_im_def_vBereich" ; +VAL_ 279 PCF_Freigabe 0 "keine_Freigabe_PreCrashFront" 1 "Freigabe_PreCrashFront" ; +VAL_ 279 AWV1_ECD_Anlauf 0 "ECD_Anlauf_nicht_aktivieren" 1 "ECD_Anlauf_aktivieren" ; +VAL_ 279 PCF_Time_to_collision 255 "Objektstatus=0x0__oder_berechneter_TTC_Wert_groesser_als_Maximalwert" ; +VAL_ 679 ACC_Regelgeschw 1023 "keine_Anzeige" ; +VAL_ 679 ACC_Einheit_maxSetzgeschw 0 "kmh" 1 "mph" ; +VAL_ 679 ACC_maxSetzgeschw 511 "Init_Neutralwert" ; +VAL_ 679 ACC_minRegelgeschw 255 "keine_Anzeige" ; +VAL_ 679 ACC_maxRegelgeschw 255 "keine_Anzeige" ; +VAL_ 679 ACC_Tempolimitassistent 0 "keine_Anzeige" 1 "Tempolimitassistent_aktiv" 2 "Tempolimitassistent_nicht_verfuegbar" 3 "Tempolimitassistent_Fahreruebernahme" ; +VAL_ 679 ACC_Kurvenassistent 0 "keine_Anzeige" 1 "Kreuzung" 2 "Rechtskurve" 3 "Linkskurve" 4 "Kreisverkehr" ; +VAL_ 679 ACC_RUV 0 "keine_Anzeige" 1 "RUV_aktiv_Rechtsverkehr" 2 "RUV_aktiv_Linksverkehr" ; +VAL_ 679 ACC_Tachokranz 0 "Tachokranz_nicht_beleuchtet" 1 "Tachokranz_beleuchtet" ; +VAL_ 679 ACC_Typ_Tachokranz_unten 0 "LEDs_an" 1 "LEDs_aus" ; +VAL_ 679 ACC_ENG_Texte 0 "keine_Anzeige" 1 "keine_Laenderverfuegbarkeit" 2 "nicht_verfuegbar" 3 "Geschwindigkeitsgrenze" ; +VAL_ 681 AWV_Warnung 0 "keine_Anzeige" 1 "latente_Vorwarnung" 2 "Vorwarnung" 3 "Akutwarnung" 4 "Eingriff" 5 "Fahreruebernahmeaufforderung" 6 "Abbiegewarnung" ; +VAL_ 681 AWV_Texte 0 "keine_Anzeige" 1 "Systemstoerung" 2 "keine_Sensorsicht" 3 "Demomodus" 4 "System_aus" 5 "nicht_definiert" 6 "ESC_aus" 7 "zurzeit_eingeschraenkt" ; +VAL_ 681 AWV_Status_Anzeige 0 "Init" 1 "verfuegbar" 2 "nicht_verfuegbar" ; +VAL_ 681 AWV_Einstellung_System_FSG 0 "deaktiviert" 1 "aktiviert" ; +VAL_ 681 AWV_Einstellung_Warnung_FSG 0 "Aus" 1 "Setting_2" 2 "Setting_3" 3 "Setting_4" 4 "Setting_5" 5 "Ein" ; +VAL_ 681 AWV_Warnlevel 0 "keine_Gefaehrdung" 63 "max_Gefaehrdung" ; +VAL_ 391 GearPosition 2 "P" 3 "R" 4 "N" 5 "D" 6 "D" ; +VAL_ 391 RegenBrakingMode 0 "default" 1 "B1" 2 "B2" 3 "B3" ; +VAL_ 870 Fast_Send_Rate_Active 0 "1 Hz" 1 "50 Hz" ; VAL_ 1720 KBI_Variante_USA 0 "keine USA-Variante" 1 "USA-Variante" ; VAL_ 1720 KBI_Variante 0 "Zeiger Kombiinstrument" 1 "Volldisplay Kombiinstrument" ; VAL_ 1720 KBI_BCmE_aktiv 0 "Anzeige_nicht_aktiv" 1 "Anzeige_aktiv" ; diff --git a/panda/board/README.md b/panda/board/README.md index dbb84cd6a..4bd4b89a2 100644 --- a/panda/board/README.md +++ b/panda/board/README.md @@ -21,15 +21,19 @@ Programming **Panda** ``` -./flash.sh # for any panda +./recover.sh # flash bootstub +``` + +``` +./flash.sh # flash application ``` Troubleshooting ---- -If your panda will not flash and is quickly blinking a single Green LED, use `recover.sh`. - -A [panda paw](https://comma.ai/shop/products/panda-paw) can be used to put panda into DFU mode. +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`. +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. [dfu-util](http://github.com/dsigma/dfu-util.git) for flashing diff --git a/panda/board/SConscript b/panda/board/SConscript index ebdca1301..7c9127904 100644 --- a/panda/board/SConscript +++ b/panda/board/SConscript @@ -11,7 +11,7 @@ build_projects = {} build_projects["pedal"] = { "MAIN": "pedal/main.c", "STARTUP_FILE": "stm32fx/startup_stm32f205xx.s", - "LINKER_SCRIPT": "stm32fx/stm32fx_flash.ld", + "LINKER_SCRIPT": "stm32fx/stm32f2_flash.ld", "APP_START_ADDRESS": "0x8004000", "PROJECT_FLAGS": [ "-mcpu=cortex-m3", @@ -29,7 +29,7 @@ build_projects["pedal_usb"]["PROJECT_FLAGS"].append("-DPEDAL_USB") build_projects["panda"] = { "MAIN": "main.c", "STARTUP_FILE": "stm32fx/startup_stm32f413xx.s", - "LINKER_SCRIPT": "stm32fx/stm32fx_flash.ld", + "LINKER_SCRIPT": "stm32fx/stm32f4_flash.ld", "APP_START_ADDRESS": "0x8004000", "PROJECT_FLAGS": [ "-mcpu=cortex-m4", @@ -152,6 +152,9 @@ for project_name in build_projects: "-std=gnu11", ] + project["PROJECT_FLAGS"] + common_flags + if ("ENABLE_SPI" in os.environ or "h7" in project_name) and not project_name.startswith('pedal'): + flags.append('-DENABLE_SPI') + project_env = Environment( ENV=os.environ, CC=PREFIX + 'gcc', diff --git a/panda/board/boards/black.h b/panda/board/boards/black.h index a1297345b..043df7ed4 100644 --- a/panda/board/boards/black.h +++ b/panda/board/boards/black.h @@ -57,26 +57,6 @@ void black_set_usb_load_switch(bool enabled) { set_gpio_output(GPIOB, 1, !enabled); } -void black_set_usb_power_mode(uint8_t mode) { - bool valid = false; - switch (mode) { - case USB_POWER_CLIENT: - black_set_usb_load_switch(false); - valid = true; - break; - case USB_POWER_CDP: - black_set_usb_load_switch(true); - valid = true; - break; - default: - puts("Invalid USB power mode\n"); - break; - } - if (valid) { - usb_power_mode = mode; - } -} - void black_set_gps_mode(uint8_t mode) { switch (mode) { case GPS_DISABLED: @@ -162,9 +142,6 @@ void black_init(void) { // Turn on USB load switch. black_set_usb_load_switch(true); - // Set right power mode - black_set_usb_power_mode(USB_POWER_CDP); - // Initialize harness harness_init(); @@ -204,21 +181,22 @@ const harness_configuration black_harness_config = { const board board_black = { .board_type = "Black", + .board_tick = unused_board_tick, .harness_config = &black_harness_config, .has_gps = true, .has_hw_gmlan = false, .has_obd = true, .has_lin = false, + .has_spi = false, + .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, .init = black_init, .enable_can_transceiver = black_enable_can_transceiver, .enable_can_transceivers = black_enable_can_transceivers, .set_led = black_set_led, - .set_usb_power_mode = black_set_usb_power_mode, .set_gps_mode = black_set_gps_mode, .set_can_mode = black_set_can_mode, - .usb_power_mode_tick = unused_usb_power_mode_tick, .check_ignition = black_check_ignition, .read_current = unused_read_current, .set_fan_enabled = unused_set_fan_enabled, diff --git a/panda/board/boards/board_declarations.h b/panda/board/boards/board_declarations.h index 50a7ee451..b00f39fc3 100644 --- a/panda/board/boards/board_declarations.h +++ b/panda/board/boards/board_declarations.h @@ -3,10 +3,8 @@ typedef void (*board_init)(void); typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); typedef void (*board_enable_can_transceivers)(bool enabled); typedef void (*board_set_led)(uint8_t color, bool enabled); -typedef void (*board_set_usb_power_mode)(uint8_t mode); typedef void (*board_set_gps_mode)(uint8_t mode); typedef void (*board_set_can_mode)(uint8_t mode); -typedef void (*board_usb_power_mode_tick)(uint32_t uptime); typedef bool (*board_check_ignition)(void); typedef uint32_t (*board_read_current)(void); typedef void (*board_set_ir_power)(uint8_t percentage); @@ -14,6 +12,7 @@ typedef void (*board_set_fan_enabled)(bool enabled); typedef void (*board_set_phone_power)(bool enabled); typedef void (*board_set_clock_source_mode)(uint8_t mode); typedef void (*board_set_siren)(bool enabled); +typedef void (*board_board_tick)(bool ignition, bool usb_enum, bool heartbeat_seen); struct board { const char *board_type; @@ -22,16 +21,16 @@ struct board { const bool has_hw_gmlan; const bool has_obd; const bool has_lin; + const bool has_spi; + const bool has_canfd; const bool has_rtc_battery; const uint16_t fan_max_rpm; board_init init; board_enable_can_transceiver enable_can_transceiver; board_enable_can_transceivers enable_can_transceivers; board_set_led set_led; - board_set_usb_power_mode set_usb_power_mode; board_set_gps_mode set_gps_mode; board_set_can_mode set_can_mode; - board_usb_power_mode_tick usb_power_mode_tick; board_check_ignition check_ignition; board_read_current read_current; board_set_ir_power set_ir_power; @@ -39,6 +38,7 @@ struct board { board_set_phone_power set_phone_power; board_set_clock_source_mode set_clock_source_mode; board_set_siren set_siren; + board_board_tick board_tick; }; // ******************* Definitions ******************** @@ -51,6 +51,8 @@ struct board { #define HW_TYPE_UNO 5U #define HW_TYPE_DOS 6U #define HW_TYPE_RED_PANDA 7U +#define HW_TYPE_RED_PANDA_V2 8U +#define HW_TYPE_TRES 9U // LED colors #define LED_RED 0U @@ -73,6 +75,3 @@ struct board { #define CAN_MODE_GMLAN_CAN2 1U #define CAN_MODE_GMLAN_CAN3 2U #define CAN_MODE_OBD_CAN2 3U - -// ********************* Globals ********************** -uint8_t usb_power_mode = USB_POWER_NONE; diff --git a/panda/board/boards/dos.h b/panda/board/boards/dos.h index 38d647037..630a30952 100644 --- a/panda/board/boards/dos.h +++ b/panda/board/boards/dos.h @@ -53,23 +53,15 @@ void dos_set_bootkick(bool enabled){ set_gpio_output(GPIOC, 4, !enabled); } -void dos_set_usb_power_mode(uint8_t mode) { - bool valid = false; - switch (mode) { - case USB_POWER_CLIENT: - dos_set_bootkick(false); - valid = true; - break; - case USB_POWER_CDP: - dos_set_bootkick(true); - valid = true; - break; - default: - puts("Invalid USB power mode\n"); - break; - } - if (valid) { - usb_power_mode = mode; +void dos_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen) { + if (ignition && !usb_enum) { + // enable bootkick if ignition seen + dos_set_bootkick(true); + } else if (heartbeat_seen) { + // disable once openpilot is up + dos_set_bootkick(false); + } else { + } } @@ -147,6 +139,15 @@ void dos_init(void) { set_gpio_output(GPIOC, 10, 1); 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); +#endif + // C8: FAN PWM aka TIM3_CH3 set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); @@ -173,6 +174,9 @@ void dos_init(void) { dos_set_led(LED_GREEN, false); dos_set_led(LED_BLUE, false); + // Bootkick + dos_set_bootkick(true); + // Set normal CAN mode dos_set_can_mode(CAN_MODE_NORMAL); @@ -201,21 +205,26 @@ const harness_configuration dos_harness_config = { const board board_dos = { .board_type = "Dos", + .board_tick = dos_board_tick, .harness_config = &dos_harness_config, .has_gps = false, .has_hw_gmlan = false, .has_obd = true, .has_lin = false, +#ifdef ENABLE_SPI + .has_spi = true, +#else + .has_spi = false, +#endif + .has_canfd = false, .has_rtc_battery = true, .fan_max_rpm = 6500U, .init = dos_init, .enable_can_transceiver = dos_enable_can_transceiver, .enable_can_transceivers = dos_enable_can_transceivers, .set_led = dos_set_led, - .set_usb_power_mode = dos_set_usb_power_mode, .set_gps_mode = unused_set_gps_mode, .set_can_mode = dos_set_can_mode, - .usb_power_mode_tick = unused_usb_power_mode_tick, .check_ignition = dos_check_ignition, .read_current = unused_read_current, .set_fan_enabled = dos_set_fan_enabled, diff --git a/panda/board/boards/grey.h b/panda/board/boards/grey.h index 4ff6f8987..94d66bf9f 100644 --- a/panda/board/boards/grey.h +++ b/panda/board/boards/grey.h @@ -35,21 +35,22 @@ void grey_set_gps_mode(uint8_t mode) { const board board_grey = { .board_type = "Grey", + .board_tick = unused_board_tick, .harness_config = &white_harness_config, .has_gps = true, .has_hw_gmlan = true, .has_obd = false, .has_lin = true, + .has_spi = false, + .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, .init = grey_init, .enable_can_transceiver = white_enable_can_transceiver, .enable_can_transceivers = white_enable_can_transceivers, .set_led = white_set_led, - .set_usb_power_mode = white_set_usb_power_mode, .set_gps_mode = grey_set_gps_mode, .set_can_mode = white_set_can_mode, - .usb_power_mode_tick = unused_usb_power_mode_tick, .check_ignition = white_check_ignition, .read_current = white_read_current, .set_fan_enabled = unused_set_fan_enabled, diff --git a/panda/board/boards/pedal.h b/panda/board/boards/pedal.h index 72286e646..fe9a8ff44 100644 --- a/panda/board/boards/pedal.h +++ b/panda/board/boards/pedal.h @@ -30,11 +30,6 @@ void pedal_set_led(uint8_t color, bool enabled) { } } -void pedal_set_usb_power_mode(uint8_t mode){ - usb_power_mode = mode; - puts("Trying to set USB power mode on pedal. This is not supported.\n"); -} - void pedal_set_gps_mode(uint8_t mode) { UNUSED(mode); puts("Trying to set ESP/GPS mode on pedal. This is not supported.\n"); @@ -78,21 +73,22 @@ const harness_configuration pedal_harness_config = { const board board_pedal = { .board_type = "Pedal", + .board_tick = unused_board_tick, .harness_config = &pedal_harness_config, .has_gps = false, .has_hw_gmlan = false, .has_obd = false, .has_lin = false, + .has_spi = false, + .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, .init = pedal_init, .enable_can_transceiver = pedal_enable_can_transceiver, .enable_can_transceivers = pedal_enable_can_transceivers, .set_led = pedal_set_led, - .set_usb_power_mode = pedal_set_usb_power_mode, .set_gps_mode = pedal_set_gps_mode, .set_can_mode = pedal_set_can_mode, - .usb_power_mode_tick = unused_usb_power_mode_tick, .check_ignition = pedal_check_ignition, .read_current = unused_read_current, .set_fan_enabled = unused_set_fan_enabled, diff --git a/panda/board/boards/red.h b/panda/board/boards/red.h index ac22f78f6..76f06faa7 100644 --- a/panda/board/boards/red.h +++ b/panda/board/boards/red.h @@ -53,25 +53,6 @@ void red_set_usb_load_switch(bool enabled) { set_gpio_output(GPIOB, 14, !enabled); } -void red_set_usb_power_mode(uint8_t mode) { - bool valid = false; - switch (mode) { - case USB_POWER_CLIENT: - red_set_usb_load_switch(false); - valid = true; - break; - case USB_POWER_CDP: - red_set_usb_load_switch(true); - valid = true; - break; - default: - break; - } - if (valid) { - usb_power_mode = mode; - } -} - void red_set_can_mode(uint8_t mode) { switch (mode) { case CAN_MODE_NORMAL: @@ -118,13 +99,6 @@ bool red_check_ignition(void) { void red_init(void) { common_init_gpio(); - //C4,A1: OBD_SBU1, OBD_SBU2 - set_gpio_pullup(GPIOC, 4, PULL_NONE); - set_gpio_mode(GPIOC, 4, MODE_ANALOG); - - set_gpio_pullup(GPIOA, 1, PULL_NONE); - set_gpio_mode(GPIOA, 1, MODE_ANALOG); - //C10,C11 : OBD_SBU1_RELAY, OBD_SBU2_RELAY set_gpio_output_type(GPIOC, 10, OUTPUT_TYPE_OPEN_DRAIN); set_gpio_pullup(GPIOC, 10, PULL_NONE); @@ -136,12 +110,30 @@ void red_init(void) { set_gpio_mode(GPIOC, 11, MODE_OUTPUT); set_gpio_output(GPIOC, 11, 1); + // G11,B3,D7,B4: transceiver enable + set_gpio_pullup(GPIOG, 11, PULL_NONE); + set_gpio_mode(GPIOG, 11, MODE_OUTPUT); + + set_gpio_pullup(GPIOB, 3, PULL_NONE); + set_gpio_mode(GPIOB, 3, MODE_OUTPUT); + + set_gpio_pullup(GPIOD, 7, PULL_NONE); + set_gpio_mode(GPIOD, 7, MODE_OUTPUT); + + set_gpio_pullup(GPIOB, 4, PULL_NONE); + set_gpio_mode(GPIOB, 4, MODE_OUTPUT); + + // B14: usb load switch + set_gpio_pullup(GPIOB, 14, PULL_NONE); + set_gpio_mode(GPIOB, 14, MODE_OUTPUT); + + //B1: 5VOUT_S + set_gpio_pullup(GPIOB, 1, PULL_NONE); + set_gpio_mode(GPIOB, 1, MODE_ANALOG); + // Turn on USB load switch. red_set_usb_load_switch(true); - // Set right power mode - red_set_usb_power_mode(USB_POWER_CDP); - // Initialize harness harness_init(); @@ -181,21 +173,22 @@ const harness_configuration red_harness_config = { const board board_red = { .board_type = "Red", + .board_tick = unused_board_tick, .harness_config = &red_harness_config, .has_gps = false, .has_hw_gmlan = false, .has_obd = true, .has_lin = false, + .has_spi = false, + .has_canfd = true, .has_rtc_battery = false, .fan_max_rpm = 0U, .init = red_init, .enable_can_transceiver = red_enable_can_transceiver, .enable_can_transceivers = red_enable_can_transceivers, .set_led = red_set_led, - .set_usb_power_mode = red_set_usb_power_mode, .set_gps_mode = unused_set_gps_mode, .set_can_mode = red_set_can_mode, - .usb_power_mode_tick = unused_usb_power_mode_tick, .check_ignition = red_check_ignition, .read_current = unused_read_current, .set_fan_enabled = unused_set_fan_enabled, diff --git a/panda/board/boards/red_chiplet.h b/panda/board/boards/red_chiplet.h new file mode 100644 index 000000000..3eb09832d --- /dev/null +++ b/panda/board/boards/red_chiplet.h @@ -0,0 +1,115 @@ +// ///////////////////// // +// Red Panda chiplet + Harness // +// ///////////////////// // + +// Most hardware functionality is similar to red panda + +void red_chiplet_enable_can_transceiver(uint8_t transceiver, bool enabled) { + switch (transceiver) { + case 1U: + set_gpio_output(GPIOG, 11, !enabled); + break; + case 2U: + set_gpio_output(GPIOB, 10, !enabled); + break; + case 3U: + set_gpio_output(GPIOD, 7, !enabled); + break; + case 4U: + set_gpio_output(GPIOB, 11, !enabled); + break; + default: + break; + } +} + +void red_chiplet_enable_can_transceivers(bool enabled) { + uint8_t main_bus = (car_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) { + red_chiplet_enable_can_transceiver(i, true); + } else { + red_chiplet_enable_can_transceiver(i, enabled); + } + } +} + +void red_chiplet_set_usb_load_switch(bool enabled) { + set_gpio_output(GPIOD, 3, enabled); +} + +void red_chiplet_init(void) { + common_init_gpio(); + + //A8, A9 : 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_output_type(GPIOA, 9, OUTPUT_TYPE_OPEN_DRAIN); + set_gpio_pullup(GPIOA, 9, PULL_NONE); + set_gpio_mode(GPIOA, 9, MODE_OUTPUT); + set_gpio_output(GPIOA, 9, 1); + + // G11,B10,D7,B11: transceiver enable + set_gpio_pullup(GPIOG, 11, PULL_NONE); + set_gpio_mode(GPIOG, 11, MODE_OUTPUT); + + set_gpio_pullup(GPIOB, 10, PULL_NONE); + set_gpio_mode(GPIOB, 10, MODE_OUTPUT); + + set_gpio_pullup(GPIOD, 7, PULL_NONE); + set_gpio_mode(GPIOD, 7, MODE_OUTPUT); + + set_gpio_pullup(GPIOB, 11, PULL_NONE); + set_gpio_mode(GPIOB, 11, MODE_OUTPUT); + + // D3: usb load switch + set_gpio_pullup(GPIOD, 3, PULL_NONE); + set_gpio_mode(GPIOD, 3, MODE_OUTPUT); + + //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_usb_load_switch(true); + + // Initialize harness + harness_init(); + + // Initialize RTC + rtc_init(); + + // Enable CAN transceivers + red_chiplet_enable_can_transceivers(true); + + // Disable LEDs + red_set_led(LED_RED, false); + red_set_led(LED_GREEN, false); + red_set_led(LED_BLUE, false); + + // Set normal CAN mode + red_set_can_mode(CAN_MODE_NORMAL); + + // flip CAN0 and CAN2 if we are flipped + if (car_harness_status == HARNESS_STATUS_FLIPPED) { + can_flip_buses(0, 2); + } +} + +const harness_configuration red_chiplet_harness_config = { + .has_harness = true, + .GPIO_SBU1 = GPIOC, + .GPIO_SBU2 = GPIOA, + .GPIO_relay_SBU1 = GPIOA, + .GPIO_relay_SBU2 = GPIOA, + .pin_SBU1 = 4, + .pin_SBU2 = 1, + .pin_relay_SBU1 = 8, + .pin_relay_SBU2 = 9, + .adc_channel_SBU1 = 4, // ADC12_INP4 + .adc_channel_SBU2 = 17 // ADC1_INP17 +}; diff --git a/panda/board/boards/red_v2.h b/panda/board/boards/red_v2.h new file mode 100644 index 000000000..8527432ee --- /dev/null +++ b/panda/board/boards/red_v2.h @@ -0,0 +1,30 @@ +// ///////////////////// // +// Red Panda V2 with chiplet + Harness // +// ///////////////////// // + +const board board_red_v2 = { + .board_type = "Red_v2", + .board_tick = unused_board_tick, + .harness_config = &red_chiplet_harness_config, + .has_gps = false, + .has_hw_gmlan = false, + .has_obd = true, + .has_lin = false, + .has_spi = false, + .has_canfd = true, + .has_rtc_battery = true, + .fan_max_rpm = 0U, + .init = red_chiplet_init, + .enable_can_transceiver = red_chiplet_enable_can_transceiver, + .enable_can_transceivers = red_chiplet_enable_can_transceivers, + .set_led = red_set_led, + .set_gps_mode = unused_set_gps_mode, + .set_can_mode = red_set_can_mode, + .check_ignition = red_check_ignition, + .read_current = unused_read_current, + .set_fan_enabled = unused_set_fan_enabled, + .set_ir_power = unused_set_ir_power, + .set_phone_power = unused_set_phone_power, + .set_clock_source_mode = unused_set_clock_source_mode, + .set_siren = unused_set_siren +}; diff --git a/panda/board/boards/tres.h b/panda/board/boards/tres.h new file mode 100644 index 000000000..095783a86 --- /dev/null +++ b/panda/board/boards/tres.h @@ -0,0 +1,46 @@ +// ///////////////// +// Tres + Harness // +// ///////////////// + +void tres_init(void) { + // Enable USB 3.3V LDO for USB block + register_set_bits(&(PWR->CR3), PWR_CR3_USBREGEN); + register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); + while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0); + + red_chiplet_init(); + + // 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); +} + +const board board_tres = { + .board_type = "Tres", + .board_tick = unused_board_tick, + .harness_config = &red_chiplet_harness_config, + .has_gps = false, + .has_hw_gmlan = false, + .has_obd = true, + .has_lin = false, + .has_spi = true, + .has_canfd = true, + .has_rtc_battery = true, + .fan_max_rpm = 0U, + .init = tres_init, + .enable_can_transceiver = red_chiplet_enable_can_transceiver, + .enable_can_transceivers = red_chiplet_enable_can_transceivers, + .set_led = red_set_led, + .set_gps_mode = unused_set_gps_mode, + .set_can_mode = red_set_can_mode, + .check_ignition = red_check_ignition, + .read_current = unused_read_current, + .set_fan_enabled = unused_set_fan_enabled, + .set_ir_power = unused_set_ir_power, + .set_phone_power = unused_set_phone_power, + .set_clock_source_mode = unused_set_clock_source_mode, + .set_siren = unused_set_siren +}; diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h index f5d097fd0..d4461b497 100644 --- a/panda/board/boards/uno.h +++ b/panda/board/boards/uno.h @@ -56,7 +56,7 @@ void uno_set_gps_load_switch(bool enabled) { } void uno_set_bootkick(bool enabled){ - if(enabled){ + if (enabled) { set_gpio_output(GPIOB, 14, false); } else { // We want the pin to be floating, not forced high! @@ -73,25 +73,6 @@ void uno_set_phone_power(bool enabled){ set_gpio_output(GPIOB, 4, enabled); } -void uno_set_usb_power_mode(uint8_t mode) { - bool valid = false; - switch (mode) { - case USB_POWER_CLIENT: - valid = true; - break; - case USB_POWER_CDP: - uno_bootkick(); - valid = true; - break; - default: - puts("Invalid USB power mode\n"); - break; - } - if (valid) { - usb_power_mode = mode; - } -} - void uno_set_gps_mode(uint8_t mode) { switch (mode) { case GPS_DISABLED: @@ -145,9 +126,11 @@ void uno_set_can_mode(uint8_t mode){ } } -void uno_usb_power_mode_tick(uint32_t uptime){ - UNUSED(uptime); - if(bootkick_timer != 0U){ +void uno_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen) { + UNUSED(ignition); + UNUSED(usb_enum); + UNUSED(heartbeat_seen); + if (bootkick_timer != 0U) { bootkick_timer--; } else { uno_set_bootkick(false); @@ -262,21 +245,22 @@ const harness_configuration uno_harness_config = { const board board_uno = { .board_type = "Uno", + .board_tick = uno_board_tick, .harness_config = &uno_harness_config, .has_gps = true, .has_hw_gmlan = false, .has_obd = true, .has_lin = false, + .has_spi = false, + .has_canfd = false, .has_rtc_battery = true, .fan_max_rpm = 5100U, .init = uno_init, .enable_can_transceiver = uno_enable_can_transceiver, .enable_can_transceivers = uno_enable_can_transceivers, .set_led = uno_set_led, - .set_usb_power_mode = uno_set_usb_power_mode, .set_gps_mode = uno_set_gps_mode, .set_can_mode = uno_set_can_mode, - .usb_power_mode_tick = uno_usb_power_mode_tick, .check_ignition = uno_check_ignition, .read_current = unused_read_current, .set_fan_enabled = uno_set_fan_enabled, diff --git a/panda/board/boards/unused_funcs.h b/panda/board/boards/unused_funcs.h index 540106575..be675d89f 100644 --- a/panda/board/boards/unused_funcs.h +++ b/panda/board/boards/unused_funcs.h @@ -2,10 +2,6 @@ void unused_set_gps_mode(uint8_t mode) { UNUSED(mode); } -void unused_usb_power_mode_tick(uint32_t uptime) { - UNUSED(uptime); -} - void unused_set_ir_power(uint8_t percentage) { UNUSED(percentage); } @@ -29,3 +25,9 @@ void unused_set_siren(bool enabled) { uint32_t unused_read_current(void) { return 0U; } + +void unused_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen) { + UNUSED(ignition); + UNUSED(usb_enum); + UNUSED(heartbeat_seen); +} \ No newline at end of file diff --git a/panda/board/boards/white.h b/panda/board/boards/white.h index 58c71a46a..15335494c 100644 --- a/panda/board/boards/white.h +++ b/panda/board/boards/white.h @@ -43,7 +43,6 @@ void white_set_led(uint8_t color, bool enabled) { } void white_set_usb_power_mode(uint8_t mode){ - bool valid_mode = true; switch (mode) { case USB_POWER_CLIENT: // B2,A13: set client mode @@ -61,14 +60,9 @@ void white_set_usb_power_mode(uint8_t mode){ set_gpio_output(GPIOA, 13, 0); break; default: - valid_mode = false; puts("Invalid usb power mode\n"); break; } - - if (valid_mode) { - usb_power_mode = mode; - } } void white_set_gps_mode(uint8_t mode) { @@ -243,21 +237,22 @@ const harness_configuration white_harness_config = { const board board_white = { .board_type = "White", + .board_tick = unused_board_tick, .harness_config = &white_harness_config, .has_gps = false, .has_hw_gmlan = true, .has_obd = false, .has_lin = true, + .has_spi = false, + .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, .init = white_init, .enable_can_transceiver = white_enable_can_transceiver, .enable_can_transceivers = white_enable_can_transceivers, .set_led = white_set_led, - .set_usb_power_mode = white_set_usb_power_mode, .set_gps_mode = white_set_gps_mode, .set_can_mode = white_set_can_mode, - .usb_power_mode_tick = unused_usb_power_mode_tick, .check_ignition = white_check_ignition, .read_current = white_read_current, .set_fan_enabled = unused_set_fan_enabled, diff --git a/panda/board/bootstub.c b/panda/board/bootstub.c index a31721232..a2bf7dad7 100644 --- a/panda/board/bootstub.c +++ b/panda/board/bootstub.c @@ -39,7 +39,6 @@ int main(void) { disable_interrupts(); clock_init(); - detect_external_debug_serial(); detect_board_type(); if (enter_bootloader_mode == ENTER_SOFTLOADER_MAGIC) { diff --git a/panda/board/bootstub_declarations.h b/panda/board/bootstub_declarations.h index 8e9f07312..06a24a2b9 100644 --- a/panda/board/bootstub_declarations.h +++ b/panda/board/bootstub_declarations.h @@ -3,6 +3,7 @@ void puts(const char *a){ UNUSED(a); } void puth(uint8_t i){ UNUSED(i); } void puth2(uint8_t i){ UNUSED(i); } void puth4(uint8_t i){ UNUSED(i); } +void hexdump(const void *a, int l){ UNUSED(a); UNUSED(l); } typedef struct board board; typedef struct harness_configuration harness_configuration; // No CAN support on bootloader diff --git a/panda/board/config.h b/panda/board/config.h index 6a25a398d..cc54df256 100644 --- a/panda/board/config.h +++ b/panda/board/config.h @@ -6,6 +6,7 @@ //#define DEBUG_USB //#define DEBUG_SPI //#define DEBUG_FAULTS +//#define DEBUG_COMMS #define DEEPSLEEP_WAKEUP_DELAY 3U diff --git a/panda/board/dfu_util_f4.sh b/panda/board/dfu_util_f4.sh new file mode 100755 index 000000000..8685c5098 --- /dev/null +++ b/panda/board/dfu_util_f4.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env sh +set -e + +DFU_UTIL="dfu-util" + +scons -u -j$(nproc) + +PYTHONPATH=.. python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true +sleep 1 +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D obj/panda.bin.signed +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.panda.bin diff --git a/panda/board/recover_h7.sh b/panda/board/dfu_util_h7.sh similarity index 100% rename from panda/board/recover_h7.sh rename to panda/board/dfu_util_h7.sh diff --git a/panda/board/drivers/bxcan.h b/panda/board/drivers/bxcan.h index 41b22373f..9cab16412 100644 --- a/panda/board/drivers/bxcan.h +++ b/panda/board/drivers/bxcan.h @@ -68,31 +68,34 @@ void can_set_gmlan(uint8_t bus) { } } +void update_can_health_pkt(uint8_t can_number, bool error_irq) { + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + uint32_t esr_reg = CAN->ESR; + + if (error_irq) { + can_health[can_number].total_error_cnt += 1U; + CAN->MSR = CAN_MSR_ERRI; + llcan_clear_send(CAN); + } + + can_health[can_number].bus_off = ((esr_reg & CAN_ESR_BOFF) >> CAN_ESR_BOFF_Pos); + can_health[can_number].bus_off_cnt += can_health[can_number].bus_off; + can_health[can_number].error_warning = ((esr_reg & CAN_ESR_EWGF) >> CAN_ESR_EWGF_Pos); + can_health[can_number].error_passive = ((esr_reg & CAN_ESR_EPVF) >> CAN_ESR_EPVF_Pos); + + can_health[can_number].last_error = ((esr_reg & CAN_ESR_LEC) >> CAN_ESR_LEC_Pos); + if ((can_health[can_number].last_error != 0U) && (can_health[can_number].last_error != 7U)) { + can_health[can_number].last_stored_error = can_health[can_number].last_error; + } + + can_health[can_number].receive_error_cnt = ((esr_reg & CAN_ESR_REC) >> CAN_ESR_REC_Pos); + can_health[can_number].transmit_error_cnt = ((esr_reg & CAN_ESR_TEC) >> CAN_ESR_TEC_Pos); +} + // CAN error -void can_sce(CAN_TypeDef *CAN) { +void can_sce(uint8_t can_number) { ENTER_CRITICAL(); - - #ifdef DEBUG - if (CAN==CAN1) puts("CAN1: "); - if (CAN==CAN2) puts("CAN2: "); - #ifdef CAN3 - if (CAN==CAN3) puts("CAN3: "); - #endif - puts("MSR:"); - puth(CAN->MSR); - puts(" TSR:"); - puth(CAN->TSR); - puts(" RF0R:"); - puth(CAN->RF0R); - puts(" RF1R:"); - puth(CAN->RF1R); - puts(" ESR:"); - puth(CAN->ESR); - puts("\n"); - #endif - - can_err_cnt += 1; - llcan_clear_send(CAN); + update_can_health_pkt(can_number, true); EXIT_CRITICAL(); } @@ -107,11 +110,13 @@ 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->TSR |= (CAN_TSR_TERR0 | CAN_TSR_ALST0); + } if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { // add successfully transmitted message to my fifo if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) { - can_txd_cnt += 1; - if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) { CANPacket_t to_push; to_push.returned = 1U; @@ -123,19 +128,7 @@ void process_can(uint8_t can_number) { WORD_TO_BYTE_ARRAY(&to_push.data[0], CAN->sTxMailBox[0].TDLR); WORD_TO_BYTE_ARRAY(&to_push.data[4], CAN->sTxMailBox[0].TDHR); - can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U; - } - - if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) { - #ifdef DEBUG - puts("CAN TX ERROR!\n"); - #endif - } - - if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) { - #ifdef DEBUG - puts("CAN TX ARBITRATION LOST!\n"); - #endif + rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; } // clear interrupt @@ -144,7 +137,7 @@ void process_can(uint8_t can_number) { } if (can_pop(can_queues[bus_number], &to_send)) { - can_tx_cnt += 1; + can_health[can_number].total_tx_cnt += 1U; // only send if we have received a packet CAN->sTxMailBox[0].TIR = ((to_send.extended != 0U) ? (to_send.addr << 3) : (to_send.addr << 21)) | (to_send.extended << 2); CAN->sTxMailBox[0].TDTR = to_send.data_len_code; @@ -157,6 +150,7 @@ void process_can(uint8_t can_number) { } } + update_can_health_pkt(can_number, false); EXIT_CRITICAL(); } } @@ -166,8 +160,14 @@ void process_can(uint8_t can_number) { void can_rx(uint8_t can_number) { CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(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->RF0R &= ~(CAN_RF0R_FOVR0); + } + while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) { - can_rx_cnt += 1; + can_health[can_number].total_rx_cnt += 1U; // can is live pending_can_live = 1; @@ -197,30 +197,32 @@ void can_rx(uint8_t can_number) { to_send.data_len_code = to_push.data_len_code; (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]); can_send(&to_send, bus_fwd_num, true); + can_health[can_number].total_fwd_cnt += 1U; } - can_rx_errs += safety_rx_hook(&to_push) ? 0U : 1U; + safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U; ignition_can_hook(&to_push); current_board->set_led(LED_BLUE, true); - can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U; + rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; // next + update_can_health_pkt(can_number, false); CAN->RF0R |= CAN_RF0R_RFOM0; } } void CAN1_TX_IRQ_Handler(void) { process_can(0); } void CAN1_RX0_IRQ_Handler(void) { can_rx(0); } -void CAN1_SCE_IRQ_Handler(void) { can_sce(CAN1); } +void CAN1_SCE_IRQ_Handler(void) { can_sce(0); } void CAN2_TX_IRQ_Handler(void) { process_can(1); } void CAN2_RX0_IRQ_Handler(void) { can_rx(1); } -void CAN2_SCE_IRQ_Handler(void) { can_sce(CAN2); } +void CAN2_SCE_IRQ_Handler(void) { can_sce(1); } void CAN3_TX_IRQ_Handler(void) { process_can(2); } void CAN3_RX0_IRQ_Handler(void) { can_rx(2); } -void CAN3_SCE_IRQ_Handler(void) { can_sce(CAN3); } +void CAN3_SCE_IRQ_Handler(void) { can_sce(2); } bool can_init(uint8_t can_number) { bool ret = false; diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index e38f2066b..0af0b41b0 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -12,13 +12,16 @@ typedef struct { uint32_t can_data_speed; bool canfd_enabled; bool brs_enabled; + bool canfd_non_iso; } bus_config_t; -uint32_t can_rx_errs = 0; -uint32_t can_send_errs = 0; -uint32_t can_fwd_errs = 0; +uint32_t safety_tx_blocked = 0; +uint32_t safety_rx_invalid = 0; +uint32_t tx_buffer_overflow = 0; +uint32_t rx_buffer_overflow = 0; uint32_t gmlan_send_errs = 0; -uint32_t blocked_msg_cnt = 0; + +can_health_t can_health[] = {{0}, {0}, {0}}; extern int can_live; extern int pending_can_live; @@ -50,25 +53,19 @@ void process_can(uint8_t can_number); #ifdef STM32H7 __attribute__((section(".ram_d1"))) can_buffer(rx_q, 0x1000) -__attribute__((section(".ram_d1"))) can_buffer(txgmlan_q, 0x1A0) +__attribute__((section(".ram_d1"))) can_buffer(tx2_q, 0x1A0) +__attribute__((section(".ram_d2"))) can_buffer(txgmlan_q, 0x1A0) #else can_buffer(rx_q, 0x1000) +can_buffer(tx2_q, 0x1A0) can_buffer(txgmlan_q, 0x1A0) #endif can_buffer(tx1_q, 0x1A0) -can_buffer(tx2_q, 0x1A0) can_buffer(tx3_q, 0x1A0) // FIXME: // cppcheck-suppress misra-c2012-9.3 can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q}; -// global CAN stats -int can_rx_cnt = 0; -int can_tx_cnt = 0; -int can_txd_cnt = 0; -int can_err_cnt = 0; -int can_overflow_cnt = 0; - // ********************* interrupt safe queue ********************* bool can_pop(can_ring *q, CANPacket_t *elem) { bool ret = 0; @@ -105,7 +102,6 @@ bool can_push(can_ring *q, CANPacket_t *elem) { } EXIT_CRITICAL(); if (!ret) { - can_overflow_cnt++; #ifdef DEBUG puts("can_push to "); if (q == &can_rx_q) { @@ -161,10 +157,10 @@ void can_clear(can_ring *q) { // Helpers // Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3 bus_config_t bus_config[] = { - { .bus_lookup = 0U, .can_num_lookup = 0U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false }, - { .bus_lookup = 1U, .can_num_lookup = 1U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false }, - { .bus_lookup = 2U, .can_num_lookup = 2U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false }, - { .bus_lookup = 0xFFU, .can_num_lookup = 0xFFU, .can_speed = 333U, .can_data_speed = 333U, .canfd_enabled = false, .brs_enabled = false }, + { .bus_lookup = 0U, .can_num_lookup = 0U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, + { .bus_lookup = 1U, .can_num_lookup = 1U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, + { .bus_lookup = 2U, .can_num_lookup = 2U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, + { .bus_lookup = 0xFFU, .can_num_lookup = 0xFFU, .can_speed = 333U, .can_data_speed = 333U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, }; #define CANIF_FROM_CAN_NUM(num) (cans[num]) @@ -174,6 +170,9 @@ bus_config_t bus_config[] = { void can_init_all(void) { bool ret = true; for (uint8_t i=0U; i < CAN_CNT; i++) { + if (!current_board->has_canfd) { + bus_config[i].can_data_speed = 0U; + } can_clear(can_queues[i]); ret &= can_init(i); } @@ -230,13 +229,23 @@ void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook) { if ((bus_number == 3U) && (bus_config[3].can_num_lookup == 0xFFU)) { gmlan_send_errs += bitbang_gmlan(to_push) ? 0U : 1U; } else { - can_fwd_errs += can_push(can_queues[bus_number], to_push) ? 0U : 1U; + tx_buffer_overflow += can_push(can_queues[bus_number], to_push) ? 0U : 1U; process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); } } } else { - blocked_msg_cnt += 1U; + safety_tx_blocked += 1U; to_push->rejected = 1U; - can_send_errs += can_push(&can_rx_q, to_push) ? 0U : 1U; + rx_buffer_overflow += can_push(&can_rx_q, to_push) ? 0U : 1U; } } + +bool is_speed_valid(uint32_t speed, const uint32_t *speeds, uint8_t len) { + bool ret = false; + for (uint8_t i = 0U; i < len; i++) { + if (speeds[i] == speed) { + ret = true; + } + } + return ret; +} diff --git a/panda/board/drivers/fan.h b/panda/board/drivers/fan.h index 9d33df70a..cb5fbabe6 100644 --- a/panda/board/drivers/fan.h +++ b/panda/board/drivers/fan.h @@ -37,10 +37,13 @@ void fan_tick(void){ if (fan_state.stall_counter > FAN_STALL_THRESHOLD) { // Stall detected, power cycling fan controller current_board->set_fan_enabled(false); - fan_state.error_integral = 0U; + + // 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)); } } else { fan_state.stall_counter = 0U; + fan_state.error_integral = 0.0f; } // Update controller diff --git a/panda/board/drivers/fdcan.h b/panda/board/drivers/fdcan.h index 8a87c4149..0d3aa1687 100644 --- a/panda/board/drivers/fdcan.h +++ b/panda/board/drivers/fdcan.h @@ -4,9 +4,6 @@ #define CANFD -#define BUS_OFF_FAIL_LIMIT 2U -uint8_t bus_off_err[] = {0U, 0U, 0U}; - typedef struct { volatile uint32_t header[2]; volatile uint32_t data_word[CANPACKET_DATA_SIZE_MAX/4U]; @@ -23,6 +20,7 @@ bool can_set_speed(uint8_t can_number) { CANx, bus_config[bus_number].can_speed, bus_config[bus_number].can_data_speed, + bus_config[bus_number].canfd_non_iso, can_loopback, (unsigned int)(can_silent) & (1U << can_number) ); @@ -34,21 +32,46 @@ void can_set_gmlan(uint8_t bus) { puts("GMLAN not available on red panda\n"); } -void cycle_transceiver(uint8_t can_number) { - // FDCAN1 = trans 1, FDCAN3 = trans 3, FDCAN2 = trans 2 normal or 4 flipped harness - uint8_t transceiver_number = can_number; - if (can_number == 2U) { - uint8_t flip = (car_harness_status == HARNESS_STATUS_FLIPPED) ? 2U : 0U; - transceiver_number += flip; +// ***************************** CAN ***************************** +void update_can_health_pkt(uint8_t can_number, bool error_irq) { + ENTER_CRITICAL(); + + FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number); + uint32_t psr_reg = CANx->PSR; + uint32_t ecr_reg = CANx->ECR; + + can_health[can_number].bus_off = ((psr_reg & FDCAN_PSR_BO) >> FDCAN_PSR_BO_Pos); + can_health[can_number].bus_off_cnt += can_health[can_number].bus_off; + can_health[can_number].error_warning = ((psr_reg & FDCAN_PSR_EW) >> FDCAN_PSR_EW_Pos); + can_health[can_number].error_passive = ((psr_reg & FDCAN_PSR_EP) >> FDCAN_PSR_EP_Pos); + + can_health[can_number].last_error = ((psr_reg & FDCAN_PSR_LEC) >> FDCAN_PSR_LEC_Pos); + if ((can_health[can_number].last_error != 0U) && (can_health[can_number].last_error != 7U)) { + can_health[can_number].last_stored_error = can_health[can_number].last_error; } - current_board->enable_can_transceiver(transceiver_number, false); - delay(20000); - current_board->enable_can_transceiver(transceiver_number, true); - bus_off_err[can_number] = 0U; - puts("Cycled transceiver number: "); puth(transceiver_number); puts("\n"); + + can_health[can_number].last_data_error = ((psr_reg & FDCAN_PSR_DLEC) >> FDCAN_PSR_DLEC_Pos); + if ((can_health[can_number].last_data_error != 0U) && (can_health[can_number].last_data_error != 7U)) { + can_health[can_number].last_data_stored_error = can_health[can_number].last_data_error; + } + + can_health[can_number].receive_error_cnt = ((ecr_reg & FDCAN_ECR_REC) >> FDCAN_ECR_REC_Pos); + can_health[can_number].transmit_error_cnt = ((ecr_reg & FDCAN_ECR_TEC) >> FDCAN_ECR_TEC_Pos); + + + 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; + } + llcan_clear_send(CANx); + } + EXIT_CRITICAL(); } -// ***************************** CAN ***************************** void process_can(uint8_t can_number) { if (can_number != 0xffU) { ENTER_CRITICAL(); @@ -61,7 +84,8 @@ void process_can(uint8_t can_number) { if ((CANx->TXFQS & FDCAN_TXFQS_TFQF) == 0) { CANPacket_t to_send; if (can_pop(can_queues[bus_number], &to_send)) { - can_tx_cnt += 1; + 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); uint8_t tx_index = (CANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F; // only send if we have received a packet @@ -80,7 +104,6 @@ void process_can(uint8_t can_number) { CANx->TXBAR = (1UL << tx_index); // Send back to USB - can_txd_cnt += 1; CANPacket_t to_push; to_push.returned = 1U; @@ -90,33 +113,13 @@ void process_can(uint8_t can_number) { to_push.bus = to_send.bus; to_push.data_len_code = to_send.data_len_code; (void)memcpy(to_push.data, to_send.data, dlc_to_len[to_push.data_len_code]); - can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U; + rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; usb_cb_ep3_out_complete(); } } - // Recover after Bus-off state - if (((CANx->PSR & FDCAN_PSR_BO) != 0) && ((CANx->CCCR & FDCAN_CCCR_INIT) != 0)) { - bus_off_err[can_number] += 1U; - puts("CAN is in Bus_Off state! Resetting... CAN number: "); puth(can_number); puts("\n"); - if (bus_off_err[can_number] > BUS_OFF_FAIL_LIMIT) { - cycle_transceiver(can_number); - } - CANx->IR = 0xFFC60000U; // Reset all flags(Only errors!) - CANx->CCCR &= ~(FDCAN_CCCR_INIT); - uint32_t timeout_counter = 0U; - while((CANx->CCCR & FDCAN_CCCR_INIT) != 0) { - // Delay for about 1ms - delay(10000); - timeout_counter++; - - if(timeout_counter >= CAN_INIT_TIMEOUT_MS){ - puts(CAN_NAME_FROM_CANIF(CANx)); puts(" Bus_Off reset timed out!\n"); - break; - } - } - } + update_can_health_pkt(can_number, false); EXIT_CRITICAL(); } } @@ -126,19 +129,18 @@ void process_can(uint8_t can_number) { void can_rx(uint8_t can_number) { FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - uint8_t rx_fifo_idx; // Rx FIFO 0 new message if((CANx->IR & FDCAN_IR_RF0N) != 0) { CANx->IR |= FDCAN_IR_RF0N; while((CANx->RXF0S & FDCAN_RXF0S_F0FL) != 0) { - can_rx_cnt += 1; + can_health[can_number].total_rx_cnt += 1U; // can is live pending_can_live = 1; // getting new message index (0 to 63) - rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F); + uint8_t rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F); uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET); CANPacket_t to_push; @@ -176,13 +178,14 @@ void can_rx(uint8_t can_number) { to_send.data_len_code = to_push.data_len_code; (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]); can_send(&to_send, bus_fwd_num, true); + can_health[can_number].total_fwd_cnt += 1U; } - can_rx_errs += safety_rx_hook(&to_push) ? 0U : 1U; + safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U; ignition_can_hook(&to_push); current_board->set_led(LED_BLUE, true); - can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U; + rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; // Enable CAN FD and BRS if CAN FD message was received if (!(bus_config[can_number].canfd_enabled) && (canfd_frame)) { @@ -196,15 +199,10 @@ void can_rx(uint8_t can_number) { CANx->RXF0A = rx_fifo_idx; } - } else if((CANx->IR & (FDCAN_IR_PEA | FDCAN_IR_PED | FDCAN_IR_RF0L | FDCAN_IR_RF0F | FDCAN_IR_EW | FDCAN_IR_MRAF | FDCAN_IR_TOO)) != 0) { - #ifdef DEBUG - puts("FDCAN error, FDCAN_IR: ");puth(CANx->IR);puts("\n"); - #endif - CANx->IR |= (FDCAN_IR_PEA | FDCAN_IR_PED | FDCAN_IR_RF0L | FDCAN_IR_RF0F | FDCAN_IR_EW | FDCAN_IR_MRAF | FDCAN_IR_TOO); // Clean all error flags - can_err_cnt += 1; - } else { - } + // Error handling + bool error_irq = ((CANx->IR & (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EW | FDCAN_IR_EP | FDCAN_IR_ELO | FDCAN_IR_BO | FDCAN_IR_TEFL | FDCAN_IR_RF0L)) != 0); + update_can_health_pkt(can_number, error_irq); } void FDCAN1_IT0_IRQ_Handler(void) { can_rx(0); } diff --git a/panda/board/drivers/spi.h b/panda/board/drivers/spi.h new file mode 100644 index 000000000..422678556 --- /dev/null +++ b/panda/board/drivers/spi.h @@ -0,0 +1,164 @@ +#pragma once + +#define SPI_BUF_SIZE 1024U + +#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]; +#else +uint8_t spi_buf_rx[SPI_BUF_SIZE]; +uint8_t spi_buf_tx[SPI_BUF_SIZE]; +#endif + +#define SPI_CHECKSUM_START 0xABU +#define SPI_SYNC_BYTE 0x5AU +#define SPI_HACK 0x79U +#define SPI_DACK 0x85U +#define SPI_NACK 0x1FU + +// SPI states +enum { + SPI_STATE_HEADER, + SPI_STATE_HEADER_ACK, + SPI_STATE_HEADER_NACK, + SPI_STATE_DATA_RX, + SPI_STATE_DATA_RX_ACK, + SPI_STATE_DATA_TX +}; + +uint8_t spi_state = SPI_STATE_HEADER; +uint8_t spi_endpoint; +uint16_t spi_data_len_mosi; +uint16_t spi_data_len_miso; + +#define SPI_HEADER_SIZE 7U + +// low level SPI prototypes +void llspi_init(void); +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(); + + // Start the first packet! + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); +} + +bool check_checksum(uint8_t *data, uint16_t len) { + // TODO: can speed this up by casting the bulk to uint32_t and xor-ing the bytes afterwards + uint8_t checksum = SPI_CHECKSUM_START; + for(uint16_t i = 0U; i < len; i++){ + checksum ^= data[i]; + } + return checksum == 0U; +} + +void spi_handle_rx(void) { + uint8_t next_rx_state = SPI_STATE_HEADER; + + // parse header + spi_endpoint = spi_buf_rx[1]; + spi_data_len_mosi = (spi_buf_rx[3] << 8) | spi_buf_rx[2]; + 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)) { + // response: ACK and start receiving data portion + spi_buf_tx[0] = SPI_HACK; + next_rx_state = SPI_STATE_HEADER_ACK; + } else { + // response: NACK and reset state machine + puts("- 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; + } + 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)) { + 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; + } else { + puts("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; + } else { + puts("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; + } 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; + } else { + puts("SPI: did expect data for can_write\n"); + } + } else { + puts("SPI: unexpected endpoint"); puth(spi_endpoint); puts("\n"); + } + } else { + // Checksum was incorrect + reponse_ack = false; + puts("- incorrect data checksum\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; + + // 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; + + // Write response + llspi_miso_dma(spi_buf_tx, response_len + 4U); + + next_rx_state = SPI_STATE_DATA_TX; + } else { + puts("SPI: RX unexpected state: "); puth(spi_state); puts("\n"); + } + + spi_state = next_rx_state; +} + +void spi_handle_tx(void) { + if (spi_state == SPI_STATE_HEADER_ACK) { + // ACK was sent, queue up the RX buf for the data + checksum + spi_state = SPI_STATE_DATA_RX; + llspi_mosi_dma(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi + 1U); + } else if (spi_state == SPI_STATE_HEADER_NACK) { + // Reset state + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); + } else if (spi_state == SPI_STATE_DATA_TX) { + // Reset state + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); + } else { + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); + puts("SPI: TX unexpected state: "); puth(spi_state); puts("\n"); + } +} diff --git a/panda/board/drivers/uart.h b/panda/board/drivers/uart.h index d6107a6c3..2caa44fa5 100644 --- a/panda/board/drivers/uart.h +++ b/panda/board/drivers/uart.h @@ -150,14 +150,8 @@ void clear_uart_buff(uart_ring *q) { // ************************ High-level debug functions ********************** void putch(const char a) { - if (has_external_debug_serial) { - // assuming debugging is important if there's external serial connected - while (!putc(&uart_ring_debug, a)); - - } else { - // misra-c2012-17.7: serial debug function, ok to ignore output - (void)injectc(&uart_ring_debug, a); - } + // misra-c2012-17.7: serial debug function, ok to ignore output + (void)injectc(&uart_ring_debug, a); } void puts(const char *a) { diff --git a/panda/board/early_init.h b/panda/board/early_init.h index ae9a90362..6c60b20fb 100644 --- a/panda/board/early_init.h +++ b/panda/board/early_init.h @@ -48,7 +48,6 @@ void early_initialization(void) { // early GPIOs float everything early_gpio_float(); - detect_external_debug_serial(); detect_board_type(); if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) { diff --git a/panda/board/faults.h b/panda/board/faults.h index 8f6ee28fa..ad938b9d0 100644 --- a/panda/board/faults.h +++ b/panda/board/faults.h @@ -26,6 +26,7 @@ #define FAULT_INTERRUPT_RATE_CLOCK_SOURCE (1U << 20) #define FAULT_INTERRUPT_RATE_TICK (1U << 21) #define FAULT_INTERRUPT_RATE_EXTI (1U << 22) +#define FAULT_INTERRUPT_RATE_SPI (1U << 23) // Permanent faults #define PERMANENT_FAULTS 0U diff --git a/panda/board/health.h b/panda/board/health.h index 3c7d14422..32b9bdd14 100644 --- a/panda/board/health.h +++ b/panda/board/health.h @@ -1,12 +1,15 @@ -// When changing this struct, python/__init__.py needs to be kept up to date! -#define HEALTH_PACKET_VERSION 8 +// When changing these structs, python/__init__.py needs to be kept up to date! + +#define HEALTH_PACKET_VERSION 11 + struct __attribute__((packed)) health_t { uint32_t uptime_pkt; uint32_t voltage_pkt; uint32_t current_pkt; - uint32_t can_rx_errs_pkt; - uint32_t can_send_errs_pkt; - uint32_t can_fwd_errs_pkt; + uint32_t safety_tx_blocked_pkt; + uint32_t safety_rx_invalid_pkt; + uint32_t tx_buffer_overflow_pkt; + uint32_t rx_buffer_overflow_pkt; uint32_t gmlan_send_errs_pkt; uint32_t faults_pkt; uint8_t ignition_line_pkt; @@ -14,14 +17,38 @@ struct __attribute__((packed)) health_t { uint8_t controls_allowed_pkt; uint8_t gas_interceptor_detected_pkt; uint8_t car_harness_status_pkt; - uint8_t usb_power_mode_pkt; uint8_t safety_mode_pkt; uint16_t safety_param_pkt; uint8_t fault_status_pkt; uint8_t power_save_enabled_pkt; uint8_t heartbeat_lost_pkt; uint16_t alternative_experience_pkt; - uint32_t blocked_msg_cnt_pkt; float interrupt_load; uint8_t fan_power; + uint8_t safety_rx_checks_invalid; }; + +#define CAN_HEALTH_PACKET_VERSION 3 +typedef struct __attribute__((packed)) { + uint8_t bus_off; + uint32_t bus_off_cnt; + uint8_t error_warning; + uint8_t error_passive; + uint8_t last_error; // real time LEC value + 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 + uint32_t total_tx_cnt; + uint32_t total_rx_cnt; + uint32_t total_fwd_cnt; // Messages forwarded from one bus to another + uint16_t can_speed; + uint16_t can_data_speed; + uint8_t canfd_enabled; + uint8_t brs_enabled; + uint8_t canfd_non_iso; +} can_health_t; diff --git a/panda/board/main.c b/panda/board/main.c index 7d67119b0..5b978fea3 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -12,6 +12,8 @@ #include "power_saving.h" #include "safety.h" +#include "health.h" + #include "drivers/can_common.h" #ifdef STM32H7 @@ -51,20 +53,6 @@ void debug_ring_callback(uart_ring *ring) { if (rcv == 'x') { NVIC_SystemReset(); } - - // enable CDP mode - if (rcv == 'C') { - puts("switching USB to CDP mode\n"); - current_board->set_usb_power_mode(USB_POWER_CDP); - } - if (rcv == 'c') { - puts("switching USB to client mode\n"); - current_board->set_usb_power_mode(USB_POWER_CLIENT); - } - if (rcv == 'D') { - puts("switching USB to DCP mode\n"); - current_board->set_usb_power_mode(USB_POWER_DCP); - } } } @@ -85,7 +73,8 @@ void set_safety_mode(uint16_t mode, uint16_t param) { } } } - blocked_msg_cnt = 0; + safety_tx_blocked = 0; + safety_rx_invalid = 0; switch (mode_copy) { case SAFETY_SILENT: @@ -158,15 +147,13 @@ void tick_handler(void) { // siren current_board->set_siren((loop_counter & 1U) && (siren_enabled || (siren_countdown > 0U))); - // tick drivers + // tick drivers at 8Hz fan_tick(); // decimated to 1Hz if (loop_counter == 0U) { can_live = pending_can_live; - current_board->usb_power_mode_tick(uptime_cnt); - //puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n"); // reset this every 16th pass @@ -188,6 +175,10 @@ void tick_handler(void) { // unless we are in power saving mode current_board->set_led(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED)); + // tick drivers at 1Hz + const bool recent_heartbeat = heartbeat_counter == 0U; + current_board->board_tick(check_started(), usb_enumerated, recent_heartbeat); + // increase heartbeat counter and cap it at the uint32 limit if (heartbeat_counter < __UINT32_MAX__) { heartbeat_counter += 1U; @@ -254,11 +245,6 @@ void tick_handler(void) { fan_set_power(0U); } } - - // enter CDP mode when car starts to ensure we are charging a turned off EON - if (check_started() && ((usb_power_mode != USB_POWER_CDP) || !usb_enumerated)) { - current_board->set_usb_power_mode(USB_POWER_CDP); - } } // check registers @@ -289,7 +275,6 @@ void EXTI_IRQ_Handler(void) { exti_irq_clear(); clock_init(); - current_board->set_usb_power_mode(USB_POWER_CDP); set_power_save_state(POWER_SAVE_STATUS_DISABLED); deepsleep_allowed = false; heartbeat_counter = 0U; @@ -327,7 +312,6 @@ int main(void) { // init early devices clock_init(); peripherals_init(); - detect_external_debug_serial(); detect_board_type(); adc_init(); @@ -342,7 +326,6 @@ int main(void) { puts("Config:\n"); puts(" Board type: "); puts(current_board->board_type); puts("\n"); - puts(has_external_debug_serial ? " Real serial\n" : " USB serial\n"); // init board current_board->init(); @@ -350,13 +333,6 @@ int main(void) { // panda has an FPU, let's use it! enable_fpu(); - // enable main uart if it's connected - if (has_external_debug_serial) { - // WEIRDNESS: without this gate around the UART, it would "crash", but only if the ESP is enabled - // assuming it's because the lines were left floating and spurious noise was on them - uart_init(&uart_ring_debug, 115200); - } - if (current_board->has_gps) { uart_init(&uart_ring_gps, 9600); } else { @@ -390,6 +366,12 @@ int main(void) { // enable USB (right before interrupts or enum can fail!) usb_init(); +#ifdef ENABLE_SPI + if (current_board->has_spi) { + spi_init(); + } +#endif + puts("**** INTERRUPTS ON ****\n"); enable_interrupts(); @@ -399,19 +381,17 @@ int main(void) { for (cnt=0;;cnt++) { if (power_save_status == POWER_SAVE_STATUS_DISABLED) { #ifdef DEBUG_FAULTS - if(fault_status == FAULT_STATUS_NONE){ + if (fault_status == FAULT_STATUS_NONE) { #endif - uint32_t div_mode = ((usb_power_mode == USB_POWER_DCP) ? 4U : 1U); - // useful for debugging, fade breaks = panda is overloaded - for(uint32_t fade = 0U; fade < MAX_LED_FADE; fade += div_mode){ + for (uint32_t fade = 0U; fade < MAX_LED_FADE; fade += 1U) { current_board->set_led(LED_RED, true); delay(fade >> 4); current_board->set_led(LED_RED, false); delay((MAX_LED_FADE - fade) >> 4); } - for(uint32_t fade = MAX_LED_FADE; fade > 0U; fade -= div_mode){ + for (uint32_t fade = MAX_LED_FADE; fade > 0U; fade -= 1U) { current_board->set_led(LED_RED, true); delay(fade >> 4); current_board->set_led(LED_RED, false); @@ -430,7 +410,6 @@ int main(void) { if (deepsleep_allowed && !usb_enumerated && !check_started() && ignition_seen && (heartbeat_counter > 20U)) { usb_soft_disconnect(true); fan_set_power(0U); - current_board->set_usb_power_mode(USB_POWER_CLIENT); NVIC_DisableIRQ(TICK_TIMER_IRQ); delay(512000U); diff --git a/panda/board/main_comms.h b/panda/board/main_comms.h index 1f4914a87..3c41596c3 100644 --- a/panda/board/main_comms.h +++ b/panda/board/main_comms.h @@ -1,5 +1,3 @@ -#include "health.h" - extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used // Prototypes @@ -20,18 +18,18 @@ int get_health_pkt(void *dat) { health->controls_allowed_pkt = controls_allowed; health->gas_interceptor_detected_pkt = gas_interceptor_detected; - health->can_rx_errs_pkt = can_rx_errs; - health->can_send_errs_pkt = can_send_errs; - health->can_fwd_errs_pkt = can_fwd_errs; + health->safety_tx_blocked_pkt = safety_tx_blocked; + health->safety_rx_invalid_pkt = safety_rx_invalid; + 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->usb_power_mode_pkt = usb_power_mode; health->safety_mode_pkt = (uint8_t)(current_safety_mode); health->safety_param_pkt = current_safety_param; health->alternative_experience_pkt = alternative_experience; health->power_save_enabled_pkt = (uint8_t)(power_save_status == POWER_SAVE_STATUS_ENABLED); health->heartbeat_lost_pkt = (uint8_t)(heartbeat_lost); - health->blocked_msg_cnt_pkt = blocked_msg_cnt; + health->safety_rx_checks_invalid = safety_rx_checks_invalid; health->fault_status_pkt = fault_status; health->faults_pkt = faults; @@ -178,6 +176,14 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { unsigned int resp_len = 0; uart_ring *ur = NULL; timestamp_t t; + +#ifdef DEBUG_COMMS + puts("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); puts("\n"); + puts("- request "); puth(req->request); puts("\n"); + puts("- param1 "); puth(req->param1); puts("\n"); + puts("- param2 "); puth(req->param2); puts("\n"); +#endif + switch (req->request) { // **** 0xa0: get rtc time case 0xa0: @@ -243,20 +249,24 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { case 0xb3: current_board->set_phone_power(req->param1 > 0U); break; - // **** 0xc0: get CAN debug info - case 0xc0: - puts("can tx: "); puth(can_tx_cnt); - puts(" txd: "); puth(can_txd_cnt); - puts(" rx: "); puth(can_rx_cnt); - puts(" err: "); puth(can_err_cnt); - puts("\n"); - break; // **** 0xc1: get hardware type case 0xc1: resp[0] = hw_type; resp_len = 1; break; // **** 0xd0: fetch serial number + case 0xc2: + COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); + if (req->param1 < 3U) { + can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); + can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); + can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; + can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; + can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; + resp_len = sizeof(can_health[req->param1]); + (void)memcpy(resp, &can_health[req->param1], resp_len); + } + break; case 0xd0: // addresses are OTP if (req->param1 == 1U) { @@ -377,12 +387,12 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { case 0xdd: resp[0] = HEALTH_PACKET_VERSION; resp[1] = CAN_PACKET_VERSION; - resp_len = 2; + resp[2] = CAN_HEALTH_PACKET_VERSION; + resp_len = 3; break; // **** 0xde: set can bitrate case 0xde: - if (req->param1 < BUS_CNT) { - // TODO: add sanity check, ideally check if value is correct(from array of correct values) + if ((req->param1 < BUS_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { bus_config[req->param1].can_speed = req->param2; bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); UNUSED(ret); @@ -459,10 +469,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { can_loopback = (req->param1 > 0U); can_init_all(); break; - // **** 0xe6: set USB power - case 0xe6: - current_board->set_usb_power_mode(req->param1); - break; // **** 0xe7: set power save state case 0xe7: set_power_save_state(req->param1); @@ -537,10 +543,11 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { heartbeat_disabled = true; } break; - // **** 0xde: set CAN FD data bitrate + // **** 0xf9: set CAN FD data bitrate case 0xf9: - if (req->param1 < CAN_CNT) { - // TODO: add sanity check, ideally check if value is correct (from array of correct values) + if ((req->param1 < CAN_CNT) && + current_board->has_canfd && + is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { bus_config[req->param1].can_data_speed = req->param2; bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); @@ -548,18 +555,18 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { UNUSED(ret); } break; - // **** 0xfa: check if CAN FD and BRS are enabled - case 0xfa: - if (req->param1 < CAN_CNT) { - resp[0] = bus_config[req->param1].canfd_enabled; - resp[1] = bus_config[req->param1].brs_enabled; - resp_len = 2; - } - break; // **** 0xfb: allow highest power saving mode (stop) to be entered case 0xfb: deepsleep_allowed = true; break; + // **** 0xfc: set CAN FD non-ISO mode + case 0xfc: + if ((req->param1 < CAN_CNT) && current_board->has_canfd) { + bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; default: puts("NO HANDLER "); puth(req->request); diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h index c1fc87035..208afadfb 100644 --- a/panda/board/main_declarations.h +++ b/panda/board/main_declarations.h @@ -3,6 +3,7 @@ void puts(const char *a); void puth(unsigned int i); void puth2(unsigned int i); void puth4(unsigned int i); +void hexdump(const void *a, int l); typedef struct board board; typedef struct harness_configuration harness_configuration; void can_flip_buses(uint8_t bus1, uint8_t bus2); diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c index 757361bb5..5e9c1eac2 100644 --- a/panda/board/pedal/main.c +++ b/panda/board/pedal/main.c @@ -275,7 +275,6 @@ int main(void) { // init devices clock_init(); peripherals_init(); - detect_external_debug_serial(); detect_board_type(); // init board diff --git a/panda/board/recover.sh b/panda/board/recover.sh index 8685c5098..f003cba67 100755 --- a/panda/board/recover.sh +++ b/panda/board/recover.sh @@ -1,11 +1,8 @@ #!/usr/bin/env sh set -e -DFU_UTIL="dfu-util" - scons -u -j$(nproc) - -PYTHONPATH=.. python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true +# 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 -$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D obj/panda.bin.signed -$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.panda.bin +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 e31783a0b..d9f0753e8 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -164,6 +164,7 @@ int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], cons // 1Hz safety function called by main. Now just a check for lagging safety messages void safety_tick(const addr_checks *rx_checks) { + bool rx_checks_invalid = false; uint32_t ts = microsecond_timer_get(); if (rx_checks != NULL) { for (int i=0; i < rx_checks->len; i++) { @@ -176,8 +177,14 @@ void safety_tick(const addr_checks *rx_checks) { if (lagging) { controls_allowed = 0; } + + if (lagging || !is_msg_valid(rx_checks->check, i)) { + rx_checks_invalid = true; + } } } + + safety_rx_checks_invalid = rx_checks_invalid; } void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter) { @@ -250,6 +257,12 @@ void generic_rx_checks(bool stock_ecu_detected) { } brake_pressed_prev = brake_pressed; + // exit controls on rising edge of regen paddle + if (regen_braking && (!regen_braking_prev || vehicle_moving)) { + controls_allowed = 0; + } + regen_braking_prev = regen_braking; + // check if stock ECU is on bus broken by car harness if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) { relay_malfunction_set(); @@ -309,6 +322,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { gas_pressed_prev = false; brake_pressed = false; brake_pressed_prev = false; + regen_braking = false; + regen_braking_prev = false; cruise_engaged_prev = false; vehicle_speed = 0; vehicle_moving = false; @@ -318,7 +333,10 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { rt_torque_last = 0; ts_angle_last = 0; desired_angle_last = 0; - ts_last = 0; + ts_torque_check_last = 0; + ts_steer_req_mismatch_last = 0; + valid_steer_req_count = 0; + invalid_steer_req_count = 0; torque_meas.max = 0; torque_meas.max = 0; @@ -329,6 +347,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { controls_allowed = false; relay_malfunction_reset(); + safety_rx_checks_invalid = false; int set_status = -1; // not set int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config); @@ -492,10 +511,10 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi violation |= rt_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last); if (ts_elapsed > limits.max_rt_interval) { rt_torque_last = desired_torque; - ts_last = ts; + ts_torque_check_last = ts; } } @@ -504,16 +523,51 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi violation = true; } - // no torque if request bit isn't high - if ((steer_req == 0) && (desired_torque != 0)) { - violation = true; + // certain safety modes set their steer request bit low for one or more frame at a + // predefined max frequency to avoid steering faults in certain situations + bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0); + if (!limits.has_steer_req_tolerance) { + if (steer_req_mismatch) { + violation = true; + } + + } else { + if (steer_req_mismatch) { + if (invalid_steer_req_count == 0) { + // disallow torque cut if not enough recent matching steer_req messages + if (valid_steer_req_count < limits.min_valid_request_frames) { + violation = true; + } + + // or we've cut torque too recently in time + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last); + if (ts_elapsed < limits.min_valid_request_rt_interval) { + violation = true; + } + } else { + // or we're cutting more frames consecutively than allowed + if (invalid_steer_req_count >= limits.max_invalid_request_frames) { + violation = true; + } + } + + valid_steer_req_count = 0; + ts_steer_req_mismatch_last = ts; + invalid_steer_req_count = MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames); + } else { + valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames); + invalid_steer_req_count = 0; + } } // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { + valid_steer_req_count = 0; + invalid_steer_req_count = 0; desired_torque_last = 0; rt_torque_last = 0; - ts_last = ts; + ts_torque_check_last = ts; + ts_steer_req_mismatch_last = ts; } return violation; diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index 36ea9dade..b158cf8a4 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -28,9 +28,6 @@ const SteeringLimits CHRYSLER_RAM_HD_STEERING_LIMITS = { .type = TorqueMotorLimited, }; -const int CHRYSLER_STANDSTILL_THRSLD = 10; // about 1m/s -const int CHRYSLER_RAM_STANDSTILL_THRSLD = 3; // about 1m/s changed from wheel rpm to km/h - typedef struct { const int EPS_2; const int ESP_1; @@ -204,16 +201,14 @@ static int chrysler_rx_hook(CANPacket_t *to_push) { } // TODO: use the same message for both - // update speed + // update vehicle moving if ((chrysler_platform != CHRYSLER_PACIFICA) && (bus == 0) && (addr == chrysler_addrs->ESP_8)) { - vehicle_speed = (((GET_BYTE(to_push, 4) & 0x3U) << 8) + GET_BYTE(to_push, 5))*0.0078125; - vehicle_moving = (int)vehicle_speed > CHRYSLER_RAM_STANDSTILL_THRSLD; + vehicle_moving = ((GET_BYTE(to_push, 4) << 8) + GET_BYTE(to_push, 5)) != 0U; } if ((chrysler_platform == CHRYSLER_PACIFICA) && (bus == 0) && (addr == 514)) { int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4); int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4); - vehicle_speed = (speed_l + speed_r) / 2; - vehicle_moving = (int)vehicle_speed > CHRYSLER_STANDSTILL_THRSLD; + vehicle_moving = (speed_l != 0) || (speed_r != 0); } // exit controls on rising edge of gas press diff --git a/panda/board/safety/safety_elm327.h b/panda/board/safety/safety_elm327.h index 7a847c4a3..5535471ea 100644 --- a/panda/board/safety/safety_elm327.h +++ b/panda/board/safety/safety_elm327.h @@ -13,7 +13,7 @@ static int elm327_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { //Check valid 29 bit send addresses for ISO 15765-4 //Check valid 11 bit send addresses for ISO 15765-4 if ((addr != 0x18DB33F1) && ((addr & 0x1FFF00FF) != 0x18DA00F1) && - ((addr & 0x1FFFFF00) != 0x700)) { + ((addr & 0x1FFFFF00) != 0x600) && ((addr & 0x1FFFFF00) != 0x700)) { tx = 0; } return tx; diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index c605d47cd..6556d957d 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -1,37 +1,41 @@ // Safety-relevant CAN messages for Ford vehicles. -#define MSG_ENG_BRAKE_DATA 0x165 // RX from PCM, for driver brake pedal and cruise state -#define MSG_ENG_VEHICLE_SP_THROTTLE 0x204 // RX from PCM, for driver throttle input -#define MSG_DESIRED_TORQ_BRK 0x213 // RX from ABS, for standstill state -#define MSG_STEERING_DATA_FD1 0x083 // TX by OP, ACC control buttons for cancel -#define MSG_LANE_ASSIST_DATA1 0x3CA // TX by OP, Lane Keeping Assist -#define MSG_LATERAL_MOTION_CONTROL 0x3D3 // TX by OP, Lane Centering Assist -#define MSG_IPMA_DATA 0x3D8 // TX by OP, IPMA HUD user interface +#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_Steering_Data_FD1 0x083 // TX by OP, various driver switches and LKAS/CC buttons +#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 +#define MSG_IPMA_Data 0x3D8 // TX by OP, IPMA and LKAS user interface // CAN bus numbers. #define FORD_MAIN_BUS 0U #define FORD_CAM_BUS 2U const CanMsg FORD_TX_MSGS[] = { - {MSG_STEERING_DATA_FD1, 0, 8}, - {MSG_LANE_ASSIST_DATA1, 0, 8}, - {MSG_LATERAL_MOTION_CONTROL, 0, 8}, - {MSG_IPMA_DATA, 0, 8}, + {MSG_Steering_Data_FD1, 0, 8}, + {MSG_Steering_Data_FD1, 2, 8}, + {MSG_ACCDATA_3, 0, 8}, + {MSG_Lane_Assist_Data1, 0, 8}, + {MSG_LateralMotionControl, 0, 8}, + {MSG_IPMA_Data, 0, 8}, }; #define FORD_TX_LEN (sizeof(FORD_TX_MSGS) / sizeof(FORD_TX_MSGS[0])) AddrCheckStruct ford_addr_checks[] = { - {.msg = {{MSG_ENG_BRAKE_DATA, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, - {.msg = {{MSG_ENG_VEHICLE_SP_THROTTLE, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, - {.msg = {{MSG_DESIRED_TORQ_BRK, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.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 }}}, }; #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 bool ford_lkas_msg_check(int addr) { - return (addr == MSG_LANE_ASSIST_DATA1) - || (addr == MSG_LATERAL_MOTION_CONTROL) - || (addr == MSG_IPMA_DATA); + return (addr == MSG_ACCDATA_3) + || (addr == MSG_Lane_Assist_Data1) + || (addr == MSG_LateralMotionControl) + || (addr == MSG_IPMA_Data); } static int ford_rx_hook(CANPacket_t *to_push) { @@ -41,20 +45,20 @@ static int ford_rx_hook(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); // Update in motion state from standstill signal - if (addr == MSG_DESIRED_TORQ_BRK) { + if (addr == MSG_DesiredTorqBrk) { // Signal: VehStop_D_Stat vehicle_moving = ((GET_BYTE(to_push, 3) >> 3) & 0x3U) == 0U; } // Update gas pedal - if (addr == MSG_ENG_VEHICLE_SP_THROTTLE) { + if (addr == MSG_EngVehicleSpThrottle) { // Pedal position: (0.1 * val) in percent // Signal: ApedPos_Pc_ActlArb gas_pressed = (((GET_BYTE(to_push, 0) & 0x03U) << 8) | GET_BYTE(to_push, 1)) > 0U; } // Update brake pedal and cruise state - if (addr == MSG_ENG_BRAKE_DATA) { + if (addr == MSG_EngBrakeData) { // Signal: BpedDrvAppl_D_Actl brake_pressed = ((GET_BYTE(to_push, 0) >> 4) & 0x3U) == 2U; @@ -82,23 +86,23 @@ static int ford_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { tx = 0; } - // Cruise button check, only allow cancel button to be sent - if (addr == MSG_STEERING_DATA_FD1) { - // Violation if any button other than cancel is pressed - // Signal: CcAslButtnCnclPress - bool violation = (GET_BYTE(to_send, 0) | - (GET_BYTE(to_send, 1) & 0xFEU) | - GET_BYTE(to_send, 2) | - GET_BYTE(to_send, 3) | - GET_BYTE(to_send, 4) | - GET_BYTE(to_send, 5)) != 0U; + // Safety check for Steering_Data_FD1 button signals + // Note: Many other signals in this message are not relevant to safety (e.g. blinkers, wiper switches, high beam) + // which we passthru in OP. + if (addr == MSG_Steering_Data_FD1) { + // Violation if resume button is pressed while controls not allowed, or + // if cancel button is pressed when cruise isn't engaged. + bool violation = false; + violation |= (GET_BIT(to_send, 8U) == 1U) && !cruise_engaged_prev; // Signal: CcAslButtnCnclPress (cancel) + violation |= (GET_BIT(to_send, 25U) == 1U) && !controls_allowed; // Signal: CcAsllButtnResPress (resume) + if (violation) { tx = 0; } } // Safety check for Lane_Assist_Data1 action - if (addr == MSG_LANE_ASSIST_DATA1) { + if (addr == MSG_Lane_Assist_Data1) { // Do not allow steering using Lane_Assist_Data1 (Lane-Departure Aid). // This message must be sent for Lane Centering to work, and can include // values such as the steering angle or lane curvature for debugging, @@ -110,7 +114,7 @@ static int ford_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { } // Safety check for LateralMotionControl action - if (addr == MSG_LATERAL_MOTION_CONTROL) { + if (addr == MSG_LateralMotionControl) { // Signal: LatCtl_D_Rq unsigned int steer_control_type = (GET_BYTE(to_send, 4) >> 2) & 0x7U; bool steer_control_enabled = steer_control_type != 0U; diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 5460a53f0..b703f4721 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -16,9 +16,30 @@ const int GM_MAX_RATE_DOWN = 17; const int GM_DRIVER_TORQUE_ALLOWANCE = 50; const int GM_DRIVER_TORQUE_FACTOR = 4; -const int GM_MAX_GAS = 3072; -const int GM_MAX_REGEN = 1404; -const int GM_MAX_BRAKE = 350; +typedef struct { + const int max_gas; + const int max_regen; + const int inactive_regen; + const int max_brake; +} GmLongLimits; + +const GmLongLimits GM_ASCM_LONG_LIMITS = { + .max_gas = 3072, + .max_regen = 1404, + .inactive_regen = 1404, + .max_brake = 400, +}; + +const GmLongLimits GM_CAM_LONG_LIMITS = { + .max_gas = 3400, + .max_regen = 1514, + .inactive_regen = 1554, + .max_brake = 400, +}; + +const GmLongLimits *gm_long_limits; + +const int GM_STANDSTILL_THRSLD = 10; // 0.311kph const CanMsg GM_ASCM_TX_MSGS[] = {{384, 0, 4}, {1033, 0, 7}, {1034, 0, 7}, {715, 0, 8}, {880, 0, 6}, // pt bus {161, 1, 7}, {774, 1, 8}, {776, 1, 7}, {784, 1, 2}, // obs bus @@ -26,20 +47,27 @@ const CanMsg GM_ASCM_TX_MSGS[] = {{384, 0, 4}, {1033, 0, 7}, {1034, 0, 7}, {715, {0x104c006c, 3, 3}, {0x10400060, 3, 5}}; // gmlan const CanMsg GM_CAM_TX_MSGS[] = {{384, 0, 4}, // pt bus - {481, 2, 7}}; // camera bus + {481, 2, 7}, {388, 2, 8}}; // camera bus + +const CanMsg GM_CAM_LONG_TX_MSGS[] = {{384, 0, 4}, {789, 0, 5}, {715, 0, 8}, {880, 0, 6}, // pt bus + {388, 2, 8}}; // camera bus // TODO: do checksum and counter checks. Add correct timestep, 0.1s for now. AddrCheckStruct gm_addr_checks[] = { {.msg = {{388, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, {.msg = {{842, 0, 5, .expected_timestep = 100000U}, { 0 }, { 0 }}}, {.msg = {{481, 0, 7, .expected_timestep = 100000U}, { 0 }, { 0 }}}, - {.msg = {{241, 0, 6, .expected_timestep = 100000U}, { 0 }, { 0 }}}, + {.msg = {{190, 0, 6, .expected_timestep = 100000U}, // Volt, Silverado, Acadia Denali + {190, 0, 7, .expected_timestep = 100000U}, // Bolt EUV + {190, 0, 8, .expected_timestep = 100000U}}}, // Escalade {.msg = {{452, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, + {.msg = {{201, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, }; #define GM_RX_CHECK_LEN (sizeof(gm_addr_checks) / sizeof(gm_addr_checks[0])) addr_checks gm_rx_checks = {gm_addr_checks, GM_RX_CHECK_LEN}; const uint16_t GM_PARAM_HW_CAM = 1; +const uint16_t GM_PARAM_HW_CAM_LONG = 2; enum { GM_BTN_UNPRESS = 1, @@ -49,6 +77,8 @@ enum { }; enum {GM_ASCM, GM_CAM} gm_hw = GM_ASCM; +bool gm_cam_long = false; +bool gm_pcm_cruise = false; static int gm_rx_hook(CANPacket_t *to_push) { @@ -64,60 +94,60 @@ static int gm_rx_hook(CANPacket_t *to_push) { update_sample(&torque_driver, torque_driver_new); } - // sample speed, really only care if car is moving or not - // rear left wheel speed + // sample rear wheel speeds if (addr == 842) { - vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); + int left_rear_speed = (GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1); + int right_rear_speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3); + vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD); } // ACC steering wheel buttons (GM_CAM is tied to the PCM) - if ((addr == 481) && (gm_hw == GM_ASCM)) { + if ((addr == 481) && !gm_pcm_cruise) { int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4; + // enter controls on falling edge of set or rising edge of resume (avoids fault) + bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET); + bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME); + if (set || res) { + controls_allowed = 1; + } + // exit controls on cancel press if (button == GM_BTN_CANCEL) { controls_allowed = 0; } - // enter controls on falling edge of set or resume - bool set = (button == GM_BTN_UNPRESS) && (cruise_button_prev == GM_BTN_SET); - bool res = (button == GM_BTN_UNPRESS) && (cruise_button_prev == GM_BTN_RESUME); - if (set || res) { - controls_allowed = 1; - } - cruise_button_prev = button; } - // speed > 0 - if (addr == 241) { - // Brake pedal's potentiometer returns near-zero reading - // even when pedal is not pressed - brake_pressed = GET_BYTE(to_push, 1) >= 10U; + // Reference for brake pressed signals: + // https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py + if ((addr == 190) && (gm_hw == GM_ASCM)) { + brake_pressed = GET_BYTE(to_push, 1) >= 8U; + } + + if ((addr == 201) && (gm_hw == GM_CAM)) { + brake_pressed = GET_BIT(to_push, 40U) != 0U; } if (addr == 452) { gas_pressed = GET_BYTE(to_push, 5) != 0U; // enter controls on rising edge of ACC, exit controls when ACC off - if (gm_hw == GM_CAM) { + if (gm_pcm_cruise) { bool cruise_engaged = (GET_BYTE(to_push, 1) >> 5) != 0U; pcm_cruise_check(cruise_engaged); } } - // exit controls on regen paddle if (addr == 189) { - bool regen = GET_BYTE(to_push, 0) & 0x20U; - if (regen) { - controls_allowed = 0; - } + regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U; } bool stock_ecu_detected = (addr == 384); // ASCMLKASteeringCmd - // Only check ASCMGasRegenCmd if ASCM, GM_CAM uses stock longitudinal - if ((gm_hw == GM_ASCM) && (addr == 715)) { + // Check ASCMGasRegenCmd only if we're blocking it + if (!gm_pcm_cruise && (addr == 715)) { stock_ecu_detected = true; } generic_rx_checks(stock_ecu_detected); @@ -137,30 +167,25 @@ static int gm_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { int addr = GET_ADDR(to_send); if (gm_hw == GM_CAM) { - tx = msg_allowed(to_send, GM_CAM_TX_MSGS, sizeof(GM_CAM_TX_MSGS)/sizeof(GM_CAM_TX_MSGS[0])); + if (gm_cam_long) { + tx = msg_allowed(to_send, GM_CAM_LONG_TX_MSGS, sizeof(GM_CAM_LONG_TX_MSGS)/sizeof(GM_CAM_LONG_TX_MSGS[0])); + } else { + tx = msg_allowed(to_send, GM_CAM_TX_MSGS, sizeof(GM_CAM_TX_MSGS)/sizeof(GM_CAM_TX_MSGS[0])); + } } else { tx = msg_allowed(to_send, GM_ASCM_TX_MSGS, sizeof(GM_ASCM_TX_MSGS)/sizeof(GM_ASCM_TX_MSGS[0])); } - // disallow actuator commands if gas or brake (with vehicle moving) are pressed - // and the the latching controls_allowed flag is True - int pedal_pressed = brake_pressed_prev && vehicle_moving; - bool alt_exp_allow_gas = alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS; - if (!alt_exp_allow_gas) { - pedal_pressed = pedal_pressed || gas_pressed_prev; - } - bool current_controls_allowed = controls_allowed && !pedal_pressed; - // BRAKE: safety check if (addr == 789) { int brake = ((GET_BYTE(to_send, 0) & 0xFU) << 8) + GET_BYTE(to_send, 1); brake = (0x1000 - brake) & 0xFFF; - if (!current_controls_allowed || !longitudinal_allowed) { + if (!longitudinal_allowed) { if (brake != 0) { tx = 0; } } - if (brake > GM_MAX_BRAKE) { + if (brake > gm_long_limits->max_brake) { tx = 0; } } @@ -172,7 +197,7 @@ static int gm_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { bool violation = 0; desired_torque = to_signed(desired_torque, 11); - if (current_controls_allowed) { + if (controls_allowed) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, GM_MAX_STEER, -GM_MAX_STEER); @@ -189,23 +214,23 @@ static int gm_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { violation |= rt_rate_limit_check(desired_torque, rt_torque_last, GM_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last); if (ts_elapsed > GM_RT_INTERVAL) { rt_torque_last = desired_torque; - ts_last = ts; + ts_torque_check_last = ts; } } // no torque if controls is not allowed - if (!current_controls_allowed && (desired_torque != 0)) { + if (!controls_allowed && (desired_torque != 0)) { violation = 1; } // reset to 0 if either controls is not allowed or there's a violation - if (violation || !current_controls_allowed) { + if (violation || !controls_allowed) { desired_torque_last = 0; rt_torque_last = 0; - ts_last = ts; + ts_torque_check_last = ts; } if (violation) { @@ -217,10 +242,9 @@ static int gm_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { if (addr == 715) { int gas_regen = ((GET_BYTE(to_send, 2) & 0x7FU) << 5) + ((GET_BYTE(to_send, 3) & 0xF8U) >> 3); // Disabled message is !engaged with gas - // value that corresponds to max regen. - if (!current_controls_allowed || !longitudinal_allowed) { - // Stock ECU sends max regen when not enabled - if (gas_regen != GM_MAX_REGEN) { + // value that corresponds to inactive regen. + if (!longitudinal_allowed) { + if (gas_regen != gm_long_limits->inactive_regen) { tx = 0; } } @@ -231,13 +255,14 @@ static int gm_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { tx = 0; } } - if (gas_regen > GM_MAX_GAS) { + // Enforce gas/regen actuation limits (max_regen <= gas_regen <= max_gas) + if ((gas_regen < gm_long_limits->max_regen) || (gas_regen > gm_long_limits->max_gas)) { tx = 0; } } // BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal - if ((addr == 481) && (gm_hw == GM_CAM)) { + if ((addr == 481) && gm_pcm_cruise) { int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U; bool allowed_cancel = (button == 6) && cruise_engaged_prev; @@ -255,15 +280,21 @@ static int gm_fwd_hook(int bus_num, CANPacket_t *to_fwd) { int bus_fwd = -1; if (gm_hw == GM_CAM) { + int addr = GET_ADDR(to_fwd); if (bus_num == 0) { - bus_fwd = 2; + // block PSCMStatus; forwarded through openpilot to hide an alert from the camera + bool is_pscm_msg = (addr == 388); + if (!is_pscm_msg) { + bus_fwd = 2; + } } if (bus_num == 2) { - // block lkas message, forward all others - int addr = GET_ADDR(to_fwd); + // block lkas message and acc messages if gm_cam_long, forward all others bool is_lkas_msg = (addr == 384); - if (!is_lkas_msg) { + bool is_acc_msg = (addr == 789) || (addr == 715) || (addr == 880); + int block_msg = is_lkas_msg || (is_acc_msg && gm_cam_long); + if (!block_msg) { bus_fwd = 0; } } @@ -274,6 +305,18 @@ static int gm_fwd_hook(int bus_num, CANPacket_t *to_fwd) { static const addr_checks* gm_init(uint16_t param) { gm_hw = GET_FLAG(param, GM_PARAM_HW_CAM) ? GM_CAM : GM_ASCM; + + if (gm_hw == GM_ASCM) { + gm_long_limits = &GM_ASCM_LONG_LIMITS; + } else if (gm_hw == GM_CAM) { + gm_long_limits = &GM_CAM_LONG_LIMITS; + } else { + } + +#ifdef ALLOW_DEBUG + gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG); +#endif + gm_pcm_cruise = (gm_hw == GM_CAM) && !gm_cam_long; return &gm_rx_checks; } diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index 1d20cfe80..d1f4daa86 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -263,14 +263,6 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { tx = msg_allowed(to_send, HONDA_N_TX_MSGS, sizeof(HONDA_N_TX_MSGS)/sizeof(HONDA_N_TX_MSGS[0])); } - // disallow actuator commands if gas or brake (with vehicle moving) are pressed - // and the latching controls_allowed flag is True - int pedal_pressed = brake_pressed_prev && vehicle_moving; - bool alt_exp_allow_gas = alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS; - if (!alt_exp_allow_gas) { - pedal_pressed = pedal_pressed || gas_pressed_prev; - } - bool current_controls_allowed = controls_allowed && !(pedal_pressed); int bus_pt = honda_get_pt_bus(); int bus_buttons = (honda_bosch_radarless) ? 2 : bus_pt; // the camera controls ACC on radarless Bosch cars @@ -278,7 +270,7 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { if ((addr == 0x30C) && (bus == bus_pt)) { int pcm_speed = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1); int pcm_gas = GET_BYTE(to_send, 2); - if (!current_controls_allowed || !longitudinal_allowed) { + if (!longitudinal_allowed) { if ((pcm_speed != 0) || (pcm_gas != 0)) { tx = 0; } @@ -288,7 +280,7 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { // BRAKE: safety check (nidec) if ((addr == 0x1FA) && (bus == bus_pt)) { honda_brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3U); - if (!current_controls_allowed || !longitudinal_allowed) { + if (!longitudinal_allowed) { if (honda_brake != 0) { tx = 0; } @@ -305,7 +297,7 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { if ((addr == 0x1DF) && (bus == bus_pt)) { int accel = (GET_BYTE(to_send, 3) << 3) | ((GET_BYTE(to_send, 4) >> 5) & 0x7U); accel = to_signed(accel, 11); - if (!current_controls_allowed || !longitudinal_allowed) { + if (!longitudinal_allowed) { if (accel != 0) { tx = 0; } @@ -316,7 +308,7 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { int gas = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1); gas = to_signed(gas, 16); - if (!current_controls_allowed || !longitudinal_allowed) { + if (!longitudinal_allowed) { if (gas != HONDA_BOSCH_NO_GAS_VALUE) { tx = 0; } @@ -328,7 +320,7 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { // STEER: safety check if ((addr == 0xE4) || (addr == 0x194)) { - if (!current_controls_allowed) { + if (!controls_allowed) { bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1); if (steer_applied) { tx = 0; @@ -345,7 +337,7 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { // GAS: safety check (interceptor) if (addr == 0x200) { - if (!current_controls_allowed || !longitudinal_allowed) { + if (!longitudinal_allowed) { if (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)) { tx = 0; } @@ -355,7 +347,7 @@ static int honda_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW // ensuring that only the cancel button press is sent (VAL 2) when controls are off. // This avoids unintended engagements while still allowing resume spam - if ((addr == 0x296) && !current_controls_allowed && (bus == bus_buttons)) { + if ((addr == 0x296) && !controls_allowed && (bus == bus_buttons)) { if (((GET_BYTE(to_send, 0) >> 5) & 0x7U) != 2U) { tx = 0; } diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 0fed48a80..0620e9197 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -1,3 +1,5 @@ +#include "safety_hyundai_common.h" + const SteeringLimits HYUNDAI_STEERING_LIMITS = { .max_steer = 384, .max_rt_delta = 112, @@ -7,9 +9,14 @@ const SteeringLimits HYUNDAI_STEERING_LIMITS = { .driver_torque_allowance = 50, .driver_torque_factor = 2, .type = TorqueDriverLimited, -}; -const int HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph + // the EPS faults when the steering angle is above a certain threshold for too long. to prevent this, + // we allow setting CF_Lkas_ActToi bit to 0 while maintaining the requested torque value for two consecutive frames + .min_valid_request_frames = 89, + .max_invalid_request_frames = 2, + .min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames + .has_steer_req_tolerance = true, +}; const int HYUNDAI_MAX_ACCEL = 200; // 1/100 m/s2 const int HYUNDAI_MIN_ACCEL = -350; // 1/100 m/s2 @@ -45,11 +52,19 @@ AddrCheckStruct hyundai_addr_checks[] = { {881, 0, 8, .expected_timestep = 10000U}, { 0 }}}, {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, - {.msg = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, - {1057, 2, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }}}, + {.msg = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, }; #define HYUNDAI_ADDR_CHECK_LEN (sizeof(hyundai_addr_checks) / sizeof(hyundai_addr_checks[0])) +AddrCheckStruct hyundai_cam_scc_addr_checks[] = { + {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, + {881, 0, 8, .expected_timestep = 10000U}, { 0 }}}, + {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{1057, 2, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, +}; +#define HYUNDAI_CAM_SCC_ADDR_CHECK_LEN (sizeof(hyundai_cam_scc_addr_checks) / sizeof(hyundai_cam_scc_addr_checks[0])) + AddrCheckStruct hyundai_long_addr_checks[] = { {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, {881, 0, 8, .expected_timestep = 10000U}, { 0 }}}, @@ -69,28 +84,7 @@ AddrCheckStruct hyundai_legacy_addr_checks[] = { }; #define HYUNDAI_LEGACY_ADDR_CHECK_LEN (sizeof(hyundai_legacy_addr_checks) / sizeof(hyundai_legacy_addr_checks[0])) -const int HYUNDAI_PARAM_EV_GAS = 1; -const int HYUNDAI_PARAM_HYBRID_GAS = 2; -const int HYUNDAI_PARAM_LONGITUDINAL = 4; -const int HYUNDAI_PARAM_CAMERA_SCC = 8; - -enum { - HYUNDAI_BTN_NONE = 0, - HYUNDAI_BTN_RESUME = 1, - HYUNDAI_BTN_SET = 2, - HYUNDAI_BTN_CANCEL = 4, -}; - -// some newer HKG models can re-enable after spamming cancel button, -// so keep track of user button presses to deny engagement if no interaction -const uint8_t HYUNDAI_PREV_BUTTON_SAMPLES = 8; // roughly 160 ms -uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button - bool hyundai_legacy = false; -bool hyundai_ev_gas_signal = false; -bool hyundai_hybrid_gas_signal = false; -bool hyundai_camera_scc = false; -bool hyundai_longitudinal = false; addr_checks hyundai_rx_checks = {hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN}; @@ -178,20 +172,10 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); // SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others - if (valid && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) { - // enter controls on rising edge of ACC and user button press, exit controls when ACC off - if (!hyundai_longitudinal && (addr == 1057)) { - // 2 bits: 13-14 - int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3U; - if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) { - controls_allowed = 1; - } - - if (!cruise_engaged) { - controls_allowed = 0; - } - cruise_engaged_prev = cruise_engaged; - } + 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; + hyundai_common_cruise_state_check(cruise_engaged); } if (valid && (bus == 0)) { @@ -205,43 +189,22 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { if (addr == 1265) { int cruise_button = GET_BYTE(to_push, 0) & 0x7U; int main_button = GET_BIT(to_push, 3U); - - if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || (main_button != 0)) { - hyundai_last_button_interaction = 0U; - } else { - hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES); - } - - if (hyundai_longitudinal) { - // exit controls on cancel press - if (cruise_button == HYUNDAI_BTN_CANCEL) { - controls_allowed = 0; - } - - // enter controls on falling edge of resume or set - bool set = (cruise_button == HYUNDAI_BTN_NONE) && (cruise_button_prev == HYUNDAI_BTN_SET); - bool res = (cruise_button == HYUNDAI_BTN_NONE) && (cruise_button_prev == HYUNDAI_BTN_RESUME); - if (set || res) { - controls_allowed = 1; - } - - cruise_button_prev = cruise_button; - } + hyundai_common_cruise_buttons_check(cruise_button, main_button); } - // read gas pressed signal + // gas press, different for EV, hybrid, and ICE models if ((addr == 881) && hyundai_ev_gas_signal) { gas_pressed = (((GET_BYTE(to_push, 4) & 0x7FU) << 1) | GET_BYTE(to_push, 3) >> 7) != 0U; } else if ((addr == 881) && hyundai_hybrid_gas_signal) { gas_pressed = GET_BYTE(to_push, 7) != 0U; - } else if (addr == 608) { // ICE + } else if ((addr == 608) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) { gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0U; } else { } // sample wheel speed, averaging opposite corners if (addr == 902) { - int hyundai_speed = (GET_BYTES_04(to_push) & 0x3FFFU) + ((GET_BYTES_48(to_push) >> 16) & 0x3FFFU); // FL + RR + uint32_t hyundai_speed = (GET_BYTES_04(to_push) & 0x3FFFU) + ((GET_BYTES_48(to_push) >> 16) & 0x3FFFU); // FL + RR hyundai_speed /= 2; vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD; } @@ -360,19 +323,17 @@ static int hyundai_fwd_hook(int bus_num, CANPacket_t *to_fwd) { } static const addr_checks* hyundai_init(uint16_t param) { + hyundai_common_init(param); hyundai_legacy = false; - hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS); - hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS); - hyundai_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC); - hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; -#ifdef ALLOW_DEBUG - // TODO: add longitudinal support for camera-based SCC platform - hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL) && !hyundai_camera_scc; -#endif + if (hyundai_camera_scc) { + hyundai_longitudinal = false; + } if (hyundai_longitudinal) { hyundai_rx_checks = (addr_checks){hyundai_long_addr_checks, HYUNDAI_LONG_ADDR_CHECK_LEN}; + } else if (hyundai_camera_scc) { + hyundai_rx_checks = (addr_checks){hyundai_cam_scc_addr_checks, HYUNDAI_CAM_SCC_ADDR_CHECK_LEN}; } else { hyundai_rx_checks = (addr_checks){hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN}; } @@ -380,12 +341,11 @@ static const addr_checks* hyundai_init(uint16_t param) { } static const addr_checks* hyundai_legacy_init(uint16_t param) { + hyundai_common_init(param); hyundai_legacy = true; hyundai_longitudinal = false; hyundai_camera_scc = false; - hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS); - hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS); - hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; + hyundai_rx_checks = (addr_checks){hyundai_legacy_addr_checks, HYUNDAI_LEGACY_ADDR_CHECK_LEN}; return &hyundai_rx_checks; } diff --git a/panda/board/safety/safety_hyundai_canfd.h b/panda/board/safety/safety_hyundai_canfd.h index 4e03dafe2..5086a39bc 100644 --- a/panda/board/safety/safety_hyundai_canfd.h +++ b/panda/board/safety/safety_hyundai_canfd.h @@ -1,15 +1,22 @@ +#include "safety_hyundai_common.h" + const SteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { .max_steer = 270, .max_rt_delta = 112, .max_rt_interval = 250000, - .max_rate_up = 3, - .max_rate_down = 7, + .max_rate_up = 2, + .max_rate_down = 3, .driver_torque_allowance = 250, .driver_torque_factor = 2, .type = TorqueDriverLimited, -}; -const uint32_t HYUNDAI_CANFD_STANDSTILL_THRSLD = 30; // ~1kph + // the EPS faults when the steering angle is above a certain threshold for too long. to prevent this, + // we allow setting torque actuation bit to 0 while maintaining the requested torque value for two consecutive frames + .min_valid_request_frames = 89, + .max_invalid_request_frames = 2, + .min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames + .has_steer_req_tolerance = true, +}; const CanMsg HYUNDAI_CANFD_HDA2_TX_MSGS[] = { {0x50, 0, 16}, // LKAS @@ -17,38 +24,80 @@ const CanMsg HYUNDAI_CANFD_HDA2_TX_MSGS[] = { {0x2A4, 0, 24}, // CAM_0x2A4 }; +const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = { + {0x50, 0, 16}, // LKAS + {0x1CF, 1, 8}, // CRUISE_BUTTON + {0x2A4, 0, 24}, // CAM_0x2A4 + {0x51, 0, 32}, // ADRV_0x51 + {0x730, 1, 8}, // tester present for ADAS ECU disable + {0x12A, 1, 16}, // LFA + {0x160, 1, 16}, // ADRV_0x160 + {0x1E0, 1, 16}, // LFAHDA_CLUSTER + {0x1A0, 1, 32}, // CRUISE_INFO + {0x1EA, 1, 32}, // ADRV_0x1ea + {0x200, 1, 8}, // ADRV_0x200 + {0x345, 1, 8}, // ADRV_0x345 + {0x1DA, 1, 32}, // ADRV_0x1da +}; + const CanMsg HYUNDAI_CANFD_HDA1_TX_MSGS[] = { {0x12A, 0, 16}, // LFA {0x1A0, 0, 32}, // CRUISE_INFO - {0x1CF, 0, 8}, // CRUISE_BUTTON + {0x1CF, 2, 8}, // CRUISE_BUTTON {0x1E0, 0, 16}, // LFAHDA_CLUSTER }; AddrCheckStruct hyundai_canfd_addr_checks[] = { {.msg = {{0x35, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, - {0x105, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}}, - {.msg = {{0x65, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, - {0x65, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}}, + {0x35, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, + {0x105, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}}}, + {.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, + {0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }}}, {.msg = {{0xa0, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, {0xa0, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}}, {.msg = {{0xea, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, {0xea, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}}, - {.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, - {0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}}, + {.msg = {{0x1a0, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, + {0x1a0, 2, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }}}, {.msg = {{0x1cf, 1, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U}, {0x1cf, 0, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U}, {0x1aa, 0, 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}}}, }; #define HYUNDAI_CANFD_ADDR_CHECK_LEN (sizeof(hyundai_canfd_addr_checks) / sizeof(hyundai_canfd_addr_checks[0])) +AddrCheckStruct hyundai_canfd_long_addr_checks[] = { + {.msg = {{0x35, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, + {0x35, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, + {0x105, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}}}, + {.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, + {0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }}}, + {.msg = {{0xa0, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, + {0xa0, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}}, + {.msg = {{0xea, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, + {0xea, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}}, + {.msg = {{0x1cf, 1, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U}, + {0x1cf, 0, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U}, + {0x1aa, 0, 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}}}, +}; +#define HYUNDAI_CANFD_LONG_ADDR_CHECK_LEN (sizeof(hyundai_canfd_long_addr_checks) / sizeof(hyundai_canfd_long_addr_checks[0])) + +AddrCheckStruct hyundai_canfd_ice_addr_checks[] = { + {.msg = {{0x100, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0xa0, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0xea, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{0x1aa, 0, 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}}, +}; +#define HYUNDAI_CANFD_ICE_ADDR_CHECK_LEN (sizeof(hyundai_canfd_ice_addr_checks) / sizeof(hyundai_canfd_ice_addr_checks[0])) + addr_checks hyundai_canfd_rx_checks = {hyundai_canfd_addr_checks, HYUNDAI_CANFD_ADDR_CHECK_LEN}; uint16_t hyundai_canfd_crc_lut[256]; -const int HYUNDAI_PARAM_CANFD_HDA2 = 1; -const int HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 2; +const int HYUNDAI_PARAM_CANFD_HDA2 = 16; +const int HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32; bool hyundai_canfd_hda2 = false; bool hyundai_canfd_alt_buttons = false; @@ -106,6 +155,7 @@ static int hyundai_canfd_rx_hook(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); const int pt_bus = hyundai_canfd_hda2 ? 1 : 0; + const int scc_bus = hyundai_camera_scc ? 2 : pt_bus; if (valid && (bus == pt_bus)) { // driver torque @@ -127,38 +177,22 @@ static int hyundai_canfd_rx_hook(CANPacket_t *to_push) { cruise_button = (GET_BYTE(to_push, 4) >> 4) & 0x7U; main_button = GET_BIT(to_push, 34U); } - - if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || (main_button != 0)) { - hyundai_last_button_interaction = 0U; - } else { - hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES); - } + hyundai_common_cruise_buttons_check(cruise_button, main_button); } - // cruise state - if (addr == 0x175) { - bool cruise_engaged = GET_BIT(to_push, 68U); - if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) { - controls_allowed = 1; - } - - if (!cruise_engaged) { - controls_allowed = 0; - } - cruise_engaged_prev = cruise_engaged; - } - - // gas press - if ((addr == 0x35) && hyundai_canfd_hda2) { + // gas press, different for EV, hybrid, and ICE models + if ((addr == 0x35) && hyundai_ev_gas_signal) { gas_pressed = GET_BYTE(to_push, 5) != 0U; - } else if ((addr == 0x105) && !hyundai_canfd_hda2) { + } else if ((addr == 0x105) && hyundai_hybrid_gas_signal) { gas_pressed = (GET_BIT(to_push, 103U) != 0U) || (GET_BYTE(to_push, 13) != 0U) || (GET_BIT(to_push, 112U) != 0U); + } else if ((addr == 0x100) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) { + gas_pressed = GET_BIT(to_push, 176U) != 0U; } else { } // brake press - if (addr == 0x65) { - brake_pressed = GET_BIT(to_push, 57U) != 0U; + if (addr == 0x175) { + brake_pressed = GET_BIT(to_push, 81U) != 0U; } // vehicle moving @@ -167,12 +201,27 @@ static int hyundai_canfd_rx_hook(CANPacket_t *to_push) { for (int i = 8; i < 15; i+=2) { speed += GET_BYTE(to_push, i) | (GET_BYTE(to_push, i + 1) << 8U); } - vehicle_moving = (speed / 4U) > HYUNDAI_CANFD_STANDSTILL_THRSLD; + vehicle_moving = (speed / 4U) > HYUNDAI_STANDSTILL_THRSLD; + } + } + + if (valid && (bus == scc_bus)) { + // cruise state + if ((addr == 0x1a0) && !hyundai_longitudinal) { + bool cruise_engaged = ((GET_BYTE(to_push, 8) >> 4) & 0x3U) != 0U; + hyundai_common_cruise_state_check(cruise_engaged); } } const int steer_addr = hyundai_canfd_hda2 ? 0x50 : 0x12a; - generic_rx_checks((addr == steer_addr) && (bus == 0)); + bool stock_ecu_detected = (addr == steer_addr) && (bus == 0); + if (hyundai_longitudinal) { + // on HDA2, ensure ADRV ECU is still knocked out + // on others, ensure accel msg is blocked from camera + const int stock_scc_bus = hyundai_canfd_hda2 ? 1 : 0; + stock_ecu_detected = stock_ecu_detected || ((addr == 0x1a0) && (bus == stock_scc_bus)); + } + generic_rx_checks(stock_ecu_detected); return valid; } @@ -182,28 +231,28 @@ static int hyundai_canfd_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed int tx = 0; int addr = GET_ADDR(to_send); - int bus = GET_BUS(to_send); - if (hyundai_canfd_hda2) { + if (hyundai_canfd_hda2 && !hyundai_longitudinal) { tx = msg_allowed(to_send, HYUNDAI_CANFD_HDA2_TX_MSGS, sizeof(HYUNDAI_CANFD_HDA2_TX_MSGS)/sizeof(HYUNDAI_CANFD_HDA2_TX_MSGS[0])); + } else if (hyundai_canfd_hda2 && hyundai_longitudinal) { + tx = msg_allowed(to_send, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS, sizeof(HYUNDAI_CANFD_HDA2_LONG_TX_MSGS)/sizeof(HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[0])); } else { tx = msg_allowed(to_send, HYUNDAI_CANFD_HDA1_TX_MSGS, sizeof(HYUNDAI_CANFD_HDA1_TX_MSGS)/sizeof(HYUNDAI_CANFD_HDA1_TX_MSGS[0])); } // steering - const int steer_addr = hyundai_canfd_hda2 ? 0x50 : 0x12a; - if ((addr == steer_addr) && (bus == 0)) { - int desired_torque = ((GET_BYTE(to_send, 6) & 0xFU) << 7U) | (GET_BYTE(to_send, 5) >> 1U); - desired_torque -= 1024; + const int steer_addr = (hyundai_canfd_hda2 && !hyundai_longitudinal) ? 0x50 : 0x12a; + if (addr == steer_addr) { + int desired_torque = (((GET_BYTE(to_send, 6) & 0xFU) << 7U) | (GET_BYTE(to_send, 5) >> 1U)) - 1024U; + bool steer_req = GET_BIT(to_send, 52U) != 0U; - if (steer_torque_cmd_checks(desired_torque, -1, HYUNDAI_CANFD_STEERING_LIMITS)) { + if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_CANFD_STEERING_LIMITS)) { tx = 0; } } // cruise buttons check - const int buttons_bus = hyundai_canfd_hda2 ? 1 : 0; - if ((addr == 0x1cf) && (bus == buttons_bus)) { + if (addr == 0x1cf) { int button = GET_BYTE(to_send, 2) & 0x7U; bool is_cancel = (button == HYUNDAI_BTN_CANCEL); bool is_resume = (button == HYUNDAI_BTN_RESUME); @@ -214,11 +263,44 @@ static int hyundai_canfd_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed } } + // 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)) { + tx = 0; + } + } + + // ACCEL: safety check + if (addr == 0x1a0) { + int desired_accel_raw = (((GET_BYTE(to_send, 17) & 0x7U) << 8) | GET_BYTE(to_send, 16)) - 1023U; + int desired_accel_val = ((GET_BYTE(to_send, 18) << 4) | (GET_BYTE(to_send, 17) >> 4)) - 1023U; + + bool violation = false; + + if (hyundai_longitudinal) { + if (!longitudinal_allowed) { + if ((desired_accel_raw != 0) || (desired_accel_val != 0)) { + violation = true; + } + } + violation |= max_limit_check(desired_accel_raw, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL); + violation |= max_limit_check(desired_accel_val, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL); + } else { + // only used to cancel on here + if ((desired_accel_raw != 0) || (desired_accel_val != 0)) { + violation = true; + } + } + + if (violation) { + tx = 0; + } + } + return tx; } static int hyundai_canfd_fwd_hook(int bus_num, CANPacket_t *to_fwd) { - int bus_fwd = -1; int addr = GET_ADDR(to_fwd); @@ -233,7 +315,10 @@ static int hyundai_canfd_fwd_hook(int bus_num, CANPacket_t *to_fwd) { // HUD icons int is_lfahda_msg = ((addr == 0x1e0) && !hyundai_canfd_hda2); - int block_msg = is_lkas_msg || is_lfa_msg || is_lfahda_msg; + // CRUISE_INFO for non-HDA2, we send our own longitudinal commands + int is_scc_msg = ((addr == 0x1a0) && hyundai_longitudinal && !hyundai_canfd_hda2); + + int block_msg = is_lkas_msg || is_lfa_msg || is_lfahda_msg || is_scc_msg; if (!block_msg) { bus_fwd = 0; } @@ -243,11 +328,27 @@ static int hyundai_canfd_fwd_hook(int bus_num, CANPacket_t *to_fwd) { } static const addr_checks* hyundai_canfd_init(uint16_t param) { + hyundai_common_init(param); + gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut); - hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; hyundai_canfd_hda2 = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2); hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS); + // no long for ICE yet + if (!hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) { + hyundai_longitudinal = false; + } + + if (hyundai_longitudinal) { + hyundai_canfd_rx_checks = (addr_checks){hyundai_canfd_long_addr_checks, HYUNDAI_CANFD_LONG_ADDR_CHECK_LEN}; + } else { + if (!hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) { + hyundai_canfd_rx_checks = (addr_checks){hyundai_canfd_ice_addr_checks, HYUNDAI_CANFD_ICE_ADDR_CHECK_LEN}; + } else { + hyundai_canfd_rx_checks = (addr_checks){hyundai_canfd_addr_checks, HYUNDAI_CANFD_ADDR_CHECK_LEN}; + } + } + return &hyundai_canfd_rx_checks; } diff --git a/panda/board/safety/safety_hyundai_common.h b/panda/board/safety/safety_hyundai_common.h new file mode 100644 index 000000000..f1f3b3412 --- /dev/null +++ b/panda/board/safety/safety_hyundai_common.h @@ -0,0 +1,82 @@ +#ifndef SAFETY_HYUNDAI_COMMON_H +#define SAFETY_HYUNDAI_COMMON_H + +const int HYUNDAI_PARAM_EV_GAS = 1; +const int HYUNDAI_PARAM_HYBRID_GAS = 2; +const int HYUNDAI_PARAM_LONGITUDINAL = 4; +const int HYUNDAI_PARAM_CAMERA_SCC = 8; + +const uint8_t HYUNDAI_PREV_BUTTON_SAMPLES = 8; // roughly 160 ms +const uint32_t HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph + +enum { + HYUNDAI_BTN_NONE = 0, + HYUNDAI_BTN_RESUME = 1, + HYUNDAI_BTN_SET = 2, + HYUNDAI_BTN_CANCEL = 4, +}; + +// common state +bool hyundai_ev_gas_signal = false; +bool hyundai_hybrid_gas_signal = false; +bool hyundai_longitudinal = false; +bool hyundai_camera_scc = false; +uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button + +void hyundai_common_init(uint16_t param) { + hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS); + hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS); + hyundai_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC); + + hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; + +#ifdef ALLOW_DEBUG + hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL); +#else + hyundai_longitudinal = false; +#endif +} + +void hyundai_common_cruise_state_check(const int cruise_engaged) { + // some newer HKG models can re-enable after spamming cancel button, + // so keep track of user button presses to deny engagement if no interaction + + // enter controls on rising edge of ACC and recent user button press, exit controls when ACC off + if (!hyundai_longitudinal) { + if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) { + controls_allowed = 1; + } + + if (!cruise_engaged) { + controls_allowed = 0; + } + cruise_engaged_prev = cruise_engaged; + } +} + +void hyundai_common_cruise_buttons_check(const int cruise_button, const int main_button) { + if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || + (main_button != 0)) { + hyundai_last_button_interaction = 0U; + } else { + hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES); + } + + if (hyundai_longitudinal) { + // enter controls on falling edge of resume or set + bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET); + bool res = (cruise_button != HYUNDAI_BTN_RESUME) && (cruise_button_prev == HYUNDAI_BTN_RESUME); + if (set || res) { + controls_allowed = 1; + } + + // exit controls on cancel press + if (cruise_button == HYUNDAI_BTN_CANCEL) { + controls_allowed = 0; + } + + cruise_button_prev = cruise_button; + } +} + +#endif diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h index b97bd1fb6..189a19af8 100644 --- a/panda/board/safety/safety_nissan.h +++ b/panda/board/safety/safety_nissan.h @@ -62,10 +62,11 @@ static int nissan_rx_hook(CANPacket_t *to_push) { } if (addr == 0x285) { - // Get current speed - // Factor 0.005 - vehicle_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.005 / 3.6; - vehicle_moving = vehicle_speed > 0.; + // Get current speed and standstill + 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; } // X-Trail 0x15c, Leaf 0x239 diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index bbb758c71..a1a98a15f 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -20,8 +20,6 @@ const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = { .type = TorqueDriverLimited, }; -const int SUBARU_STANDSTILL_THRSLD = 20; // about 1kph - const CanMsg SUBARU_TX_MSGS[] = { {0x122, 0, 8}, {0x221, 0, 8}, @@ -104,11 +102,9 @@ static int subaru_rx_hook(CANPacket_t *to_push) { pcm_cruise_check(cruise_engaged); } - // sample wheel speed, averaging opposite corners + // update vehicle moving with any non-zero wheel speed if ((addr == 0x13a) && (bus == alt_bus)) { - int subaru_speed = ((GET_BYTES_04(to_push) >> 12) & 0x1FFFU) + ((GET_BYTES_48(to_push) >> 6) & 0x1FFFU); // FR + RL - subaru_speed /= 2; - vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD; + vehicle_moving = ((GET_BYTES_04(to_push) >> 12) != 0U) || (GET_BYTES_48(to_push) != 0U); } if ((addr == 0x13c) && (bus == alt_bus)) { diff --git a/panda/board/safety/safety_subaru_legacy.h b/panda/board/safety/safety_subaru_legacy.h index cca7a4462..7e58e20fa 100644 --- a/panda/board/safety/safety_subaru_legacy.h +++ b/panda/board/safety/safety_subaru_legacy.h @@ -43,11 +43,9 @@ static int subaru_legacy_rx_hook(CANPacket_t *to_push) { pcm_cruise_check(cruise_engaged); } - // sample wheel speed, averaging opposite corners + // update vehicle moving with any non-zero wheel speed if (addr == 0xD4) { - int subaru_speed = ((GET_BYTES_04(to_push) >> 16) & 0xFFFFU) + (GET_BYTES_48(to_push) & 0xFFFFU); // FR + RL - subaru_speed /= 2; - vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD; + vehicle_moving = ((GET_BYTES_04(to_push) >> 12) != 0U) || (GET_BYTES_48(to_push) != 0U); } if (addr == 0xD1) { diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index e77ad3ab8..510bc10e3 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -6,14 +6,19 @@ const SteeringLimits TOYOTA_STEERING_LIMITS = { .max_rt_delta = 450, // the real time limit is 1800/sec, a 20% buffer .max_rt_interval = 250000, .type = TorqueMotorLimited, + + // the EPS faults when the steering angle rate is above a certain threshold for too long. to prevent this, + // we allow setting STEER_REQUEST bit to 0 while maintaining the requested torque value for a single frame + .min_valid_request_frames = 18, + .max_invalid_request_frames = 1, + .min_valid_request_rt_interval = 170000, // 170ms; a ~10% buffer on cutting every 19 frames + .has_steer_req_tolerance = true, }; // longitudinal limits const int TOYOTA_MAX_ACCEL = 2000; // 2.0 m/s2 const int TOYOTA_MIN_ACCEL = -3500; // -3.5 m/s2 -const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph - // 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 // Threshold calculated from DBC gains: round((((15 + 75.555) / 0.159375) + ((15 + 151.111) / 0.159375)) / 2) = 805 @@ -98,15 +103,10 @@ static int toyota_rx_hook(CANPacket_t *to_push) { } } - // sample speed if (addr == 0xaa) { - int speed = 0; - // sum 4 wheel speeds - for (uint8_t i=0U; i<8U; i+=2U) { - int wheel_speed = (GET_BYTE(to_push, i) << 8U) + GET_BYTE(to_push, (i+1U)); - speed += wheel_speed - 0x1a6f; - } - vehicle_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD; + // 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); + vehicle_moving = !standstill; } // most cars have brake_pressed on 0x226, corolla and rav4 on 0x224 diff --git a/panda/board/safety/safety_volkswagen_common.h b/panda/board/safety/safety_volkswagen_common.h new file mode 100644 index 000000000..ce2bf580f --- /dev/null +++ b/panda/board/safety/safety_volkswagen_common.h @@ -0,0 +1,10 @@ +#ifndef SAFETY_VOLKSWAGEN_COMMON_H +#define SAFETY_VOLKSWAGEN_COMMON_H + +const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL = 1; + +bool volkswagen_longitudinal = false; +bool volkswagen_set_button_prev = false; +bool volkswagen_resume_button_prev = false; + +#endif diff --git a/panda/board/safety/safety_volkswagen_mqb.h b/panda/board/safety/safety_volkswagen_mqb.h index da288e6cb..b0ecf9856 100644 --- a/panda/board/safety/safety_volkswagen_mqb.h +++ b/panda/board/safety/safety_volkswagen_mqb.h @@ -1,4 +1,6 @@ +#include "safety_volkswagen_common.h" +// lateral limits const SteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = { .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) .max_rt_delta = 75, // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75 @@ -10,18 +12,27 @@ const SteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = { .type = TorqueDriverLimited, }; +// longitudinal limits +// acceleration in m/s2 * 1000 to avoid floating point math +const int VOLKSWAGEN_MQB_MAX_ACCEL = 2000; +const int VOLKSWAGEN_MQB_MIN_ACCEL = -3500; + #define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds #define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque #define MSG_ESP_05 0x106 // RX from ABS, for brake switch state #define MSG_TSK_06 0x120 // RX from ECU, for ACC status from drivetrain coordinator #define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input +#define MSG_ACC_06 0x122 // TX by OP, ACC control instructions to the drivetrain coordinator #define MSG_HCA_01 0x126 // TX by OP, Heading Control Assist steering torque #define MSG_GRA_ACC_01 0x12B // TX by OP, ACC control buttons for cancel/resume +#define MSG_ACC_07 0x12E // TX by OP, ACC control instructions to the drivetrain coordinator +#define MSG_ACC_02 0x30C // TX by OP, ACC HUD data to the instrument cluster #define MSG_LDW_02 0x397 // TX by OP, Lane line recognition and text alerts // Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration -const CanMsg VOLKSWAGEN_MQB_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_GRA_ACC_01, 0, 8}, {MSG_GRA_ACC_01, 2, 8}, {MSG_LDW_02, 0, 8}}; -#define VOLKSWAGEN_MQB_TX_MSGS_LEN (sizeof(VOLKSWAGEN_MQB_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_TX_MSGS[0])) +const CanMsg VOLKSWAGEN_MQB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_GRA_ACC_01, 0, 8}, {MSG_GRA_ACC_01, 2, 8}, {MSG_LDW_02, 0, 8}}; +const CanMsg VOLKSWAGEN_MQB_LONG_TX_MSGS[] = {{MSG_HCA_01, 0, 8}, {MSG_LDW_02, 0, 8}, + {MSG_ACC_02, 0, 8}, {MSG_ACC_06, 0, 8}, {MSG_ACC_07, 0, 8}}; AddrCheckStruct volkswagen_mqb_addr_checks[] = { {.msg = {{MSG_ESP_19, 0, 8, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, @@ -35,7 +46,6 @@ addr_checks volkswagen_mqb_rx_checks = {volkswagen_mqb_addr_checks, VOLKSWAGEN_M uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR - static uint32_t volkswagen_mqb_get_checksum(CANPacket_t *to_push) { return (uint8_t)GET_BYTE(to_push, 0); } @@ -83,6 +93,12 @@ static uint32_t volkswagen_mqb_compute_crc(CANPacket_t *to_push) { static const addr_checks* volkswagen_mqb_init(uint16_t param) { UNUSED(param); + volkswagen_set_button_prev = false; + volkswagen_resume_button_prev = false; + +#ifdef ALLOW_DEBUG + volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL); +#endif gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f); return &volkswagen_mqb_rx_checks; } @@ -118,12 +134,41 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) { update_sample(&torque_driver, torque_driver_new); } - // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages - // Signal: TSK_06.TSK_Status if (addr == MSG_TSK_06) { + // When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage + // Always exit controls on main switch off + // Signal: TSK_06.TSK_Status int acc_status = (GET_BYTE(to_push, 3) & 0x7U); bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5); - pcm_cruise_check(cruise_engaged); + acc_main_on = cruise_engaged || (acc_status == 2); + + if (!volkswagen_longitudinal) { + pcm_cruise_check(cruise_engaged); + } + + if (!acc_main_on) { + controls_allowed = false; + } + } + + if (addr == MSG_GRA_ACC_01) { + // If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on + // Signal: GRA_ACC_01.GRA_Tip_Setzen + // Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme + if (volkswagen_longitudinal) { + bool set_button = GET_BIT(to_push, 16U); + bool resume_button = GET_BIT(to_push, 19U); + if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { + controls_allowed = acc_main_on; + } + volkswagen_set_button_prev = set_button; + volkswagen_resume_button_prev = resume_button; + } + // Always exit controls on rising edge of Cancel + // Signal: GRA_ACC_01.GRA_Abbrechen + if (GET_BIT(to_push, 13U) == 1U) { + controls_allowed = false; + } } // Signal: Motor_20.MO_Fahrpedalrohwert_01 @@ -147,8 +192,10 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send, bool longitudinal_allowe int addr = GET_ADDR(to_send); int tx = 1; - if (!msg_allowed(to_send, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN)) { - tx = 0; + if (volkswagen_longitudinal) { + tx = msg_allowed(to_send, VOLKSWAGEN_MQB_LONG_TX_MSGS, sizeof(VOLKSWAGEN_MQB_LONG_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_LONG_TX_MSGS[0])); + } else { + tx = msg_allowed(to_send, VOLKSWAGEN_MQB_STOCK_TX_MSGS, sizeof(VOLKSWAGEN_MQB_STOCK_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_STOCK_TX_MSGS[0])); } // Safety check for HCA_01 Heading Control Assist torque @@ -166,6 +213,38 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send, bool longitudinal_allowe } } + // Safety check for both ACC_06 and ACC_07 acceleration requests + // To avoid floating point math, scale upward and compare to pre-scaled safety m/s2 boundaries + if ((addr == MSG_ACC_06) || (addr == MSG_ACC_07)) { + bool violation = false; + int desired_accel = 0; + + if (addr == MSG_ACC_06) { + // Signal: ACC_06.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22) + desired_accel = ((((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) * 5U) - 7220U; + } else { + // Signal: ACC_07.ACC_Folgebeschl (acceleration in m/s2, scale 0.03, offset -4.6) + int secondary_accel = (GET_BYTE(to_send, 4) * 30U) - 4600U; + violation |= (secondary_accel != 3020); // enforce always inactive (one increment above max range) at this time + // Signal: ACC_07.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22) + desired_accel = (((GET_BYTE(to_send, 7) << 3) | ((GET_BYTE(to_send, 6) & 0xE0U) >> 5)) * 5U) - 7220U; + } + + // VW send one increment above the max range when inactive + if (desired_accel == 3010) { + desired_accel = 0; + } + + if (!controls_allowed && (desired_accel != 0)) { + violation = 1; + } + violation |= max_limit_check(desired_accel, VOLKSWAGEN_MQB_MAX_ACCEL, VOLKSWAGEN_MQB_MIN_ACCEL); + + if (violation) { + tx = 0; + } + } + // FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off. // This avoids unintended engagements while still allowing resume spam if ((addr == MSG_GRA_ACC_01) && !controls_allowed) { @@ -190,7 +269,10 @@ static int volkswagen_mqb_fwd_hook(int bus_num, CANPacket_t *to_fwd) { break; case 2: if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) { - // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera + // openpilot takes over LKAS steering control and related HUD messages from the camera + bus_fwd = -1; + } else if (volkswagen_longitudinal && ((addr == MSG_ACC_02) || (addr == MSG_ACC_06) || (addr == MSG_ACC_07))) { + // openpilot takes over acceleration/braking control and related HUD messages from the stock ACC radar bus_fwd = -1; } else { // Forward all remaining traffic from Extended CAN devices to J533 gateway diff --git a/panda/board/safety/safety_volkswagen_pq.h b/panda/board/safety/safety_volkswagen_pq.h index e3657a125..dce0a3399 100644 --- a/panda/board/safety/safety_volkswagen_pq.h +++ b/panda/board/safety/safety_volkswagen_pq.h @@ -1,3 +1,5 @@ +#include "safety_volkswagen_common.h" + // lateral limits const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = { .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) @@ -11,8 +13,9 @@ const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = { }; // longitudinal limits -const int VOLKSWAGEN_PQ_MAX_ACCEL = 2000; // 2.0 m/s2 -const int VOLKSWAGEN_PQ_MIN_ACCEL = -3500; // -3.5 m/s2 +// acceleration in m/s2 * 1000 to avoid floating point math +const int VOLKSWAGEN_PQ_MAX_ACCEL = 2000; +const int VOLKSWAGEN_PQ_MIN_ACCEL = -3500; #define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque #define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque @@ -42,11 +45,6 @@ AddrCheckStruct volkswagen_pq_addr_checks[] = { #define VOLKSWAGEN_PQ_ADDR_CHECKS_LEN (sizeof(volkswagen_pq_addr_checks) / sizeof(volkswagen_pq_addr_checks[0])) addr_checks volkswagen_pq_rx_checks = {volkswagen_pq_addr_checks, VOLKSWAGEN_PQ_ADDR_CHECKS_LEN}; -const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL = 1; -bool volkswagen_pq_longitudinal = false; -bool volkswagen_pq_set_prev = false; -bool volkswagen_pq_resume_prev = false; - static uint32_t volkswagen_pq_get_checksum(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); @@ -87,8 +85,11 @@ static uint32_t volkswagen_pq_compute_checksum(CANPacket_t *to_push) { static const addr_checks* volkswagen_pq_init(uint16_t param) { UNUSED(param); + volkswagen_set_button_prev = false; + volkswagen_resume_button_prev = false; + #ifdef ALLOW_DEBUG - volkswagen_pq_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL); + volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL); #endif return &volkswagen_pq_rx_checks; } @@ -121,7 +122,7 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) { update_sample(&torque_driver, torque_driver_new); } - if (volkswagen_pq_longitudinal) { + if (volkswagen_longitudinal) { if (addr == MSG_MOTOR_5) { // ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off // Signal: Motor_5.GRA_Hauptschalter @@ -137,11 +138,11 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) { // Signal: GRA_Neu.GRA_Neu_Recall bool set_button = GET_BIT(to_push, 16U); bool resume_button = GET_BIT(to_push, 17U); - if ((volkswagen_pq_set_prev && !set_button) || (volkswagen_pq_resume_prev && !resume_button)) { + if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { controls_allowed = acc_main_on; } - volkswagen_pq_set_prev = set_button; - volkswagen_pq_resume_prev = resume_button; + volkswagen_set_button_prev = set_button; + volkswagen_resume_button_prev = resume_button; // Exit controls on rising edge of Cancel, override Set/Resume if present simultaneously // Signal: GRA_ACC_01.GRA_Abbrechen if (GET_BIT(to_push, 9U) == 1U) { @@ -177,7 +178,7 @@ static int volkswagen_pq_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed int addr = GET_ADDR(to_send); int tx = 1; - if (volkswagen_pq_longitudinal) { + if (volkswagen_longitudinal) { tx = msg_allowed(to_send, VOLKSWAGEN_PQ_LONG_TX_MSGS, sizeof(VOLKSWAGEN_PQ_LONG_TX_MSGS) / sizeof(VOLKSWAGEN_PQ_LONG_TX_MSGS[0])); } else { tx = msg_allowed(to_send, VOLKSWAGEN_PQ_STOCK_TX_MSGS, sizeof(VOLKSWAGEN_PQ_STOCK_TX_MSGS) / sizeof(VOLKSWAGEN_PQ_STOCK_TX_MSGS[0])); @@ -251,7 +252,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_pq_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZIEGE))) { + } else if (volkswagen_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZIEGE))) { // 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 3beeecc6b..d50d4d1fe 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -46,6 +46,12 @@ typedef struct { // motor torque limits const int max_torque_error; + + // safety around steer req bit + const int min_valid_request_frames; + const int max_invalid_request_frames; + const uint32_t min_valid_request_rt_interval; + const bool has_steer_req_tolerance; } SteeringLimits; typedef struct { @@ -134,18 +140,24 @@ bool gas_pressed = false; bool gas_pressed_prev = false; bool brake_pressed = false; bool brake_pressed_prev = false; +bool regen_braking = false; +bool regen_braking_prev = false; bool cruise_engaged_prev = false; float vehicle_speed = 0; bool vehicle_moving = false; bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 int cruise_button_prev = 0; +bool safety_rx_checks_invalid = false; // for safety modes with torque steering control int desired_torque_last = 0; // last desired steer torque int rt_torque_last = 0; // last desired torque for real time check +int valid_steer_req_count = 0; // counter for steer request bit matching non-zero torque +int invalid_steer_req_count = 0; // counter to allow multiple frames of mismatching torque request bit struct sample_t torque_meas; // last 6 motor torques produced by the eps struct sample_t torque_driver; // last 6 driver torques measured -uint32_t ts_last = 0; +uint32_t ts_torque_check_last = 0; +uint32_t ts_steer_req_mismatch_last = 0; // last timestamp steer req was mismatched with torque // state for controls_allowed timeout logic bool heartbeat_engaged = false; // openpilot enabled, passed in heartbeat USB command diff --git a/panda/board/stm32fx/board.h b/panda/board/stm32fx/board.h index 90aa739fd..a9093cb1a 100644 --- a/panda/board/stm32fx/board.h +++ b/panda/board/stm32fx/board.h @@ -52,10 +52,3 @@ void detect_board_type(void) { #endif #endif } - -bool has_external_debug_serial = 0; - -void detect_external_debug_serial(void) { - // detect if external serial debugging is present - has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN); -} diff --git a/panda/board/stm32fx/clock.h b/panda/board/stm32fx/clock.h index 4f7beaae4..f2386328d 100644 --- a/panda/board/stm32fx/clock.h +++ b/panda/board/stm32fx/clock.h @@ -4,7 +4,7 @@ void clock_init(void) { while ((RCC->CR & RCC_CR_HSERDY) == 0); // divide things - register_set(&(RCC->CFGR), RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV4, 0xFF7FFCF3U); + register_set(&(RCC->CFGR), RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV2, 0xFF7FFCF3U); // 16mhz crystal register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE, 0x7F437FFFU); diff --git a/panda/board/stm32fx/llbxcan.h b/panda/board/stm32fx/llbxcan.h index f1d5187cb..a1f0182be 100644 --- a/panda/board/stm32fx/llbxcan.h +++ b/panda/board/stm32fx/llbxcan.h @@ -1,9 +1,10 @@ -// this is needed for 1 mbps support -#define CAN_QUANTA 8U -#define CAN_SEQ1 6 // roundf(quanta * 0.875f) - 1; -#define CAN_SEQ2 1 // roundf(quanta * 0.125f); +// SAE 2284-3 : minimum 16 tq, SJW 3, sample point at 81.3% +#define CAN_QUANTA 16U +#define CAN_SEQ1 12U +#define CAN_SEQ2 3U +#define CAN_SJW 3U -#define CAN_PCLK 24000U +#define CAN_PCLK 48000U // 333 = 33.3 kbps // 5000 = 500 kbps #define can_speed_to_prescaler(x) (CAN_PCLK / CAN_QUANTA * 10U / (x)) @@ -12,6 +13,10 @@ void puts(const char *a); +// kbps multiplied by 10 +const uint32_t speeds[] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U}; +const uint32_t data_speeds[] = {0U}; // No separate data speed, dummy + bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool silent) { bool ret = true; @@ -32,9 +37,10 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s if(ret){ // set time quanta from defines - register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | - (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | - (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU); + register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1U)) | + (CAN_BTR_TS2_0 * (CAN_SEQ2-1U)) | + (CAN_BTR_SJW_0 * (CAN_SJW-1U)) | + (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU); // silent loopback mode for debugging if (loopback) { @@ -97,7 +103,7 @@ bool llcan_init(CAN_TypeDef *CAN_obj) { register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); // enable certain CAN interrupts - register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_WKUIE); + register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_ERRIE | CAN_IER_LECIE | CAN_IER_BOFIE | CAN_IER_EPVIE | CAN_IER_EWGIE | CAN_IER_FOVIE0 | CAN_IER_FFIE0); if (CAN_obj == CAN1) { NVIC_EnableIRQ(CAN1_TX_IRQn); @@ -121,8 +127,6 @@ bool llcan_init(CAN_TypeDef *CAN_obj) { } void llcan_clear_send(CAN_TypeDef *CAN_obj) { - CAN_obj->TSR |= CAN_TSR_ABRQ0; - register_clear_bits(&(CAN_obj->MSR), CAN_MSR_ERRI); - // cppcheck-suppress selfAssignment ; needed to clear the register - CAN_obj->MSR = CAN_obj->MSR; + CAN_obj->TSR |= CAN_TSR_ABRQ0; // Abort message transmission on error interrupt + CAN_obj->MSR |= CAN_MSR_ERRI; // Clear error interrupt } diff --git a/panda/board/stm32fx/llspi.h b/panda/board/stm32fx/llspi.h index d07bce352..020ead505 100644 --- a/panda/board/stm32fx/llspi.h +++ b/panda/board/stm32fx/llspi.h @@ -1,131 +1,80 @@ -// IRQs: DMA2_Stream2, DMA2_Stream3, EXTI4 - -void spi_init(void); -int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out); - -// end API - -#define SPI_BUF_SIZE 256 -uint8_t spi_buf[SPI_BUF_SIZE]; -int spi_buf_count = 0; -int spi_total_count = 0; - -void spi_tx_dma(void *addr, int len) { +void llspi_miso_dma(uint8_t *addr, int len) { // disable DMA + DMA2_Stream3->CR &= ~DMA_SxCR_EN; register_clear_bits(&(SPI1->CR2), SPI_CR2_TXDMAEN); - register_clear_bits(&(DMA2_Stream3->CR), DMA_SxCR_EN); - // DMA2, stream 3, channel 3 + // setup source and length register_set(&(DMA2_Stream3->M0AR), (uint32_t)addr, 0xFFFFFFFFU); DMA2_Stream3->NDTR = len; - register_set(&(DMA2_Stream3->PAR), (uint32_t)&(SPI1->DR), 0xFFFFFFFFU); - - // channel3, increment memory, memory -> periph, enable - register_set(&(DMA2_Stream3->CR), (DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_EN), 0x1E077EFEU); - delay(0); - register_set_bits(&(DMA2_Stream3->CR), DMA_SxCR_TCIE); + // enable DMA register_set_bits(&(SPI1->CR2), SPI_CR2_TXDMAEN); - - // signal data is ready by driving low - // esp must be configured as input by this point - set_gpio_output(GPIOB, 0, 0); + DMA2_Stream3->CR |= DMA_SxCR_EN; } -void spi_rx_dma(void *addr, int len) { +void llspi_mosi_dma(uint8_t *addr, int len) { // disable DMA register_clear_bits(&(SPI1->CR2), SPI_CR2_RXDMAEN); - register_clear_bits(&(DMA2_Stream2->CR), DMA_SxCR_EN); + DMA2_Stream2->CR &= ~DMA_SxCR_EN; // drain the bus volatile uint8_t dat = SPI1->DR; (void)dat; - // DMA2, stream 2, channel 3 + // setup destination and length register_set(&(DMA2_Stream2->M0AR), (uint32_t)addr, 0xFFFFFFFFU); DMA2_Stream2->NDTR = len; - register_set(&(DMA2_Stream2->PAR), (uint32_t)&(SPI1->DR), 0xFFFFFFFFU); - - // channel3, increment memory, periph -> memory, enable - register_set(&(DMA2_Stream2->CR), (DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_EN), 0x1E077EFEU); - delay(0); - register_set_bits(&(DMA2_Stream2->CR), DMA_SxCR_TCIE); + // enable DMA + DMA2_Stream2->CR |= DMA_SxCR_EN; register_set_bits(&(SPI1->CR2), SPI_CR2_RXDMAEN); } -// ***************************** SPI IRQs ***************************** -// can't go on the stack cause it's DMAed -uint8_t spi_tx_buf[0x44]; - -// SPI RX +// SPI MOSI DMA FINISHED void DMA2_Stream2_IRQ_Handler(void) { - int *resp_len = (int*)spi_tx_buf; - (void)memset(spi_tx_buf, 0xaa, 0x44); - *resp_len = spi_cb_rx(spi_buf, 0x14, spi_tx_buf+4); - #ifdef DEBUG_SPI - puts("SPI write: "); - puth(*resp_len); - puts("\n"); - #endif - spi_tx_dma(spi_tx_buf, *resp_len + 4); - - // ack + // Clear interrupt flag + ENTER_CRITICAL(); DMA2->LIFCR = DMA_LIFCR_CTCIF2; + + spi_handle_rx(); + + EXIT_CRITICAL(); } -// SPI TX +// SPI MISO DMA FINISHED void DMA2_Stream3_IRQ_Handler(void) { - #ifdef DEBUG_SPI - puts("SPI handshake\n"); - #endif - - // reset handshake back to pull up - set_gpio_mode(GPIOB, 0, MODE_INPUT); - set_gpio_pullup(GPIOB, 0, PULL_UP); - - // ack + // Clear interrupt flag DMA2->LIFCR = DMA_LIFCR_CTCIF3; -} -void EXTI4_IRQ_Handler(void) { - volatile unsigned int pr = EXTI->PR & (1U << 4); - #ifdef DEBUG_SPI - puts("exti4\n"); - #endif - // SPI CS falling - if ((pr & (1U << 4)) != 0U) { - spi_total_count = 0; - spi_rx_dma(spi_buf, 0x14); - } - EXTI->PR = pr; + // Wait until the transaction is actually finished and clear the DR + // TODO: needs a timeout here, otherwise it gets stuck with no master clock! + while (!(SPI1->SR & SPI_SR_TXE)); + volatile uint8_t dat = SPI1->DR; + (void)dat; + SPI1->DR = 0U; + + spi_handle_tx(); } // ***************************** SPI init ***************************** -void spi_init(void) { - // Max SPI clock the ESP can produce is 80MHz. At buffer size of 256 bytes, that's a max of about 40k buffers per second - REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, 50000U, FAULT_INTERRUPT_RATE_SPI_DMA) - REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, 50000U, FAULT_INTERRUPT_RATE_SPI_DMA) - REGISTER_INTERRUPT(EXTI4_IRQn, EXTI4_IRQ_Handler, 50000U, FAULT_INTERRUPT_RATE_SPI_CS) // TODO: Figure out if this is a reasonable limit +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) - //puts("SPI init\n"); + // Setup MOSI DMA + register_set(&(DMA2_Stream2->CR), (DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_TCIE), 0x1E077EFEU); + register_set(&(DMA2_Stream2->PAR), (uint32_t)&(SPI1->DR), 0xFFFFFFFFU); + + // Setup MISO DMA + register_set(&(DMA2_Stream3->CR), (DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_TCIE), 0x1E077EFEU); + register_set(&(DMA2_Stream3->PAR), (uint32_t)&(SPI1->DR), 0xFFFFFFFFU); + + // Enable SPI and the error interrupts + // TODO: verify clock phase and polarity register_set(&(SPI1->CR1), SPI_CR1_SPE, 0xFFFFU); - - // enable SPI interrupts - //SPI1->CR2 = SPI_CR2_RXNEIE | SPI_CR2_ERRIE | SPI_CR2_TXEIE; - register_set(&(SPI1->CR2), SPI_CR2_RXNEIE, 0xF7U); + register_set(&(SPI1->CR2), 0U, 0xF7U); NVIC_EnableIRQ(DMA2_Stream2_IRQn); NVIC_EnableIRQ(DMA2_Stream3_IRQn); - //NVIC_EnableIRQ(SPI1_IRQn); - - // reset handshake back to pull up - set_gpio_mode(GPIOB, 0, MODE_INPUT); - set_gpio_pullup(GPIOB, 0, PULL_UP); - - // setup interrupt on falling edge of SPI enable (on PA4) - register_set(&(SYSCFG->EXTICR[2]), SYSCFG_EXTICR2_EXTI4_PA, 0xFFFFU); - register_set_bits(&(EXTI->IMR), (1U << 4)); - register_set_bits(&(EXTI->FTSR), (1U << 4)); - NVIC_EnableIRQ(EXTI4_IRQn); } diff --git a/panda/board/stm32fx/stm32fx_flash.ld b/panda/board/stm32fx/stm32f2_flash.ld similarity index 100% rename from panda/board/stm32fx/stm32fx_flash.ld rename to panda/board/stm32fx/stm32f2_flash.ld diff --git a/panda/board/stm32fx/stm32f4_flash.ld b/panda/board/stm32fx/stm32f4_flash.ld new file mode 100644 index 000000000..a601c1f60 --- /dev/null +++ b/panda/board/stm32fx/stm32f4_flash.ld @@ -0,0 +1,166 @@ +/* +***************************************************************************** +** +** File : stm32f4_flash.ld +** +** Abstract : Linker script for STM32F407VG Device with +** 1024KByte FLASH, 192KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Environment : Atollic TrueSTUDIO(R) +** +** Distribution: The file is distributed "as is," without any warranty +** of any kind. +** +** (c)Copyright Atollic AB. +** You may use this file as-is or modify it according to the needs of your +** project. Distribution of this file (unmodified or modified) is not +** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the +** rights to distribute the assembled, compiled & linked contents of this +** file as part of an application binary file, provided that it is built +** using the Atollic TrueSTUDIO(R) toolchain. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +enter_bootloader_mode = 0x2001FFFC; +_estack = 0x2001FFFC; /* end of 128K RAM on AHB bus*/ +_app_start = 0x08004000; /* Reserve Sector 0(16K) for bootloader */ + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0; /* required amount of heap */ +_Min_Stack_Size = 0x400; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1024K + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 256K + RAM2 (xrw) : ORIGIN = 0x20040000, LENGTH = 64K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + _exit = .; + } >FLASH + + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = .; + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : AT ( _sidata ) + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >RAM + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/panda/board/stm32fx/stm32fx_config.h b/panda/board/stm32fx/stm32fx_config.h index cbdff1dae..9361f79ac 100644 --- a/panda/board/stm32fx/stm32fx_config.h +++ b/panda/board/stm32fx/stm32fx_config.h @@ -66,6 +66,11 @@ #include "stm32fx/board.h" #include "stm32fx/clock.h" +#ifdef ENABLE_SPI + #include "drivers/spi.h" + #include "stm32fx/llspi.h" +#endif + #if !defined(BOOTSTUB) && (defined(PANDA) || defined(PEDAL_USB)) #include "drivers/uart.h" #include "stm32fx/lluart.h" diff --git a/panda/board/stm32h7/board.h b/panda/board/stm32h7/board.h index 769e9789e..f8c3ea3c2 100644 --- a/panda/board/stm32h7/board.h +++ b/panda/board/stm32h7/board.h @@ -11,26 +11,32 @@ #include "stm32h7/llrtc.h" #include "drivers/rtc.h" #include "boards/red.h" +#include "boards/red_chiplet.h" +#include "boards/red_v2.h" +#include "boards/tres.h" -uint8_t board_id(void) { + +uint8_t get_board_id(void) { return detect_with_pull(GPIOF, 7, PULL_UP) | - (detect_with_pull(GPIOF, 8, PULL_UP) << 1U) | - (detect_with_pull(GPIOF, 9, PULL_UP) << 2U) | - (detect_with_pull(GPIOF, 10, PULL_UP) << 3U); + (detect_with_pull(GPIOF, 8, PULL_UP) << 1U) | + (detect_with_pull(GPIOF, 9, PULL_UP) << 2U) | + (detect_with_pull(GPIOF, 10, PULL_UP) << 3U); } void detect_board_type(void) { - if(board_id() == 0U){ + const uint8_t board_id = get_board_id(); + + if (board_id == 0U) { hw_type = HW_TYPE_RED_PANDA; current_board = &board_red; + } else if (board_id == 1U) { + hw_type = HW_TYPE_RED_PANDA_V2; + current_board = &board_red_v2; + } else if (board_id == 2U) { + hw_type = HW_TYPE_TRES; + current_board = &board_tres; } else { hw_type = HW_TYPE_UNKNOWN; puts("Hardware type is UNKNOWN!\n"); } } - -bool has_external_debug_serial = 0; -void detect_external_debug_serial(void) { - // detect if external serial debugging is present - has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN); -} diff --git a/panda/board/stm32h7/clock.h b/panda/board/stm32h7/clock.h index 10febe15a..b1846da26 100644 --- a/panda/board/stm32h7/clock.h +++ b/panda/board/stm32h7/clock.h @@ -1,3 +1,22 @@ +/* +HSE: 25MHz +PLL1Q: 80MHz (for FDCAN) +HSI48 enabled (for USB) +CPU: 240MHz +CPU Systick: 240MHz +AXI: 120MHz +HCLK3: 60MHz +APB3 per: 60MHz +AHB1,2 per: 120MHz +APB1 per: 60MHz +APB1 tim: 120MHz +APB2 per: 60MHz +APB2 tim: 120MHz +AHB4 per: 120MHz +APB4 per: 60MHz +PCLK1: 60MHz (for USART2,3,4,5,7,8) +*/ + void clock_init(void) { //Set power mode to direct SMPS power supply(depends on the board layout) register_set(&(PWR->CR3), PWR_CR3_SMPSEN, 0xFU); // powered only by SMPS @@ -42,7 +61,7 @@ void clock_init(void) { register_set_bits(&(RCC->AHB4ENR), RCC_APB4ENR_SYSCFGEN); //////////////END OTHER CLOCKS//////////////////// - // Configure clock source for USB (HSI at 48Mhz) + // Configure clock source for USB (HSI48) register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_1 | RCC_D2CCIP2R_USBSEL_0, RCC_D2CCIP2R_USBSEL); // Configure clock source for FDCAN (PLL1Q at 80Mhz) register_set(&(RCC->D2CCIP1R), RCC_D2CCIP1R_FDCANSEL_0, RCC_D2CCIP1R_FDCANSEL); @@ -52,4 +71,8 @@ void clock_init(void) { register_set_bits(&(RCC->CR), RCC_CR_CSSHSEON); //Enable Vdd33usb supply level detector register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); + + // Enable CPU access to SRAM1 and SRAM2 (in domain D2) + register_set_bits(&(RCC->AHB2ENR), RCC_AHB2ENR_SRAM1EN); + register_set_bits(&(RCC->AHB2ENR), RCC_AHB2ENR_SRAM2EN); } diff --git a/panda/board/stm32h7/llfdcan.h b/panda/board/stm32h7/llfdcan.h index b598cc087..d21f415ce 100644 --- a/panda/board/stm32h7/llfdcan.h +++ b/panda/board/stm32h7/llfdcan.h @@ -39,6 +39,11 @@ void puts(const char *a); +// kbps multiplied by 10 +const uint32_t speeds[] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U}; +const uint32_t data_speeds[] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U, 20000U, 50000U}; + + bool fdcan_request_init(FDCAN_GlobalTypeDef *CANx) { bool ret = true; // Exit from sleep mode @@ -79,7 +84,7 @@ bool fdcan_exit_init(FDCAN_GlobalTypeDef *CANx) { return ret; } -bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_speed, bool loopback, bool silent) { +bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_speed, bool non_iso, bool loopback, bool silent) { UNUSED(speed); bool ret = fdcan_request_init(CANx); @@ -92,6 +97,7 @@ bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_sp CANx->TEST &= ~(FDCAN_TEST_LBCK); CANx->CCCR &= ~(FDCAN_CCCR_MON); CANx->CCCR &= ~(FDCAN_CCCR_ASM); + CANx->CCCR &= ~(FDCAN_CCCR_NISO); // TODO: add as a separate safety mode // Enable ASM restricted operation(for debug or automatic bitrate switching) @@ -108,7 +114,7 @@ bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_sp uint8_t sp = CAN_SP_NOMINAL; uint8_t seg1 = CAN_SEG1(tq, sp); uint8_t seg2 = CAN_SEG2(tq, sp); - uint8_t sjw = seg2; + uint8_t sjw = MIN(127U, seg2); CANx->NBTP = (((sjw & 0x7FU)-1U)<DBTP = (((sjw & 0xFU)-1U)<CCCR |= FDCAN_CCCR_NISO; + } + // Silent loopback is known as internal loopback in the docs if (loopback) { CANx->CCCR |= FDCAN_CCCR_TEST; @@ -200,6 +211,7 @@ bool llcan_init(FDCAN_GlobalTypeDef *CANx) { CANx->IE &= 0x0U; // Reset all interrupts // Messages for INT0 CANx->IE |= FDCAN_IE_RF0NE; // Rx FIFO 0 new message + CANx->IE |= FDCAN_IE_PEDE | FDCAN_IE_PEAE | FDCAN_IE_BOE | FDCAN_IE_EPE | FDCAN_IE_ELOE | FDCAN_IE_TEFLE | FDCAN_IE_RF0LE; // Messages for INT1 (Only TFE works??) CANx->ILS |= FDCAN_ILS_TFEL; @@ -230,6 +242,7 @@ bool llcan_init(FDCAN_GlobalTypeDef *CANx) { } void llcan_clear_send(FDCAN_GlobalTypeDef *CANx) { - // From H7 datasheet: Transmit cancellation is not intended for Tx FIFO operation. - UNUSED(CANx); + CANx->TXBCR = 0xFFFFU; // Abort message transmission on error interrupt + // Clear error interrupts + CANx->IR |= (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EW | FDCAN_IR_EP | FDCAN_IR_ELO | FDCAN_IR_BO | FDCAN_IR_TEFL | FDCAN_IR_RF0L); } diff --git a/panda/board/stm32h7/llspi.h b/panda/board/stm32h7/llspi.h new file mode 100644 index 000000000..0215c6fbb --- /dev/null +++ b/panda/board/stm32h7/llspi.h @@ -0,0 +1,91 @@ +// master -> panda DMA start +void llspi_mosi_dma(uint8_t *addr, int len) { + // disable DMA + SPI + register_clear_bits(&(SPI4->CFG1), SPI_CFG1_RXDMAEN); + DMA2_Stream2->CR &= ~DMA_SxCR_EN; + register_clear_bits(&(SPI4->CR1), SPI_CR1_SPE); + + // drain the bus + while ((SPI4->SR & SPI_SR_RXP) != 0U) { + volatile uint8_t dat = SPI4->RXDR; + (void)dat; + } + + // setup destination and length + register_set(&(DMA2_Stream2->M0AR), (uint32_t)addr, 0xFFFFFFFFU); + DMA2_Stream2->NDTR = len; + + // enable DMA + SPI + DMA2_Stream2->CR |= DMA_SxCR_EN; + register_set_bits(&(SPI4->CFG1), SPI_CFG1_RXDMAEN); + register_set_bits(&(SPI4->CR1), SPI_CR1_SPE); +} + +// panda -> master DMA start +void llspi_miso_dma(uint8_t *addr, int len) { + // disable DMA + DMA2_Stream3->CR &= ~DMA_SxCR_EN; + register_clear_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); + + // 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; + + // enable DMA + register_set_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); + DMA2_Stream3->CR |= DMA_SxCR_EN; +} + +// master -> panda DMA finished +void DMA2_Stream2_IRQ_Handler(void) { + // Clear interrupt flag + ENTER_CRITICAL(); + DMA2->LIFCR = DMA_LIFCR_CTCIF2; + + spi_handle_rx(); + + EXIT_CRITICAL(); +} + +// panda -> master DMA finished +void DMA2_Stream3_IRQ_Handler(void) { + // Clear interrupt flag + DMA2->LIFCR = DMA_LIFCR_CTCIF3; + + // Wait until the transaction is actually finished and clear the DR + // TODO: needs a timeout here, otherwise it gets stuck with no master clock! + while (!(SPI4->SR & SPI_SR_TXC)); + volatile uint8_t dat = SPI4->TXDR; + (void)dat; + + spi_handle_tx(); +} + + +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) + + // Setup MOSI DMA + register_set(&(DMAMUX1_Channel10->CCR), 83U, 0xFFFFFFFFU); + register_set(&(DMA2_Stream2->CR), (DMA_SxCR_MINC | DMA_SxCR_TCIE), 0x1E077EFEU); + register_set(&(DMA2_Stream2->PAR), (uint32_t)&(SPI4->RXDR), 0xFFFFFFFFU); + + // Setup MISO DMA, memory -> peripheral + register_set(&(DMAMUX1_Channel11->CCR), 84U, 0xFFFFFFFFU); + register_set(&(DMA2_Stream3->CR), (DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_TCIE), 0x1E077EFEU); + register_set(&(DMA2_Stream3->PAR), (uint32_t)&(SPI4->TXDR), 0xFFFFFFFFU); + + // Enable SPI + 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); + register_set(&(SPI4->CR2), 0, 0xFFFFU); + + NVIC_EnableIRQ(DMA2_Stream2_IRQn); + NVIC_EnableIRQ(DMA2_Stream3_IRQn); +} diff --git a/panda/board/stm32h7/peripherals.h b/panda/board/stm32h7/peripherals.h index e43c59396..cc52e6084 100644 --- a/panda/board/stm32h7/peripherals.h +++ b/panda/board/stm32h7/peripherals.h @@ -11,17 +11,26 @@ void gpio_usart2_init(void) { set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2); } +void gpio_uart7_init(void) { + // E7,E8: UART 7 for debugging + set_gpio_alternate(GPIOE, 7, GPIO_AF7_UART7); + set_gpio_alternate(GPIOE, 8, GPIO_AF7_UART7); +} + // Common GPIO initialization void common_init_gpio(void) { /// E2,E3,E4: RGB LED set_gpio_pullup(GPIOE, 2, PULL_NONE); set_gpio_mode(GPIOE, 2, MODE_OUTPUT); + set_gpio_output_type(GPIOE, 2, OUTPUT_TYPE_OPEN_DRAIN); set_gpio_pullup(GPIOE, 3, PULL_NONE); set_gpio_mode(GPIOE, 3, MODE_OUTPUT); + set_gpio_output_type(GPIOE, 3, OUTPUT_TYPE_OPEN_DRAIN); set_gpio_pullup(GPIOE, 4, PULL_NONE); set_gpio_mode(GPIOE, 4, MODE_OUTPUT); + set_gpio_output_type(GPIOE, 4, OUTPUT_TYPE_OPEN_DRAIN); // F7,F8,F9,F10: BOARD ID set_gpio_pullup(GPIOF, 7, PULL_NONE); @@ -36,29 +45,14 @@ void common_init_gpio(void) { set_gpio_pullup(GPIOF, 10, PULL_NONE); set_gpio_mode(GPIOF, 10, MODE_INPUT); - // G11,B3,D7,B4: transceiver enable - set_gpio_pullup(GPIOG, 11, PULL_NONE); - set_gpio_mode(GPIOG, 11, MODE_OUTPUT); + //C4,A1: OBD_SBU1, OBD_SBU2 + set_gpio_pullup(GPIOC, 4, PULL_NONE); + set_gpio_mode(GPIOC, 4, MODE_ANALOG); - // Speed was set to high by default after reset, changing to low - GPIOB->OSPEEDR = GPIO_OSPEEDR_OSPEED3; - set_gpio_pullup(GPIOB, 3, PULL_NONE); - set_gpio_mode(GPIOB, 3, MODE_OUTPUT); - - set_gpio_pullup(GPIOD, 7, PULL_NONE); - set_gpio_mode(GPIOD, 7, MODE_OUTPUT); - - set_gpio_pullup(GPIOB, 4, PULL_NONE); - set_gpio_mode(GPIOB, 4, MODE_OUTPUT); - - // B14: usb load switch - set_gpio_pullup(GPIOB, 14, PULL_NONE); - set_gpio_mode(GPIOB, 14, MODE_OUTPUT); - - //B1,F11 5VOUT_S, VOLT_S - set_gpio_pullup(GPIOB, 1, PULL_NONE); - set_gpio_mode(GPIOB, 1, MODE_ANALOG); + set_gpio_pullup(GPIOA, 1, PULL_NONE); + set_gpio_mode(GPIOA, 1, MODE_ANALOG); + //F11: VOLT_S set_gpio_pullup(GPIOF, 11, PULL_NONE); set_gpio_mode(GPIOF, 11, MODE_ANALOG); @@ -70,7 +64,7 @@ void common_init_gpio(void) { set_gpio_pullup(GPIOB, 9, PULL_NONE); set_gpio_alternate(GPIOB, 9, GPIO_AF9_FDCAN1); - + // B5,B6 (mplex to B12,B13): FDCAN2 set_gpio_pullup(GPIOB, 12, PULL_NONE); set_gpio_pullup(GPIOB, 13, PULL_NONE); @@ -80,7 +74,7 @@ void common_init_gpio(void) { set_gpio_pullup(GPIOB, 6, PULL_NONE); set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); - + // G9,G10: FDCAN3 set_gpio_pullup(GPIOG, 9, PULL_NONE); set_gpio_alternate(GPIOG, 9, GPIO_AF2_FDCAN3); @@ -91,6 +85,7 @@ void common_init_gpio(void) { void flasher_peripherals_init(void) { RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; + RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; } // Peripheral initialization @@ -104,6 +99,8 @@ void peripherals_init(void) { RCC->AHB4ENR |= RCC_AHB4ENR_GPIOFEN; RCC->AHB4ENR |= RCC_AHB4ENR_GPIOGEN; + RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; // SPI + RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; // SPI DMA RCC->APB1LENR |= RCC_APB1LENR_TIM2EN; // main counter RCC->APB1LENR |= RCC_APB1LENR_TIM6EN; // interrupt timer RCC->APB2ENR |= RCC_APB2ENR_TIM8EN; // clock source timer diff --git a/panda/board/stm32h7/stm32h7_config.h b/panda/board/stm32h7/stm32h7_config.h index abaed6815..393d7ae30 100644 --- a/panda/board/stm32h7/stm32h7_config.h +++ b/panda/board/stm32h7/stm32h7_config.h @@ -72,6 +72,9 @@ #include "stm32h7/llusb.h" +#include "drivers/spi.h" +#include "stm32h7/llspi.h" + void early_gpio_float(void) { RCC->AHB4ENR = RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOCEN | RCC_AHB4ENR_GPIODEN | RCC_AHB4ENR_GPIOEEN | RCC_AHB4ENR_GPIOFEN | RCC_AHB4ENR_GPIOGEN | RCC_AHB4ENR_GPIOHEN; GPIOA->MODER = 0; GPIOB->MODER = 0; GPIOC->MODER = 0; GPIOD->MODER = 0; GPIOE->MODER = 0; GPIOF->MODER = 0; GPIOG->MODER = 0; GPIOH->MODER = 0; diff --git a/panda/board/stm32h7/stm32h7x5_flash.ld b/panda/board/stm32h7/stm32h7x5_flash.ld index aeaa4e3be..5aef66374 100644 --- a/panda/board/stm32h7/stm32h7x5_flash.ld +++ b/panda/board/stm32h7/stm32h7x5_flash.ld @@ -64,10 +64,10 @@ _Min_Stack_Size = 0x400; /* required amount of stack */ /* Specify the memory areas */ MEMORY { -DTCMRAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K -RAM_D1 (xrw) : ORIGIN = 0x24000000, LENGTH = 320K -RAM_D2 (xrw) : ORIGIN = 0x30000000, LENGTH = 32K -RAM_D3 (xrw) : ORIGIN = 0x38000000, LENGTH = 16K +DTCMRAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K /* DTCM */ +RAM_D1 (xrw) : ORIGIN = 0x24000000, LENGTH = 320K /* AXI SRAM */ +RAM_D2 (xrw) : ORIGIN = 0x30000000, LENGTH = 32K /* SRAM1(16kb) + SRAM2(16kb) */ +RAM_D3 (xrw) : ORIGIN = 0x38000000, LENGTH = 16K /* SRAM4 */ ITCMRAM (xrw) : ORIGIN = 0x00000000, LENGTH = 64K FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K } @@ -152,7 +152,7 @@ SECTIONS _edata = .; /* define a global symbol at data end */ } >DTCMRAM AT> FLASH - + /* Uninitialized data section */ . = ALIGN(4); .bss : @@ -186,6 +186,12 @@ SECTIONS *(.ram_d1*) } >RAM_D1 + .ram_d2 (NOLOAD) : + { + . = ALIGN(4); + *(.ram_d2*) + } >RAM_D2 + .ARM.attributes 0 : { *(.ARM.attributes) } } diff --git a/panda/python/__init__.py b/panda/python/__init__.py index de382f384..98b2c4a24 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -1,15 +1,18 @@ # python library to interface with panda -import datetime +import os +import sys +import time +import usb1 import struct import hashlib -import usb1 -import os -import time +import datetime import traceback -import sys +import warnings +import logging from functools import wraps from typing import Optional from itertools import accumulate + from .dfu import PandaDFU, MCU_TYPE_F2, MCU_TYPE_F4, MCU_TYPE_H7 # pylint: disable=import-error from .flash_release import flash_release # noqa pylint: disable=import-error from .update import ensure_st_up_to_date # noqa pylint: disable=import-error @@ -19,6 +22,11 @@ from .config import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, SECTOR_SIZES_FX, SECTOR_SIZ __version__ = '0.0.10' +# setup logging +LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper() +logging.basicConfig(level=LOGLEVEL, format='%(message)s') + + BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") DEBUG = os.getenv("PANDADEBUG") is not None @@ -48,7 +56,7 @@ def pack_can_buffer(arr): snds.append(b'') idx += 1 - #Apply counter to each 64 byte packet + # Apply counter to each 64 byte packet for idx in range(len(snds)): tx = b'' counter = 0 @@ -116,6 +124,16 @@ def ensure_can_packet_version(fn): return fn(self, *args, **kwargs) return wrapper +def ensure_can_health_packet_version(fn): + @wraps(fn) + def wrapper(self, *args, **kwargs): + if self.can_health_version < self.CAN_HEALTH_PACKET_VERSION: + raise RuntimeError("Panda firmware has outdated CAN health packet definition. Reflash panda firmware.") + elif self.can_health_version > self.CAN_HEALTH_PACKET_VERSION: + raise RuntimeError("Panda python library has outdated CAN health packet definition. Update panda python library.") + return fn(self, *args, **kwargs) + return wrapper + class ALTERNATIVE_EXPERIENCE: DEFAULT = 0 DISABLE_DISENGAGE_ON_GAS = 1 @@ -171,14 +189,21 @@ class Panda: HW_TYPE_UNO = b'\x05' HW_TYPE_DOS = b'\x06' HW_TYPE_RED_PANDA = b'\x07' + HW_TYPE_RED_PANDA_V2 = b'\x08' + HW_TYPE_TRES = b'\x09' CAN_PACKET_VERSION = 2 - HEALTH_PACKET_VERSION = 8 - HEALTH_STRUCT = struct.Struct(" bool: - return self.get_type() in Panda.H7_DEVICES - - def is_internal(self) -> bool: - return self.get_type() in (Panda.HW_TYPE_UNO, Panda.HW_TYPE_DOS) + def is_internal(self): + return self.get_type() in Panda.INTERNAL_DEVICES def get_serial(self): dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20) @@ -591,14 +646,8 @@ class Panda: def set_can_data_speed_kbps(self, bus, speed): self._handle.controlWrite(Panda.REQUEST_OUT, 0xf9, bus, int(speed * 10), b'') - # CAN FD and BRS status - def get_canfd_status(self, bus): - dat = self._handle.controlRead(Panda.REQUEST_IN, 0xfa, bus, 0, 2) - if dat: - a = struct.unpack("BB", dat) - return (a[0], a[1]) - else: - return (None, None) + def set_canfd_non_iso(self, bus, non_iso): + self._handle.controlWrite(Panda.REQUEST_OUT, 0xfc, bus, int(non_iso), b'') def set_uart_baud(self, uart, rate): self._handle.controlWrite(Panda.REQUEST_OUT, 0xe4, uart, int(rate / 300), b'') diff --git a/panda/python/spi.py b/panda/python/spi.py new file mode 100644 index 000000000..6a6569fed --- /dev/null +++ b/panda/python/spi.py @@ -0,0 +1,105 @@ +import math +import struct +import spidev +import logging +from functools import reduce +from typing import List + +# Constants +SYNC = 0x5A +HACK = 0x79 +DACK = 0x85 +NACK = 0x1F +CHECKSUM_START = 0xAB + +MAX_RETRY_COUNT = 5 + +USB_MAX_SIZE = 0x40 + +# This mimics the handle given by libusb1 for easy interoperability +class SpiHandle: + def __init__(self): + self.spi = spidev.SpiDev() # pylint: disable=c-extension-no-member + self.spi.open(0, 0) + + self.spi.max_speed_hz = 30000000 + + # helpers + def _calc_checksum(self, data: List[int]) -> int: + cksum = CHECKSUM_START + for b in data: + cksum ^= b + return cksum + + def _transfer(self, endpoint: int, data, max_rx_len: int = 1000) -> bytes: + logging.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len) + logging.debug("==============================================") + + for n in range(MAX_RETRY_COUNT): + logging.debug("\ntry #%d", n+1) + try: + logging.debug("- send header") + packet = struct.pack(" int: + for x in range(math.ceil(len(data) / USB_MAX_SIZE)): + self._transfer(endpoint, data[USB_MAX_SIZE*x:USB_MAX_SIZE*(x+1)]) + return len(data) + + def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes: + ret: List[int] = [] + for _ in range(math.ceil(length / USB_MAX_SIZE)): + d = self._transfer(endpoint, [], max_rx_len=USB_MAX_SIZE) + ret += d + if len(d) < USB_MAX_SIZE: + break + return bytes(ret) diff --git a/panda/python/uds.py b/panda/python/uds.py index c35fe30f5..5bf35088e 100644 --- a/panda/python/uds.py +++ b/panda/python/uds.py @@ -376,13 +376,31 @@ class CanClient(): self._recv_buffer() class IsoTpMessage(): - def __init__(self, can_client: CanClient, timeout: float = 1, debug: bool = False, max_len: int = 8): + def __init__(self, can_client: CanClient, timeout: float = 1, single_frame_mode: bool = False, separation_time: float = 0, + debug: bool = False, max_len: int = 8): self._can_client = can_client self.timeout = timeout + self.single_frame_mode = single_frame_mode self.debug = debug self.max_len = max_len - def send(self, dat: bytes) -> None: + # <= 127, separation time in milliseconds + # 0xF1 to 0xF9 UF, 100 to 900 microseconds + if 1e-4 <= separation_time <= 9e-4: + offset = int(round(separation_time, 4) * 1e4) - 1 + separation_time = 0xF1 + offset + elif 0 <= separation_time <= 0.127: + separation_time = round(separation_time * 1000) + else: + raise Exception("Separation time not in range") + + self.flow_control_msg = bytes([ + 0x30, # flow control + 0x01 if self.single_frame_mode else 0x00, # block size + separation_time, + ]).ljust(self.max_len, b"\x00") + + def send(self, dat: bytes, setup_only: bool = False) -> None: # throw away any stale data self._can_client.recv(drain=True) @@ -396,39 +414,42 @@ class IsoTpMessage(): self.rx_idx = 0 self.rx_done = False - if self.debug: + if self.debug and not setup_only: print(f"ISO-TP: REQUEST - {hex(self._can_client.tx_addr)} 0x{bytes.hex(self.tx_dat)}") - self._tx_first_frame() + self._tx_first_frame(setup_only=setup_only) - def _tx_first_frame(self) -> None: + def _tx_first_frame(self, setup_only: bool = False) -> None: if self.tx_len < self.max_len: # single frame (send all bytes) - if self.debug: + if self.debug and not setup_only: print(f"ISO-TP: TX - single frame - {hex(self._can_client.tx_addr)}") msg = (bytes([self.tx_len]) + self.tx_dat).ljust(self.max_len, b"\x00") self.tx_done = True else: # first frame (send first 6 bytes) - if self.debug: + if self.debug and not setup_only: print(f"ISO-TP: TX - first frame - {hex(self._can_client.tx_addr)}") msg = (struct.pack("!H", 0x1000 | self.tx_len) + self.tx_dat[:self.max_len - 2]).ljust(self.max_len - 2, b"\x00") - self._can_client.send([msg]) + if not setup_only: + self._can_client.send([msg]) - def recv(self, timeout=None) -> Optional[bytes]: + def recv(self, timeout=None) -> Tuple[Optional[bytes], bool]: if timeout is None: timeout = self.timeout start_time = time.monotonic() + updated = False try: while True: for msg in self._can_client.recv(): self._isotp_rx_next(msg) start_time = time.monotonic() + updated = True if self.tx_done and self.rx_done: - return self.rx_dat + return self.rx_dat, updated # no timeout indicates non-blocking if timeout == 0: - return None + return None, updated if time.monotonic() - start_time > timeout: raise MessageTimeoutError("timeout waiting for response") finally: @@ -456,9 +477,8 @@ class IsoTpMessage(): print(f"ISO-TP: RX - first frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}") if self.debug: print(f"ISO-TP: TX - flow control continue - {hex(self._can_client.tx_addr)}") - # send flow control message (send all bytes) - msg = b"\x30\x00\x00".ljust(self.max_len, b"\x00") - self._can_client.send([msg]) + # send flow control message + self._can_client.send([self.flow_control_msg]) return # consecutive rx frame @@ -470,6 +490,9 @@ class IsoTpMessage(): self.rx_dat += rx_data[1:1 + rx_size] if self.rx_len == len(self.rx_dat): self.rx_done = True + elif self.single_frame_mode: + # notify ECU to send next frame + 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 @@ -548,12 +571,12 @@ class UdsClient(): req += data # send request, wait for response - isotp_msg = IsoTpMessage(self._can_client, self.timeout, self.debug) + isotp_msg = IsoTpMessage(self._can_client, timeout=self.timeout, debug=self.debug) isotp_msg.send(req) response_pending = False while True: timeout = self.response_pending_timeout if response_pending else self.timeout - resp = isotp_msg.recv(timeout) + resp, _ = isotp_msg.recv(timeout) if resp is None: continue diff --git a/release/build_devel.sh b/release/build_devel.sh index f06e3102c..668ac0de1 100755 --- a/release/build_devel.sh +++ b/release/build_devel.sh @@ -12,10 +12,7 @@ fi # set git identity source $DIR/identity.sh -echo "[-] Setting up repo T=$SECONDS" - -cd $SOURCE_DIR -git fetch origin +echo "[-] Setting up target repo T=$SECONDS" rm -rf $TARGET_DIR mkdir -p $TARGET_DIR @@ -25,14 +22,14 @@ pre-commit uninstall || true echo "[-] bringing master-ci and devel in sync T=$SECONDS" cd $TARGET_DIR -git fetch origin master-ci -git fetch origin devel +git fetch --depth 1 origin master-ci +git fetch --depth 1 origin devel git checkout -f --track origin/master-ci git reset --hard master-ci git checkout master-ci git reset --hard origin/devel -git clean -xdf +git clean -xdff git lfs uninstall # remove everything except .git @@ -41,7 +38,7 @@ find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm - # reset source tree cd $SOURCE_DIR -git clean -xdf +git clean -xdff # do the files copy echo "[-] copying files T=$SECONDS" diff --git a/release/build_release.sh b/release/build_release.sh index 1000e607f..80106eefb 100755 --- a/release/build_release.sh +++ b/release/build_release.sh @@ -78,7 +78,7 @@ find . -name 'moc_*' -delete find . -name '__pycache__' -delete rm -rf panda/board panda/certs panda/crypto rm -rf .sconsign.dblite Jenkinsfile release/ -rm selfdrive/modeld/models/supercombo.dlc +rm selfdrive/modeld/models/supercombo.onnx # Move back signed panda fw mkdir -p panda/board/obj diff --git a/release/files_common b/release/files_common index 663d6d355..a294e1e5b 100644 --- a/release/files_common +++ b/release/files_common @@ -90,10 +90,12 @@ selfdrive/boardd/boardd_api_impl.pyx selfdrive/boardd/can_list_to_can_capnp.cc selfdrive/boardd/panda.cc selfdrive/boardd/panda.h -selfdrive/boardd/pigeon.cc -selfdrive/boardd/pigeon.h +selfdrive/boardd/spi.cc +selfdrive/boardd/panda_comms.h +selfdrive/boardd/panda_comms.cc selfdrive/boardd/set_time.py selfdrive/boardd/pandad.py +selfdrive/boardd/tests/test_boardd_loopback.py selfdrive/car/__init__.py selfdrive/car/docs_definitions.py @@ -103,6 +105,7 @@ selfdrive/car/interfaces.py selfdrive/car/vin.py selfdrive/car/disable_ecu.py selfdrive/car/fw_versions.py +selfdrive/car/fw_query_definitions.py selfdrive/car/ecu_addrs.py selfdrive/car/isotp_parallel_query.py selfdrive/car/tests/__init__.py @@ -175,7 +178,6 @@ selfdrive/controls/lib/alerts_offroad.json selfdrive/controls/lib/desire_helper.py selfdrive/controls/lib/drive_helpers.py selfdrive/controls/lib/events.py -selfdrive/controls/lib/lane_planner.py selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py selfdrive/controls/lib/latcontrol_torque.py @@ -242,6 +244,7 @@ selfdrive/locationd/models/live_kf.cc selfdrive/locationd/models/constants.py selfdrive/locationd/models/gnss_helpers.py +selfdrive/locationd/torqued.py selfdrive/locationd/calibrationd.py system/logcatd/.gitignore @@ -282,6 +285,7 @@ selfdrive/sensord/sensors_qcom2.cc selfdrive/sensord/sensors/*.cc selfdrive/sensord/sensors/*.h selfdrive/sensord/sensord +selfdrive/sensord/pigeond.py selfdrive/thermald/thermald.py selfdrive/thermald/power_monitoring.py @@ -305,6 +309,8 @@ selfdrive/ui/soundd/soundd selfdrive/ui/soundd/.gitignore selfdrive/ui/translations/*.ts selfdrive/ui/translations/languages.json +selfdrive/ui/update_translations.py +selfdrive/ui/tests/test_translations.py selfdrive/ui/qt/*.cc selfdrive/ui/qt/*.h @@ -353,7 +359,7 @@ selfdrive/modeld/models/driving.cc selfdrive/modeld/models/driving.h selfdrive/modeld/models/dmonitoring.cc selfdrive/modeld/models/dmonitoring.h -selfdrive/modeld/models/supercombo.dlc +selfdrive/modeld/models/supercombo.onnx selfdrive/modeld/models/dmonitoring_model_q.dlc selfdrive/modeld/transforms/loadyuv.cc @@ -364,12 +370,11 @@ selfdrive/modeld/transforms/transform.h selfdrive/modeld/transforms/transform.cl selfdrive/modeld/thneed/*.py -selfdrive/modeld/thneed/thneed.* +selfdrive/modeld/thneed/thneed.h +selfdrive/modeld/thneed/thneed_common.cc +selfdrive/modeld/thneed/thneed_qcom2.cc selfdrive/modeld/thneed/serialize.cc -selfdrive/modeld/thneed/compile.cc -selfdrive/modeld/thneed/optimizer.cc selfdrive/modeld/thneed/include/* -selfdrive/modeld/thneed/kernels/*.cl selfdrive/modeld/runners/snpemodel.cc selfdrive/modeld/runners/snpemodel.h @@ -536,7 +541,7 @@ opendbc/honda_civic_ex_2022_can_generated.dbc opendbc/hyundai_canfd.dbc opendbc/hyundai_kia_generic.dbc -opendbc/hyundai_kia_mando_front_radar.dbc +opendbc/hyundai_kia_mando_front_radar_generated.dbc opendbc/mazda_2017.dbc @@ -560,3 +565,18 @@ opendbc/vw_mqb_2010.dbc opendbc/tesla_can.dbc opendbc/tesla_radar.dbc opendbc/tesla_powertrain.dbc + +tinygrad_repo/openpilot/compile.py +tinygrad_repo/accel/opencl/* +tinygrad_repo/extra/onnx.py +tinygrad_repo/extra/thneed.py +tinygrad_repo/extra/utils.py +tinygrad_repo/tinygrad/llops/ops_gpu.py +tinygrad_repo/tinygrad/llops/ops_opencl.py +tinygrad_repo/tinygrad/helpers.py +tinygrad_repo/tinygrad/mlops.py +tinygrad_repo/tinygrad/ops.py +tinygrad_repo/tinygrad/shapetracker.py +tinygrad_repo/tinygrad/tensor.py +tinygrad_repo/tinygrad/nn/__init__.py +tinygrad_repo/tinygrad/nn/optim.py diff --git a/release/files_tici b/release/files_tici index 8e3249cdf..c8abd720d 100644 --- a/release/files_tici +++ b/release/files_tici @@ -9,6 +9,8 @@ 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/* diff --git a/selfdrive/assets/fonts/JetBrainsMono-Medium.ttf b/selfdrive/assets/fonts/JetBrainsMono-Medium.ttf new file mode 100644 index 000000000..a6ba5529a Binary files /dev/null and b/selfdrive/assets/fonts/JetBrainsMono-Medium.ttf differ diff --git a/selfdrive/assets/images/button_flag.png b/selfdrive/assets/images/button_flag.png new file mode 100644 index 000000000..cac4db6d4 Binary files /dev/null and b/selfdrive/assets/images/button_flag.png differ diff --git a/selfdrive/assets/img_couch.svg b/selfdrive/assets/img_couch.svg new file mode 100644 index 000000000..5b3c04831 --- /dev/null +++ b/selfdrive/assets/img_couch.svg @@ -0,0 +1,3 @@ + + + diff --git a/selfdrive/assets/img_experimental.svg b/selfdrive/assets/img_experimental.svg new file mode 100644 index 000000000..0eaec3b3c --- /dev/null +++ b/selfdrive/assets/img_experimental.svg @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/selfdrive/assets/img_experimental_grey.svg b/selfdrive/assets/img_experimental_grey.svg new file mode 100644 index 000000000..dc87105ac --- /dev/null +++ b/selfdrive/assets/img_experimental_grey.svg @@ -0,0 +1,4 @@ + + + + diff --git a/selfdrive/assets/img_experimental_white.svg b/selfdrive/assets/img_experimental_white.svg new file mode 100644 index 000000000..ae4f18fde --- /dev/null +++ b/selfdrive/assets/img_experimental_white.svg @@ -0,0 +1,4 @@ + + + + diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 6ccd6c3de..5b351ca0f 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -739,14 +739,14 @@ def main(): break except (ConnectionError, TimeoutError, WebSocketException): conn_retries += 1 - params.delete("LastAthenaPingTime") + params.remove("LastAthenaPingTime") except socket.timeout: - params.delete("LastAthenaPingTime") + params.remove("LastAthenaPingTime") except Exception: cloudlog.exception("athenad.main.exception") conn_retries += 1 - params.delete("LastAthenaPingTime") + params.remove("LastAthenaPingTime") time.sleep(backoff(conn_retries)) diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index 6bbb03a79..59ca2430c 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -27,7 +27,7 @@ def main(): except Exception: cloudlog.exception("manage_athenad.exception") finally: - params.delete(ATHENA_MGR_PID_PARAM) + params.remove(ATHENA_MGR_PID_PARAM) if __name__ == '__main__': diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index 922107509..d99e67a9f 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,9 +1,9 @@ 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', 'pigeon.cc'], LIBS=libs) +env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=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'], LIBS=libs) + env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=libs) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 23bf8f292..549690225 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -19,8 +19,6 @@ #include #include -#include - #include "cereal/gen/cpp/car.capnp.h" #include "cereal/messaging/messaging.h" #include "common/params.h" @@ -29,14 +27,12 @@ #include "common/util.h" #include "system/hardware/hw.h" -#include "selfdrive/boardd/pigeon.h" - // -- Multi-panda conventions -- // Ordering: // - The internal panda will always be the first panda // - Consecutive pandas will be sorted based on panda type, and then serial number // Connecting: -// - If a panda connection is dropped, boardd wil reconnect to all pandas +// - If a panda connection is dropped, boardd will reconnect to all pandas // - If a panda is added, we will only reconnect when we are offroad // CAN buses: // - Each panda will have it's block of 4 buses. E.g.: the second panda will use @@ -46,7 +42,7 @@ // Safety: // - SafetyConfig is a list, which is mapped to the connected pandas // - If there are more pandas connected than there are SafetyConfigs, -// the excess pandas will remain in "silent" ot "noOutput" mode +// the excess pandas will remain in "silent" or "noOutput" mode // Ignition: // - If any of the ignition sources in any panda is high, ignition is high @@ -58,7 +54,6 @@ using namespace std::chrono_literals; std::atomic ignition(false); -std::atomic pigeon_active(false); ExitHandler do_exit; @@ -70,7 +65,7 @@ static std::string get_time_str(const struct tm &time) { bool check_all_connected(const std::vector &pandas) { for (const auto& panda : pandas) { - if (!panda->connected) { + if (!panda->connected()) { do_exit = true; return false; } @@ -116,9 +111,9 @@ bool safety_setter_thread(std::vector pandas) { } // set to ELM327 for fingerprinting - pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327); - for (int i = 1; i < pandas.size(); i++) { - pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::SILENT); + 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); } Params p = Params(); @@ -140,7 +135,9 @@ bool safety_setter_thread(std::vector pandas) { } // set to ELM327 for ECU knockouts - pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); + for (Panda *panda : pandas) { + panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); + } std::string params; LOGW("waiting for params to set safety model"); @@ -185,7 +182,7 @@ bool safety_setter_thread(std::vector pandas) { return true; } -Panda *usb_connect(std::string serial="", uint32_t index=0) { +Panda *connect(std::string serial="", uint32_t index=0) { std::unique_ptr panda; try { panda = std::make_unique(serial, (index * PANDA_BUS_CNT)); @@ -199,12 +196,6 @@ Panda *usb_connect(std::string serial="", uint32_t index=0) { } //panda->enable_deepsleep(); - // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton -#ifndef __x86_64__ - static std::once_flag connected_once; - std::call_once(connected_once, &Panda::set_usb_power_mode, panda, cereal::PeripheralState::UsbPowerMode::CDP); -#endif - sync_time(panda.get(), SyncTimeDir::FROM_PANDA); return panda.release(); } @@ -234,9 +225,9 @@ void can_send_thread(std::vector pandas, bool fake_send) { //Dont send if older than 1 second if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) { for (const auto& panda : pandas) { - LOGT("sending sendcan to panda: %s", (panda->usb_serial).c_str()); + LOGT("sending sendcan to panda: %s", (panda->hw_serial).c_str()); panda->can_send(event.getSendcan()); - LOGT("sendcan sent to panda: %s", (panda->usb_serial).c_str()); + LOGT("sendcan sent to panda: %s", (panda->hw_serial).c_str()); } } } @@ -303,13 +294,19 @@ void send_empty_panda_state(PubMaster *pm) { std::optional send_panda_states(PubMaster *pm, const std::vector &pandas, bool spoofing_started) { bool ignition_local = false; + const uint32_t pandas_cnt = pandas.size(); // build msg MessageBuilder msg; auto evt = msg.initEvent(); - auto pss = evt.initPandaStates(pandas.size()); + auto pss = evt.initPandaStates(pandas_cnt); std::vector pandaStates; + pandaStates.reserve(pandas_cnt); + + std::vector> pandaCanStates; + pandaCanStates.reserve(pandas_cnt); + for (const auto& panda : pandas){ auto health_opt = panda->get_state(); if (!health_opt) { @@ -318,6 +315,16 @@ std::optional send_panda_states(PubMaster *pm, const std::vector health_t health = *health_opt; + std::array can_health{}; + for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) { + auto can_health_opt = panda->get_can_state(i); + if (!can_health_opt) { + return std::nullopt; + } + can_health[i] = *can_health_opt; + } + pandaCanStates.push_back(can_health); + if (spoofing_started) { health.ignition_line_pkt = 1; } @@ -327,7 +334,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector pandaStates.push_back(health); } - for (uint32_t i = 0; i < pandas.size(); i++) { + for (uint32_t i = 0; i < pandas_cnt; i++) { auto panda = pandas[i]; const auto &health = pandaStates[i]; @@ -337,7 +344,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector } #ifndef __x86_64__ - bool power_save_desired = !ignition_local && !pigeon_active; + bool power_save_desired = !ignition_local; if (health.power_save_enabled_pkt != power_save_desired) { panda->set_power_saving(power_save_desired); } @@ -348,20 +355,20 @@ std::optional send_panda_states(PubMaster *pm, const std::vector } #endif - if (!panda->comms_healthy) { + if (!panda->comms_healthy()) { evt.setValid(false); } auto ps = pss[i]; ps.setUptime(health.uptime_pkt); - ps.setBlockedCnt(health.blocked_msg_cnt_pkt); + ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt); + ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt); ps.setIgnitionLine(health.ignition_line_pkt); ps.setIgnitionCan(health.ignition_can_pkt); ps.setControlsAllowed(health.controls_allowed_pkt); ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt); - ps.setCanRxErrs(health.can_rx_errs_pkt); - ps.setCanSendErrs(health.can_send_errs_pkt); - ps.setCanFwdErrs(health.can_fwd_errs_pkt); + ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); + ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); ps.setGmlanSendErrs(health.gmlan_send_errs_pkt); ps.setPandaType(panda->hw_type); ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); @@ -373,6 +380,34 @@ 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.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid)); + + std::array cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; + + for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) { + const auto &can_health = pandaCanStates[i][j]; + cs[j].setBusOff((bool)can_health.bus_off); + cs[j].setBusOffCnt(can_health.bus_off_cnt); + cs[j].setErrorWarning((bool)can_health.error_warning); + cs[j].setErrorPassive((bool)can_health.error_passive); + cs[j].setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error)); + cs[j].setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error)); + cs[j].setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error)); + cs[j].setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error)); + cs[j].setReceiveErrorCnt(can_health.receive_error_cnt); + cs[j].setTransmitErrorCnt(can_health.transmit_error_cnt); + cs[j].setTotalErrorCnt(can_health.total_error_cnt); + cs[j].setTotalTxLostCnt(can_health.total_tx_lost_cnt); + cs[j].setTotalRxLostCnt(can_health.total_rx_lost_cnt); + cs[j].setTotalTxCnt(can_health.total_tx_cnt); + cs[j].setTotalRxCnt(can_health.total_rx_cnt); + cs[j].setTotalFwdCnt(can_health.total_fwd_cnt); + cs[j].setCanSpeed(can_health.can_speed); + cs[j].setCanDataSpeed(can_health.can_data_speed); + cs[j].setCanfdEnabled(can_health.canfd_enabled); + cs[j].setBrsEnabled(can_health.brs_enabled); + cs[j].setCanfdNonIso(can_health.canfd_non_iso); + } // Convert faults bitset to capnp list std::bitset fault_bits(health.faults_pkt); @@ -393,17 +428,10 @@ std::optional send_panda_states(PubMaster *pm, const std::vector } void send_peripheral_state(PubMaster *pm, Panda *panda) { - auto pandaState_opt = panda->get_state(); - if (!pandaState_opt) { - return; - } - - health_t pandaState = *pandaState_opt; - // build msg MessageBuilder msg; auto evt = msg.initEvent(); - evt.setValid(panda->comms_healthy); + evt.setValid(panda->comms_healthy()); auto ps = evt.initPeripheralState(); ps.setPandaType(panda->hw_type); @@ -417,7 +445,6 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { } uint16_t fan_speed_rpm = panda->get_fan_speed(); - ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode_pkt)); ps.setFanSpeedRpm(fan_speed_rpm); pm->send("peripheralState", msg); @@ -484,7 +511,7 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin } -void peripheral_control_thread(Panda *panda) { +void peripheral_control_thread(Panda *panda, bool no_fan_control) { util::set_thread_name("boardd_peripheral_control"); SubMaster sm({"deviceState", "driverCameraState"}); @@ -493,33 +520,18 @@ void peripheral_control_thread(Panda *panda) { uint16_t prev_fan_speed = 999; uint16_t ir_pwr = 0; uint16_t prev_ir_pwr = 999; - bool prev_charging_disabled = false; unsigned int cnt = 0; FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); - while (!do_exit && panda->connected) { + while (!do_exit && panda->connected()) { cnt++; sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? - if (!Hardware::PC() && sm.updated("deviceState")) { - // Charging mode - bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled(); - if (charging_disabled != prev_charging_disabled) { - if (charging_disabled) { - panda->set_usb_power_mode(cereal::PeripheralState::UsbPowerMode::CLIENT); - LOGW("TURN OFF CHARGING!\n"); - } else { - panda->set_usb_power_mode(cereal::PeripheralState::UsbPowerMode::CDP); - LOGW("TURN ON CHARGING!\n"); - } - prev_charging_disabled = charging_disabled; - } - } - // Other pandas don't have fan/IR to control if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue; - if (sm.updated("deviceState")) { + + if (sm.updated("deviceState") && !no_fan_control) { // Fan speed uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); if (fan_speed != prev_fan_speed || cnt % 100 == 0) { @@ -527,6 +539,7 @@ void peripheral_control_thread(Panda *panda) { prev_fan_speed = fan_speed; } } + if (sm.updated("driverCameraState")) { auto event = sm["driverCameraState"]; int cur_integ_lines = event.getDriverCameraState().getIntegLines(); @@ -543,6 +556,7 @@ void peripheral_control_thread(Panda *panda) { ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_integ_lines - CUTOFF_IL) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_IL - CUTOFF_IL))); } } + // Disable ir_pwr on front frame timeout uint64_t cur_t = nanos_since_boot(); if (cur_t - last_front_frame_t > 1e9) { @@ -561,56 +575,6 @@ void peripheral_control_thread(Panda *panda) { } } -static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) { - // create message - MessageBuilder msg; - msg.initEvent().setUbloxRaw(capnp::Data::Reader((uint8_t*)dat.data(), dat.length())); - pm.send("ubloxRaw", msg); -} - -void pigeon_thread(Panda *panda) { - util::set_thread_name("boardd_pigeon"); - - PubMaster pm({"ubloxRaw"}); - bool ignition_last = false; - - std::unique_ptr pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda)); - - while (!do_exit && panda->connected) { - bool need_reset = false; - bool ignition_local = ignition; - std::string recv = pigeon->receive(); - - // Check based on null bytes - if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) { - need_reset = true; - LOGW("received invalid ublox message while onroad, resetting panda GPS"); - } - - if (recv.length() > 0) { - pigeon_publish_raw(pm, recv); - } - - // init pigeon on rising ignition edge - // since it was turned off in low power mode - if((ignition_local && !ignition_last) || need_reset) { - pigeon_active = true; - pigeon->init(); - } else if (!ignition_local && ignition_last) { - // power off on falling edge of ignition - LOGD("powering off pigeon\n"); - pigeon->stop(); - pigeon->set_power(false); - pigeon_active = false; - } - - ignition_last = ignition_local; - - // 10ms - 100 Hz - util::sleep_for(10); - } -} - void boardd_main_thread(std::vector serials) { PubMaster pm({"pandaStates", "peripheralState"}); LOGW("attempting to connect"); @@ -629,7 +593,7 @@ void boardd_main_thread(std::vector serials) { // connect to all provided serials std::vector pandas; for (int i = 0; i < serials.size() && !do_exit; /**/) { - Panda *p = usb_connect(serials[i], i); + Panda *p = connect(serials[i], i); if (!p) { // send empty pandaState & peripheralState and try again send_empty_panda_state(&pm); @@ -648,8 +612,7 @@ void boardd_main_thread(std::vector serials) { std::vector threads; threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr); - threads.emplace_back(peripheral_control_thread, peripheral_panda); - threads.emplace_back(pigeon_thread, peripheral_panda); + threads.emplace_back(peripheral_control_thread, peripheral_panda, getenv("NO_FAN_CONTROL") != nullptr); threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); threads.emplace_back(can_recv_thread, pandas); diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index d90c4cdab..deccee3e7 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -4,75 +4,19 @@ #include #include -#include #include "cereal/messaging/messaging.h" #include "panda/board/dlc_to_len.h" -#include "common/gpio.h" #include "common/swaglog.h" #include "common/util.h" -static int init_usb_ctx(libusb_context **context) { - assert(context != nullptr); - - int err = libusb_init(context); - if (err != 0) { - LOGE("libusb initialization error"); - return err; - } - -#if LIBUSB_API_VERSION >= 0x01000106 - libusb_set_option(*context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); -#else - libusb_set_debug(*context, 3); -#endif - - return err; -} - - Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { - // init libusb - ssize_t num_devices; - libusb_device **dev_list = NULL; - int err = init_usb_ctx(&ctx); - if (err != 0) { goto fail; } - - // connect by serial - num_devices = libusb_get_device_list(ctx, &dev_list); - if (num_devices < 0) { goto fail; } - for (size_t i = 0; i < num_devices; ++i) { - libusb_device_descriptor desc; - libusb_get_device_descriptor(dev_list[i], &desc); - if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { - int ret = libusb_open(dev_list[i], &dev_handle); - if (dev_handle == NULL || ret < 0) { goto fail; } - - unsigned char desc_serial[26] = { 0 }; - ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); - if (ret < 0) { goto fail; } - - usb_serial = std::string((char *)desc_serial, ret).c_str(); - if (serial.empty() || serial == usb_serial) { - break; - } - libusb_close(dev_handle); - dev_handle = NULL; - } + // TODO: support SPI here one day... + if (serial.find("spi") != std::string::npos) { + handle = std::make_unique(serial); + } else { + handle = std::make_unique(serial); } - if (dev_handle == NULL) goto fail; - libusb_free_device_list(dev_list, 1); - dev_list = nullptr; - - if (libusb_kernel_driver_active(dev_handle, 0) == 1) { - libusb_detach_kernel_driver(dev_handle, 0); - } - - err = libusb_set_configuration(dev_handle, 1); - if (err != 0) { goto fail; } - - err = libusb_claim_interface(dev_handle, 0); - if (err != 0) { goto fail; } hw_type = get_hw_type(); @@ -83,194 +27,44 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { (hw_type == cereal::PandaState::PandaType::DOS); return; - -fail: - if (dev_list != NULL) { - libusb_free_device_list(dev_list, 1); - } - cleanup(); - throw std::runtime_error("Error connecting to panda"); } -Panda::~Panda() { - std::lock_guard lk(usb_lock); - cleanup(); - connected = false; +bool Panda::connected() { + return handle->connected; } -void Panda::cleanup() { - if (dev_handle) { - libusb_release_interface(dev_handle, 0); - libusb_close(dev_handle); - } - - if (ctx) { - libusb_exit(ctx); - } +bool Panda::comms_healthy() { + return handle->comms_healthy; } std::vector Panda::list() { - // init libusb - ssize_t num_devices; - libusb_context *context = NULL; - libusb_device **dev_list = NULL; - std::vector serials; - - int err = init_usb_ctx(&context); - if (err != 0) { return serials; } - - num_devices = libusb_get_device_list(context, &dev_list); - if (num_devices < 0) { - LOGE("libusb can't get device list"); - goto finish; - } - for (size_t i = 0; i < num_devices; ++i) { - libusb_device *device = dev_list[i]; - libusb_device_descriptor desc; - libusb_get_device_descriptor(device, &desc); - if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { - libusb_device_handle *handle = NULL; - int ret = libusb_open(device, &handle); - if (ret < 0) { goto finish; } - - unsigned char desc_serial[26] = { 0 }; - ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); - libusb_close(handle); - if (ret < 0) { goto finish; } - - serials.push_back(std::string((char *)desc_serial, ret).c_str()); - } - } - -finish: - if (dev_list != NULL) { - libusb_free_device_list(dev_list, 1); - } - if (context) { - libusb_exit(context); - } - return serials; -} - -void Panda::handle_usb_issue(int err, const char func[]) { - LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); - if (err == LIBUSB_ERROR_NO_DEVICE) { - LOGE("lost connection"); - connected = false; - } - // TODO: check other errors, is simply retrying okay? -} - -int Panda::usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { - int err; - const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(usb_lock); - do { - err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); - if (err < 0) handle_usb_issue(err, __func__); - } while (err < 0 && connected); - - return err; -} - -int Panda::usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) { - int err; - const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(usb_lock); - do { - err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); - if (err < 0) handle_usb_issue(err, __func__); - } while (err < 0 && connected); - - return err; -} - -int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - int err; - int transferred = 0; - - if (!connected) { - return 0; - } - - std::lock_guard lk(usb_lock); - do { - // Try sending can messages. If the receive buffer on the panda is full it will NAK - // and libusb will try again. After 5ms, it will time out. We will drop the messages. - err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); - - if (err == LIBUSB_ERROR_TIMEOUT) { - LOGW("Transmit buffer full"); - break; - } else if (err != 0 || length != transferred) { - handle_usb_issue(err, __func__); - } - } while(err != 0 && connected); - - return transferred; -} - -int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - int err; - int transferred = 0; - - if (!connected) { - return 0; - } - - std::lock_guard lk(usb_lock); - - do { - err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); - - if (err == LIBUSB_ERROR_TIMEOUT) { - break; // timeout is okay to exit, recv still happened - } else if (err == LIBUSB_ERROR_OVERFLOW) { - comms_healthy = false; - LOGE_100("overflow got 0x%x", transferred); - } else if (err != 0) { - handle_usb_issue(err, __func__); - } - - } while(err != 0 && connected); - - return transferred; + return PandaUsbHandle::list(); } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { - usb_write(0xdc, (uint16_t)safety_model, safety_param); + handle->control_write(0xdc, (uint16_t)safety_model, safety_param); } void Panda::set_alternative_experience(uint16_t alternative_experience) { - usb_write(0xdf, alternative_experience, 0); + handle->control_write(0xdf, alternative_experience, 0); } cereal::PandaState::PandaType Panda::get_hw_type() { unsigned char hw_query[1] = {0}; - usb_read(0xc1, 0, 0, hw_query, 1); + handle->control_read(0xc1, 0, 0, hw_query, 1); return (cereal::PandaState::PandaType)(hw_query[0]); } void Panda::set_rtc(struct tm sys_time) { // tm struct has year defined as years since 1900 - usb_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0); - usb_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0); - usb_write(0xa3, (uint16_t)sys_time.tm_mday, 0); - // usb_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0); - usb_write(0xa5, (uint16_t)sys_time.tm_hour, 0); - usb_write(0xa6, (uint16_t)sys_time.tm_min, 0); - usb_write(0xa7, (uint16_t)sys_time.tm_sec, 0); + handle->control_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0); + handle->control_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0); + handle->control_write(0xa3, (uint16_t)sys_time.tm_mday, 0); + // handle->control_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0); + handle->control_write(0xa5, (uint16_t)sys_time.tm_hour, 0); + handle->control_write(0xa6, (uint16_t)sys_time.tm_min, 0); + handle->control_write(0xa7, (uint16_t)sys_time.tm_sec, 0); } struct tm Panda::get_rtc() { @@ -284,7 +78,7 @@ struct tm Panda::get_rtc() { uint8_t second; } rtc_time = {0}; - usb_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); + handle->control_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); struct tm new_time = { 0 }; new_time.tm_year = rtc_time.year - 1900; // tm struct has year defined as years since 1900 @@ -298,64 +92,70 @@ struct tm Panda::get_rtc() { } void Panda::set_fan_speed(uint16_t fan_speed) { - usb_write(0xb1, fan_speed, 0); + handle->control_write(0xb1, fan_speed, 0); } uint16_t Panda::get_fan_speed() { uint16_t fan_speed_rpm = 0; - usb_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm)); + handle->control_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm)); return fan_speed_rpm; } void Panda::set_ir_pwr(uint16_t ir_pwr) { - usb_write(0xb0, ir_pwr, 0); + handle->control_write(0xb0, ir_pwr, 0); } std::optional Panda::get_state() { health_t health {0}; - int err = usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); + int err = handle->control_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); return err >= 0 ? std::make_optional(health) : std::nullopt; } +std::optional Panda::get_can_state(uint16_t can_number) { + can_health_t can_health {0}; + int err = handle->control_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health)); + return err >= 0 ? std::make_optional(can_health) : std::nullopt; +} + void Panda::set_loopback(bool loopback) { - usb_write(0xe5, loopback, 0); + handle->control_write(0xe5, loopback, 0); } std::optional> Panda::get_firmware_version() { std::vector fw_sig_buf(128); - int read_1 = usb_read(0xd3, 0, 0, &fw_sig_buf[0], 64); - int read_2 = usb_read(0xd4, 0, 0, &fw_sig_buf[64], 64); + int read_1 = handle->control_read(0xd3, 0, 0, &fw_sig_buf[0], 64); + int read_2 = handle->control_read(0xd4, 0, 0, &fw_sig_buf[64], 64); return ((read_1 == 64) && (read_2 == 64)) ? std::make_optional(fw_sig_buf) : std::nullopt; } std::optional Panda::get_serial() { char serial_buf[17] = {'\0'}; - int err = usb_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); + int err = handle->control_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); return err >= 0 ? std::make_optional(serial_buf) : std::nullopt; } void Panda::set_power_saving(bool power_saving) { - usb_write(0xe7, power_saving, 0); + handle->control_write(0xe7, power_saving, 0); } void Panda::enable_deepsleep() { - usb_write(0xfb, 0, 0); -} - -void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode) { - usb_write(0xe6, (uint16_t)power_mode, 0); + handle->control_write(0xfb, 0, 0); } void Panda::send_heartbeat(bool engaged) { - usb_write(0xf3, engaged, 0); + handle->control_write(0xf3, engaged, 0); } void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { - usb_write(0xde, bus, (speed * 10)); + handle->control_write(0xde, bus, (speed * 10)); } void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) { - usb_write(0xf9, bus, (speed * 10)); + handle->control_write(0xf9, bus, (speed * 10)); +} + +void Panda::set_canfd_non_iso(uint16_t bus, bool non_iso) { + handle->control_write(0xfc, bus, non_iso); } static uint8_t len_to_dlc(uint8_t len) { @@ -393,7 +193,7 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data } auto can_data = cmsg.getDat(); uint8_t data_len_code = len_to_dlc(can_data.size()); - assert(can_data.size() <= ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8)); + assert(can_data.size() <= 64); assert(can_data.size() == dlc_to_len[data_len_code]); can_header header; @@ -416,14 +216,14 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data void Panda::can_send(capnp::List::Reader can_data_list) { pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) { - usb_bulk_write(3, data, size, 5); + handle->bulk_write(3, data, size, 5); }); } bool Panda::can_receive(std::vector& out_vec) { uint8_t data[RECV_SIZE]; - int recv = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE); - if (!comms_healthy) { + int recv = handle->bulk_read(0x81, (uint8_t*)data, RECV_SIZE); + if (!comms_healthy()) { return false; } if (recv == RECV_SIZE) { @@ -438,7 +238,7 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &o for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) { if (data[i] != i / USBPACKET_MAX_SIZE) { LOGE("CAN: MALFORMED USB RECV PACKET"); - comms_healthy = false; + handle->comms_healthy = false; return false; } int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i)); diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 23a10d585..5b3cbb9a3 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -1,25 +1,26 @@ #pragma once -#include #include #include #include #include -#include +#include #include #include -#include - #include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" #include "panda/board/health.h" +#include "selfdrive/boardd/panda_comms.h" -#define TIMEOUT 0 + +#define PANDA_CAN_CNT 3 #define PANDA_BUS_CNT 4 -#define RECV_SIZE (0x4000U) + #define USB_TX_SOFT_LIMIT (0x100U) #define USBPACKET_MAX_SIZE (0x40) + +#define RECV_SIZE (0x4000U) #define CANPACKET_HEAD_SIZE 5U #define CANPACKET_MAX_SIZE 72U #define CANPACKET_REJECTED (0xC0U) @@ -36,41 +37,32 @@ struct __attribute__((packed)) can_header { }; struct can_frame { - long address; - std::string dat; - long busTime; - long src; + long address; + std::string dat; + long busTime; + long src; }; + class Panda { - private: - libusb_context *ctx = NULL; - libusb_device_handle *dev_handle = NULL; - std::mutex usb_lock; +private: + std::unique_ptr handle; std::vector recv_buf; - void handle_usb_issue(int err, const char func[]); - void cleanup(); - public: +public: Panda(std::string serial="", uint32_t bus_offset=0); - ~Panda(); - std::string usb_serial; - std::atomic connected = true; - std::atomic comms_healthy = true; + std::string hw_serial; cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; bool has_rtc = false; const uint32_t bus_offset; + bool connected(); + bool comms_healthy(); + // Static functions static std::vector list(); - // HW communication - int usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout=TIMEOUT); - int usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout=TIMEOUT); - int usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - // Panda functionality cereal::PandaState::PandaType get_hw_type(); void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); @@ -81,15 +73,16 @@ class Panda { uint16_t get_fan_speed(); void set_ir_pwr(uint16_t ir_pwr); std::optional get_state(); + std::optional get_can_state(uint16_t can_number); void set_loopback(bool loopback); std::optional> get_firmware_version(); std::optional get_serial(); void set_power_saving(bool power_saving); void enable_deepsleep(); - void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode); void send_heartbeat(bool engaged); void set_can_speed_kbps(uint16_t bus, uint16_t speed); void set_data_speed_kbps(uint16_t bus, uint16_t speed); + void set_canfd_non_iso(uint16_t bus, bool non_iso); void can_send(capnp::List::Reader can_data_list); bool can_receive(std::vector& out_vec); diff --git a/selfdrive/boardd/panda_comms.cc b/selfdrive/boardd/panda_comms.cc new file mode 100644 index 000000000..e73cb6931 --- /dev/null +++ b/selfdrive/boardd/panda_comms.cc @@ -0,0 +1,232 @@ +#include "selfdrive/boardd/panda.h" + +#include +#include + +#include "common/swaglog.h" + +static int init_usb_ctx(libusb_context **context) { + assert(context != nullptr); + + int err = libusb_init(context); + if (err != 0) { + LOGE("libusb initialization error"); + return err; + } + +#if LIBUSB_API_VERSION >= 0x01000106 + libusb_set_option(*context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); +#else + libusb_set_debug(*context, 3); +#endif + + return err; +} + +PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) { + // init libusb + ssize_t num_devices; + libusb_device **dev_list = NULL; + int err = init_usb_ctx(&ctx); + if (err != 0) { goto fail; } + + // connect by serial + num_devices = libusb_get_device_list(ctx, &dev_list); + if (num_devices < 0) { goto fail; } + for (size_t i = 0; i < num_devices; ++i) { + libusb_device_descriptor desc; + libusb_get_device_descriptor(dev_list[i], &desc); + if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { + int ret = libusb_open(dev_list[i], &dev_handle); + if (dev_handle == NULL || ret < 0) { goto fail; } + + unsigned char desc_serial[26] = { 0 }; + ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + if (ret < 0) { goto fail; } + + auto hw_serial = std::string((char *)desc_serial, ret); + if (serial.empty() || serial == hw_serial) { + break; + } + libusb_close(dev_handle); + dev_handle = NULL; + } + } + if (dev_handle == NULL) goto fail; + libusb_free_device_list(dev_list, 1); + dev_list = nullptr; + + if (libusb_kernel_driver_active(dev_handle, 0) == 1) { + libusb_detach_kernel_driver(dev_handle, 0); + } + + err = libusb_set_configuration(dev_handle, 1); + if (err != 0) { goto fail; } + + err = libusb_claim_interface(dev_handle, 0); + if (err != 0) { goto fail; } + + return; + +fail: + if (dev_list != NULL) { + libusb_free_device_list(dev_list, 1); + } + cleanup(); + throw std::runtime_error("Error connecting to panda"); +} + +PandaUsbHandle::~PandaUsbHandle() { + std::lock_guard lk(hw_lock); + cleanup(); + connected = false; +} + +void PandaUsbHandle::cleanup() { + if (dev_handle) { + libusb_release_interface(dev_handle, 0); + libusb_close(dev_handle); + } + + if (ctx) { + libusb_exit(ctx); + } +} + +std::vector PandaUsbHandle::list() { + // init libusb + ssize_t num_devices; + libusb_context *context = NULL; + libusb_device **dev_list = NULL; + std::vector serials; + + int err = init_usb_ctx(&context); + if (err != 0) { return serials; } + + num_devices = libusb_get_device_list(context, &dev_list); + if (num_devices < 0) { + LOGE("libusb can't get device list"); + goto finish; + } + for (size_t i = 0; i < num_devices; ++i) { + libusb_device *device = dev_list[i]; + libusb_device_descriptor desc; + libusb_get_device_descriptor(device, &desc); + if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { + libusb_device_handle *handle = NULL; + int ret = libusb_open(device, &handle); + if (ret < 0) { goto finish; } + + unsigned char desc_serial[26] = { 0 }; + ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + libusb_close(handle); + if (ret < 0) { goto finish; } + + serials.push_back(std::string((char *)desc_serial, ret).c_str()); + } + } + +finish: + if (dev_list != NULL) { + libusb_free_device_list(dev_list, 1); + } + if (context) { + libusb_exit(context); + } + return serials; +} + +void PandaUsbHandle::handle_usb_issue(int err, const char func[]) { + LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); + if (err == LIBUSB_ERROR_NO_DEVICE) { + LOGE("lost connection"); + connected = false; + } + // TODO: check other errors, is simply retrying okay? +} + +int PandaUsbHandle::control_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + if (!connected) { + return LIBUSB_ERROR_NO_DEVICE; + } + + std::lock_guard lk(hw_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); + if (err < 0) handle_usb_issue(err, __func__); + } while (err < 0 && connected); + + return err; +} + +int PandaUsbHandle::control_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + if (!connected) { + return LIBUSB_ERROR_NO_DEVICE; + } + + std::lock_guard lk(hw_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); + if (err < 0) handle_usb_issue(err, __func__); + } while (err < 0 && connected); + + return err; +} + +int PandaUsbHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected) { + return 0; + } + + std::lock_guard lk(hw_lock); + do { + // Try sending can messages. If the receive buffer on the panda is full it will NAK + // and libusb will try again. After 5ms, it will time out. We will drop the messages. + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + LOGW("Transmit buffer full"); + break; + } else if (err != 0 || length != transferred) { + handle_usb_issue(err, __func__); + } + } while(err != 0 && connected); + + return transferred; +} + +int PandaUsbHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected) { + return 0; + } + + std::lock_guard lk(hw_lock); + + do { + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + break; // timeout is okay to exit, recv still happened + } else if (err == LIBUSB_ERROR_OVERFLOW) { + comms_healthy = false; + LOGE_100("overflow got 0x%x", transferred); + } else if (err != 0) { + handle_usb_issue(err, __func__); + } + + } while(err != 0 && connected); + + return transferred; +} diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h new file mode 100644 index 000000000..f42eadc5b --- /dev/null +++ b/selfdrive/boardd/panda_comms.h @@ -0,0 +1,80 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include + + +#define TIMEOUT 0 +#define SPI_BUF_SIZE 1024 + +const bool PANDA_NO_RETRY = getenv("PANDA_NO_RETRY"); + + +// comms base class +class PandaCommsHandle { +public: + PandaCommsHandle(std::string serial) {}; + virtual ~PandaCommsHandle() {}; + virtual void cleanup() = 0; + + std::atomic connected = true; + std::atomic comms_healthy = true; + static std::vector list(); + + // HW communication + virtual int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT) = 0; + virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0; + virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; + virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; + +protected: + std::recursive_mutex hw_lock; +}; + +class PandaUsbHandle : public PandaCommsHandle { +public: + PandaUsbHandle(std::string serial); + ~PandaUsbHandle(); + int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT); + int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT); + int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + void cleanup(); + + static std::vector list(); + +private: + libusb_context *ctx = NULL; + libusb_device_handle *dev_handle = NULL; + std::vector recv_buf; + void handle_usb_issue(int err, const char func[]); +}; + +class PandaSpiHandle : public PandaCommsHandle { +public: + PandaSpiHandle(std::string serial); + ~PandaSpiHandle(); + int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT); + int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT); + int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + void cleanup(); + + static std::vector list(); + +private: + int spi_fd = -1; + uint8_t tx_buf[SPI_BUF_SIZE]; + uint8_t rx_buf[SPI_BUF_SIZE]; + + int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack); + int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len); + int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); + int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); +}; diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 54a28a678..971756002 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -83,7 +83,7 @@ def main() -> NoReturn: while True: try: - params.delete("PandaSignatures") + params.remove("PandaSignatures") # Flash all Pandas in DFU mode for p in PandaDFU.list(): diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc deleted file mode 100644 index d23ff90d3..000000000 --- a/selfdrive/boardd/pigeon.cc +++ /dev/null @@ -1,333 +0,0 @@ -#include "selfdrive/boardd/pigeon.h" - -#include -#include -#include - -#include -#include -#include - -#include "common/gpio.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "selfdrive/locationd/ublox_msg.h" - -// Termios on macos doesn't define all baud rate constants -#ifndef B460800 -#define B460800 0010004 -#endif - -using namespace std::string_literals; - -extern ExitHandler do_exit; - -const std::string ack = "\xb5\x62\x05\x01\x02\x00"; -const std::string nack = "\xb5\x62\x05\x00\x02\x00"; -const std::string sos_save_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"; -const std::string sos_save_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"; - -Pigeon * Pigeon::connect(Panda * p) { - PandaPigeon * pigeon = new PandaPigeon(); - pigeon->connect(p); - - return pigeon; -} - -Pigeon * Pigeon::connect(const char * tty) { - TTYPigeon * pigeon = new TTYPigeon(); - pigeon->connect(tty); - - return pigeon; -} - -bool Pigeon::wait_for_ack(const std::string &ack_, const std::string &nack_, int timeout_ms) { - std::string s; - const double start_t = millis_since_boot(); - while (!do_exit) { - s += receive(); - - if (s.find(ack_) != std::string::npos) { - LOGD("Received ACK from ublox"); - return true; - } else if (s.find(nack_) != std::string::npos) { - LOGE("Received NACK from ublox"); - return false; - } else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) { - LOGE("No response from ublox"); - return false; - } - - util::sleep_for(1); // Allow other threads to be scheduled - } - return false; -} - -bool Pigeon::wait_for_ack() { - return wait_for_ack(ack, nack); -} - -bool Pigeon::send_with_ack(const std::string &cmd) { - send(cmd); - return wait_for_ack(); -} - -sos_restore_response Pigeon::wait_for_backup_restore_status(int timeout_ms) { - std::string s; - const double start_t = millis_since_boot(); - while (!do_exit) { - s += receive(); - - size_t position = s.find("\xb5\x62\x09\x14\x08\x00\x03"); - if (position != std::string::npos && s.size() >= (position + 11)) { - return static_cast(s[position + 10]); - } else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) { - LOGE("No backup restore response from ublox"); - return error; - } - - util::sleep_for(1); // Allow other threads to be scheduled - } - return error; -} - -void Pigeon::init() { - for (int i = 0; i < 10; i++) { - if (do_exit) return; - LOGW("panda GPS start"); - - // power off pigeon - set_power(false); - util::sleep_for(100); - - // 9600 baud at init - set_baud(9600); - - // power on pigeon - set_power(true); - util::sleep_for(500); - - // baud rate upping - send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s); - util::sleep_for(100); - - // set baud rate to 460800 - set_baud(460800); - - // init from ubloxd - // To generate this data, run selfdrive/locationd/test/ubloxd.py - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x00\x00\x06\x18"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x24\x00\x00\x2A\x84"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x23\x00\x00\x29\x81"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x1E\x00\x00\x24\x72"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x39\x00\x00\x3F\xC3"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue; - - // check the backup restore status - send("\xB5\x62\x09\x14\x00\x00\x1D\x60"s); - sos_restore_response restore_status = wait_for_backup_restore_status(); - switch(restore_status) { - case restored: - LOGW("almanac backup restored"); - // clear the backup - send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s); - break; - case no_backup: - LOGW("no almanac backup found"); - break; - default: - LOGE("failed to restore almanac backup, status: %d", restore_status); - } - - auto time = util::get_time(); - if (util::time_valid(time)) { - LOGW("Sending current time to ublox"); - send(ublox::build_ubx_mga_ini_time_utc(time)); - } - - LOGW("panda GPS on"); - return; - } - LOGE("failed to initialize panda GPS"); -} - -void Pigeon::stop() { - LOGW("Storing almanac in ublox flash"); - - // Controlled GNSS stop - send("\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74"s); - - // Store almanac in flash - send("\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC"s); - - if (wait_for_ack(sos_save_ack, sos_save_nack)) { - LOGW("Done storing almanac"); - } else { - LOGE("Error storing almanac"); - } -} - -void PandaPigeon::connect(Panda * p) { - panda = p; -} - -void PandaPigeon::set_baud(int baud) { - panda->usb_write(0xe2, 1, 0); - panda->usb_write(0xe4, 1, baud/300); -} - -void PandaPigeon::send(const std::string &s) { - int len = s.length(); - const char * dat = s.data(); - - unsigned char a[0x20+1]; - a[0] = 1; - for (int i=0; iusb_bulk_write(2, a, ll+1); - } -} - -std::string PandaPigeon::receive() { - std::string r; - r.reserve(0x1000 + 0x40); - unsigned char dat[0x40]; - while (r.length() < 0x1000) { - int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat)); - if (len <= 0) break; - r.append((char*)dat, len); - } - - return r; -} - -void PandaPigeon::set_power(bool power) { - panda->usb_write(0xd9, power, 0); -} - -PandaPigeon::~PandaPigeon() { -} - -void handle_tty_issue(int err, const char func[]) { - LOGE_100("tty error %d \"%s\" in %s", err, strerror(err), func); -} - -void TTYPigeon::connect(const char * tty) { - pigeon_tty_fd = HANDLE_EINTR(open(tty, O_RDWR)); - if (pigeon_tty_fd < 0) { - handle_tty_issue(errno, __func__); - assert(pigeon_tty_fd >= 0); - } - int err = tcgetattr(pigeon_tty_fd, &pigeon_tty); - assert(err == 0); - - // configure tty - pigeon_tty.c_cflag &= ~PARENB; // disable parity - pigeon_tty.c_cflag &= ~CSTOPB; // single stop bit - pigeon_tty.c_cflag |= CS8; // 8 bits per byte - pigeon_tty.c_cflag &= ~CRTSCTS; // no RTS/CTS flow control - pigeon_tty.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines - pigeon_tty.c_lflag &= ~ICANON; // disable canonical mode - pigeon_tty.c_lflag &= ~ISIG; // disable interpretation of INTR, QUIT and SUSP - pigeon_tty.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off software flow ctrl - pigeon_tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // disable any special handling of received bytes - pigeon_tty.c_oflag &= ~OPOST; // prevent special interpretation of output bytes - pigeon_tty.c_oflag &= ~ONLCR; // prevent conversion of newline to carriage return/line feed - - // configure blocking behavior - pigeon_tty.c_cc[VMIN] = 0; // min amount of characters returned - pigeon_tty.c_cc[VTIME] = 0; // max blocking time in s/10 (0=inf) - - err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty); - assert(err == 0); -} - -void TTYPigeon::set_baud(int baud) { - speed_t baud_const = 0; - switch(baud) { - case 9600: - baud_const = B9600; - break; - case 460800: - baud_const = B460800; - break; - default: - assert(false); - } - - // make sure everything is tx'ed before changing baud - int err = tcdrain(pigeon_tty_fd); - assert(err == 0); - - // change baud - err = tcgetattr(pigeon_tty_fd, &pigeon_tty); - assert(err == 0); - err = cfsetspeed(&pigeon_tty, baud_const); - assert(err == 0); - err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty); - assert(err == 0); - - // flush - err = tcflush(pigeon_tty_fd, TCIOFLUSH); - assert(err == 0); -} - -void TTYPigeon::send(const std::string &s) { - int err = HANDLE_EINTR(write(pigeon_tty_fd, s.data(), s.length())); - - if(err < 0) { handle_tty_issue(err, __func__); } - err = tcdrain(pigeon_tty_fd); - if(err < 0) { handle_tty_issue(err, __func__); } -} - -std::string TTYPigeon::receive() { - std::string r; - r.reserve(0x1000 + 0x40); - char dat[0x40]; - while (r.length() < 0x1000) { - int len = read(pigeon_tty_fd, dat, sizeof(dat)); - if(len < 0) { - handle_tty_issue(len, __func__); - } else if (len == 0) { - break; - } else { - r.append(dat, len); - } - - } - return r; -} - -void TTYPigeon::set_power(bool power) { -#ifdef QCOM2 - int err = 0; - err += gpio_init(GPIO_UBLOX_RST_N, true); - err += gpio_init(GPIO_UBLOX_SAFEBOOT_N, true); - err += gpio_init(GPIO_UBLOX_PWR_EN, true); - - err += gpio_set(GPIO_UBLOX_RST_N, power); - err += gpio_set(GPIO_UBLOX_SAFEBOOT_N, power); - err += gpio_set(GPIO_UBLOX_PWR_EN, power); - assert(err == 0); -#endif -} - -TTYPigeon::~TTYPigeon() { - close(pigeon_tty_fd); -} diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h deleted file mode 100644 index c9ea4739d..000000000 --- a/selfdrive/boardd/pigeon.h +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include "selfdrive/boardd/panda.h" - -enum sos_restore_response : int { - unknown = 0, - failed = 1, - restored = 2, - no_backup = 3, - error = -1 -}; - -class Pigeon { - public: - static Pigeon* connect(Panda * p); - static Pigeon* connect(const char * tty); - virtual ~Pigeon(){}; - - void init(); - void stop(); - bool wait_for_ack(); - bool wait_for_ack(const std::string &ack, const std::string &nack, int timeout_ms = 1000); - bool send_with_ack(const std::string &cmd); - sos_restore_response wait_for_backup_restore_status(int timeout_ms = 1000); - virtual void set_baud(int baud) = 0; - virtual void send(const std::string &s) = 0; - virtual std::string receive() = 0; - virtual void set_power(bool power) = 0; -}; - -class PandaPigeon : public Pigeon { - Panda * panda = NULL; -public: - ~PandaPigeon(); - void connect(Panda * p); - void set_baud(int baud); - void send(const std::string &s); - std::string receive(); - void set_power(bool power); -}; - - -class TTYPigeon : public Pigeon { - int pigeon_tty_fd = -1; - struct termios pigeon_tty; -public: - ~TTYPigeon(); - void connect(const char* tty); - void set_baud(int baud); - void send(const std::string &s); - std::string receive(); - void set_power(bool power); -}; diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc new file mode 100644 index 000000000..2803f58db --- /dev/null +++ b/selfdrive/boardd/spi.cc @@ -0,0 +1,287 @@ +#include +#include + +#include +#include +#include + +#include "common/util.h" +#include "common/swaglog.h" +#include "panda/board/comms_definitions.h" +#include "selfdrive/boardd/panda_comms.h" + + +#define SPI_SYNC 0x5AU +#define SPI_HACK 0x79U +#define SPI_DACK 0x85U +#define SPI_NACK 0x1FU +#define SPI_CHECKSUM_START 0xABU + +struct __attribute__((packed)) spi_header { + uint8_t sync; + uint8_t endpoint; + uint16_t tx_len; + uint16_t max_rx_len; +}; + + +PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { + LOGD("opening SPI panda: %s", serial.c_str()); + + int err; + uint32_t spi_mode = SPI_MODE_0; + uint32_t spi_speed = 30000000; + uint8_t spi_bits_per_word = 8; + + spi_fd = open(serial.c_str(), O_RDWR); + if (spi_fd < 0) { + LOGE("failed opening SPI device %d", err); + goto fail; + } + + // SPI settings + err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode); + if (err < 0) { + LOGE("failed setting SPI mode %d", err); + goto fail; + } + + err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + if (err < 0) { + LOGE("failed setting SPI speed"); + goto fail; + } + + err = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word); + if (err < 0) { + LOGE("failed setting SPI bits per word"); + goto fail; + } + + return; + +fail: + cleanup(); + throw std::runtime_error("Error connecting to panda"); +} + +PandaSpiHandle::~PandaSpiHandle() { + std::lock_guard lk(hw_lock); + cleanup(); +} + +void PandaSpiHandle::cleanup() { + if (spi_fd != -1) { + close(spi_fd); + spi_fd = -1; + } +} + + + +int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) { + ControlPacket_t packet = { + .request = request, + .param1 = param1, + .param2 = param2, + .length = 0 + }; + return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), NULL, 0); +} + +int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) { + ControlPacket_t packet = { + .request = request, + .param1 = param1, + .param2 = param2, + .length = length + }; + return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), data, length); +} + +int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + return bulk_transfer(endpoint, data, length, NULL, 0); +} +int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + return bulk_transfer(endpoint, NULL, 0, data, length); +} + +int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len) { + std::lock_guard lk(hw_lock); + + const int xfer_size = 0x40; + + int ret = 0; + uint16_t length = (tx_data != NULL) ? tx_len : rx_len; + for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) { + int d; + if (tx_data != NULL) { + int len = std::min(xfer_size, tx_len - (xfer_size * i)); + d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0); + } else { + d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), xfer_size); + } + + if (d < 0) { + LOGE("SPI: bulk transfer failed with %d", d); + comms_healthy = false; + return -1; + } + + ret += d; + if ((rx_data != NULL) && d < xfer_size) { + break; + } + } + + return ret; +} + + + +std::vector PandaSpiHandle::list() { + // TODO: list all pandas available over SPI + return {}; +} + + + +void add_checksum(uint8_t *data, int data_len) { + data[data_len] = SPI_CHECKSUM_START; + for (int i=0; i < data_len; i++) { + data[data_len] ^= data[i]; + } +} + +bool check_checksum(uint8_t *data, int data_len) { + uint8_t checksum = SPI_CHECKSUM_START; + for (uint16_t i = 0U; i < data_len; i++) { + checksum ^= data[i]; + } + return checksum == 0U; +} + + +int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len) { + int ret; + + std::lock_guard lk(hw_lock); + do { + // TODO: handle error + ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len); + } while (ret < 0 && connected && !PANDA_NO_RETRY); + + return ret; +} + +int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) { + // TODO: add timeout? + while (true) { + int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + if (ret < 0) { + LOGE("SPI: failed to send ACK request"); + return ret; + } + + if (rx_buf[0] == ack) { + break; + } else if (rx_buf[0] == SPI_NACK) { + LOGW("SPI: got NACK"); + return -1; + } + } + + return 0; +} + +int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len) { + int ret; + uint16_t rx_data_len; + + // needs to be less, since we need to have space for the checksum + assert(tx_len < SPI_BUF_SIZE); + assert(max_rx_len < SPI_BUF_SIZE); + + spi_header header = { + .sync = SPI_SYNC, + .endpoint = endpoint, + .tx_len = tx_len, + .max_rx_len = max_rx_len + }; + + spi_ioc_transfer transfer = { + .tx_buf = (uint64_t)tx_buf, + .rx_buf = (uint64_t)rx_buf + }; + + // Send header + memcpy(tx_buf, &header, sizeof(header)); + add_checksum(tx_buf, sizeof(header)); + transfer.len = sizeof(header) + 1; + ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + if (ret < 0) { + LOGE("SPI: failed to send header"); + goto transfer_fail; + } + + // Wait for (N)ACK + tx_buf[0] = 0x12; + transfer.len = 1; + ret = wait_for_ack(transfer, SPI_HACK); + if (ret < 0) { + goto transfer_fail; + } + + // Send data + if (tx_data != NULL) { + memcpy(tx_buf, tx_data, tx_len); + } + add_checksum(tx_buf, tx_len); + transfer.len = tx_len + 1; + ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + if (ret < 0) { + LOGE("SPI: failed to send data"); + goto transfer_fail; + } + + // Wait for (N)ACK + tx_buf[0] = 0xab; + transfer.len = 1; + ret = wait_for_ack(transfer, SPI_DACK); + if (ret < 0) { + goto transfer_fail; + } + + // Read data len + transfer.len = 2; + transfer.rx_buf = (uint64_t)(rx_buf + 1); + ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + if (ret < 0) { + LOGE("SPI: failed to read rx data len"); + goto transfer_fail; + } + rx_data_len = *(uint16_t *)(rx_buf+1); + assert(rx_data_len < SPI_BUF_SIZE); + + // Read data + transfer.len = rx_data_len + 1; + transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1); + ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + if (ret < 0) { + LOGE("SPI: failed to read rx data"); + goto transfer_fail; + } + if (!check_checksum(rx_buf, rx_data_len + 4)) { + LOGE("SPI: bad checksum"); + goto transfer_fail; + } + + if (rx_data != NULL) { + memcpy(rx_data, rx_buf + 3, rx_data_len); + } + + return rx_data_len; + +transfer_fail: + return ret; +} diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py new file mode 100755 index 000000000..e9bbcb458 --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +import os +import random +import time +import unittest +from collections import defaultdict + +import cereal.messaging as messaging +from cereal import car +from common.params import Params +from common.spinner import Spinner +from common.timeout import Timeout +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car import make_can_msg +from system.hardware import TICI +from selfdrive.test.helpers import phone_only, with_processes + + +class TestBoardd(unittest.TestCase): + + @classmethod + def setUpClass(cls): + os.environ['STARTED'] = '1' + os.environ['BOARDD_LOOPBACK'] = '1' + cls.spinner = Spinner() + + @classmethod + def tearDownClass(cls): + cls.spinner.close() + + @phone_only + @with_processes(['pandad']) + def test_loopback(self): + # wait for boardd to init + time.sleep(2) + + with Timeout(60, "boardd didn't start"): + sm = messaging.SubMaster(['pandaStates']) + while sm.rcv_frame['pandaStates'] < 1 and len(sm['pandaStates']) == 0: + sm.update(1000) + + num_pandas = len(sm['pandaStates']) + if TICI: + self.assertGreater(num_pandas, 1, "connect another panda for multipanda tests") + + # boardd blocks on CarVin and CarParams + 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("CarVin", b"0"*17) + 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) + + n = 200 + for i in range(n): + self.spinner.update(f"boardd loopback {i}/{n}") + + sent_msgs = defaultdict(set) + for _ in range(random.randrange(10)): + to_send = [] + for __ in range(random.randrange(100)): + bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3]) + addr = random.randrange(1, 1<<29) + dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9))) + sent_msgs[bus].add((addr, dat)) + to_send.append(make_can_msg(addr, dat, bus)) + sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) + + for _ in range(100 * 2): + recvd = messaging.drain_sock(can, wait_for_one=True) + for msg in recvd: + for m in msg.can: + if m.src >= 128: + key = (m.address, m.dat) + assert key in sent_msgs[m.src-128], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}" + sent_msgs[m.src-128].discard(key) + + if all(len(v) == 0 for v in sent_msgs.values()): + break + + # if a set isn't empty, messages got dropped + for bus in sent_msgs.keys(): + assert not len(sent_msgs[bus]), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages" + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index f2d198338..491c551b1 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -3,7 +3,7 @@ import capnp from cereal import car from common.numpy_fast import clip -from typing import Dict, List +from typing import Dict # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. @@ -12,6 +12,14 @@ ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float: + if val > val_steady + hyst_gap: + val_steady = val - hyst_gap + elif val < val_steady - hyst_gap: + val_steady = val + hyst_gap + return val_steady + + def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule], unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder: if cur_but != unpressed: @@ -24,19 +32,6 @@ def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, cap return be -def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilder, pcm_cruise: bool = False) -> List[int]: - events = [] - for b in buttonEvents: - # do enable on both accel and decel buttons - if not pcm_cruise: - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: - events.append(EventName.buttonEnable) - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.append(EventName.buttonCancel) - return events - - def gen_empty_fingerprint(): return {i: {} for i in range(0, 8)} diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index e85735b1a..ae7ab89aa 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(candidate, fingerprint=None, car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=None, car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index 61b72d5ad..66f1b947a 100644 --- a/selfdrive/car/body/values.py +++ b/selfdrive/car/body/values.py @@ -3,10 +3,13 @@ from typing import Dict from cereal import car from selfdrive.car import dbc_dict from selfdrive.car.docs_definitions import CarInfo +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + Ecu = car.CarParams.Ecu SPEED_FROM_RPM = 0.008587 + class CarControllerParams: ANGLE_DELTA_BP = [0., 5., 15.] ANGLE_DELTA_V = [5., .8, .15] # windup limit @@ -14,13 +17,25 @@ class CarControllerParams: LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower STEER_THRESHOLD = 1.0 + class CAR: BODY = "COMMA BODY" + CAR_INFO: Dict[str, CarInfo] = { CAR.BODY: CarInfo("comma body", package="All"), } +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], + bus=0, + ), + ], +) + FW_VERSIONS = { CAR.BODY: { (Ecu.engine, 0x720, None): [ diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index aa4b37d57..61e7a3d55 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -76,13 +76,13 @@ interfaces = load_interfaces(interface_names) # **** for use live only **** -def fingerprint(logcan, sendcan): +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() - if not fixed_fingerprint and not skip_fw_query: - # Vin query only reliably works thorugh OBDII + if not skip_fw_query: + # Vin query only reliably works through OBDII bus = 1 cached_params = Params().get("CarParamsCache") @@ -95,16 +95,19 @@ def fingerprint(logcan, sendcan): cloudlog.warning("Using cached CarParams") vin, vin_rx_addr = cached_params.carVin, 0 car_fw = list(cached_params.carFw) + cached = True else: cloudlog.warning("Getting VIN & FW versions") - _, vin_rx_addr, vin = get_vin(logcan, sendcan, bus) + vin_rx_addr, vin = get_vin(logcan, sendcan, bus) ecu_rx_addrs = get_present_ecus(logcan, sendcan) - car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs) + car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas) + cached = False exact_fw_match, fw_candidates = match_fw_to_car(car_fw) else: vin, vin_rx_addr = VIN_UNKNOWN, 0 exact_fw_match, fw_candidates, car_fw = True, set(), [] + cached = False if not is_valid_vin(vin): cloudlog.event("Malformed VIN", vin=vin, error=True) @@ -165,22 +168,22 @@ def fingerprint(logcan, sendcan): car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed - cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, + cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached, fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True) return car_fingerprint, finger, vin, car_fw, source, exact_match -def get_car(logcan, sendcan): - candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan) +def get_car(logcan, sendcan, 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) candidate = "mock" - disable_radar = Params().get_bool("DisableRadar") + experimental_long = Params().get_bool("ExperimentalLongitudinalEnabled") CarInterface, CarController, CarState = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, disable_radar) + CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index fefd351a2..0f0d30782 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -58,8 +58,8 @@ class CarState(CarStateBase): ) # button presses - ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1 - ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2 + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1, + cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2) ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1 # steering wheel diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 9bb22b607..245e10650 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "chrysler" @@ -60,7 +60,7 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 14.5 if car_fw is not None: for fw in car_fw: - if fw.ecu == 'eps' and fw.fwVersion in (b"68312176AE", b"68312176AG", b"68273275AG"): + if fw.ecu == 'eps' and fw.fwVersion[:8] in (b"68312176", b"68273275"): ret.minSteerSpeed = 0. elif candidate == CAR.RAM_HD: diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 10478efa8..16530ed98 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -3,8 +3,11 @@ 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.fw_query_definitions import FwQueryConfig, Request, p16 + Ecu = car.CarParams.Ecu @@ -49,7 +52,7 @@ RAM_CARS = RAM_DT | RAM_HD @dataclass class ChryslerCarInfo(CarInfo): - package: str = "Adaptive Cruise Control" + package: str = "Adaptive Cruise Control (ACC)" harness: Enum = Harness.fca CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { @@ -129,6 +132,46 @@ FINGERPRINTS = { }], } +CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf132) +CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(0xf132) + +CHRYSLER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER) +CHRYSLER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER) + +CHRYSLER_RX_OFFSET = -0x280 + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [CHRYSLER_VERSION_REQUEST], + [CHRYSLER_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.srs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.combinationMeter], + rx_offset=CHRYSLER_RX_OFFSET, + bus=0, + ), + Request( + [CHRYSLER_VERSION_REQUEST], + [CHRYSLER_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.hcp, Ecu.engine, Ecu.transmission], + bus=0, + ), + Request( + [CHRYSLER_SOFTWARE_VERSION_REQUEST], + [CHRYSLER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine, Ecu.transmission], + bus=0, + ), + ], + extra_ecus=[ + (Ecu.hcp, 0x7e2, None), # manages transmission on hybrids + (Ecu.abs, 0x7e4, None), # alt address for abs on hybrids + ], +) + FW_VERSIONS = { CAR.RAM_1500: { (Ecu.combinationMeter, 0x742, None): [ @@ -144,7 +187,7 @@ FW_VERSIONS = { b'68428609AB', b'68500728AA', ], - (Ecu.esp, 0x747, None): [ + (Ecu.abs, 0x747, None): [ b'68432418AD', b'68432418AB', b'68436004AE', @@ -170,6 +213,7 @@ FW_VERSIONS = { b'68448163AJ', b'68500630AD', b'68539650AD', + b'68378758AM ', ], (Ecu.transmission, 0x7e1, None): [ b'68360078AL', @@ -181,12 +225,6 @@ FW_VERSIONS = { b'68540431AB', b'68484467AC', ], - (Ecu.gateway, 0x18DACBF1, None): [ - b'68402660AB', - b'68445283AB', - b'68533631AB', - b'68500483AB', - ], }, CAR.RAM_HD: { @@ -199,10 +237,11 @@ FW_VERSIONS = { b'68428503AA', b'68428505AA', ], - (Ecu.esp, 0x747, None): [ + (Ecu.abs, 0x747, None): [ b'68334977AH', b'68504022AB', b'68530686AB', + b'68504022AC', ], (Ecu.fwdRadar, 0x753, None): [ b'04672895AB', @@ -218,10 +257,6 @@ FW_VERSIONS = { b'M2370131MB', b'M2421132MB', ], - (Ecu.gateway, 0x18DACBF1, None): [ - b'68488419AB', - b'68535476AB', - ], }, } diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 52d6388d7..8e057a156 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -10,10 +10,6 @@ from common.conversions import Conversions as CV GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2 MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)" -# Makes that lack auto-resume with stock long, and auto resume in any configuration -NO_AUTO_RESUME_STOCK_LONG = {"toyota", "gm"} -NO_AUTO_RESUME = NO_AUTO_RESUME_STOCK_LONG | {"nissan", "subaru"} - class Column(Enum): MAKE = "Make" @@ -23,6 +19,7 @@ class Column(Enum): FSR_LONGITUDINAL = "No ACC accel below" FSR_STEERING = "No ALC below" STEERING_TORQUE = "Steering Torque" + AUTO_RESUME = "Resume from stop" HARNESS = "Harness" @@ -71,7 +68,18 @@ class Harness(Enum): none = "None" -CarFootnote = namedtuple("CarFootnote", ["text", "column"], defaults=[None]) +CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only"], defaults=(None, False)) + + +class CommonFootnote(Enum): + EXP_LONG_AVAIL = CarFootnote( + "Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `master-ci`. " + + "Using openpilot longitudinal may disable Automatic Emergency Braking (AEB).", + Column.LONGITUDINAL, docs_only=True) + EXP_LONG_DSU = CarFootnote( + "When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace " + + "stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).", + Column.LONGITUDINAL) def get_footnotes(footnotes: List[Enum], column: Column) -> List[Enum]: @@ -131,14 +139,26 @@ class CarInfo: self.car_name = CP.carName self.car_fingerprint = CP.carFingerprint self.make, self.model, self.years = split_name(self.name) + + op_long = "Stock" + if CP.openpilotLongitudinalControl and not CP.enableDsu: + op_long = "openpilot" + elif CP.experimentalLongitudinalAvailable or CP.enableDsu: + op_long = "openpilot available" + if CP.enableDsu: + self.footnotes.append(CommonFootnote.EXP_LONG_DSU) + else: + self.footnotes.append(CommonFootnote.EXP_LONG_AVAIL) + self.row = { Column.MAKE: self.make, Column.MODEL: self.model, Column.PACKAGE: self.package, - Column.LONGITUDINAL: "openpilot" if CP.openpilotLongitudinalControl and not CP.radarOffCan else "Stock", + Column.LONGITUDINAL: op_long, Column.FSR_LONGITUDINAL: f"{max(self.min_enable_speed * CV.MS_TO_MPH, 0):.0f} mph", Column.FSR_STEERING: f"{max(self.min_steer_speed * CV.MS_TO_MPH, 0):.0f} mph", Column.STEERING_TORQUE: Star.EMPTY, + Column.AUTO_RESUME: Star.FULL if CP.autoResumeSng else Star.EMPTY, Column.HARNESS: self.harness.value, } @@ -166,7 +186,7 @@ class CarInfo: acc = "" if self.min_enable_speed > 0: acc = f" while driving above {self.min_enable_speed * CV.MS_TO_MPH:.0f} mph" - elif CP.carName not in NO_AUTO_RESUME or (CP.carName in NO_AUTO_RESUME_STOCK_LONG and CP.openpilotLongitudinalControl): + elif CP.autoResumeSng: acc = " that automatically resumes from a stop" if self.row[Column.STEERING_TORQUE] != Star.FULL: @@ -193,4 +213,3 @@ class CarInfo: item += footnote_tag.format(f'{",".join(map(str, sups))}') return item - diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index 267701509..9f6ace2b5 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import capnp import time -import traceback from typing import Optional, Set, Tuple import cereal.messaging as messaging @@ -62,7 +61,7 @@ def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, que print(f"Duplicate ECU address: {hex(msg.address)}") ecu_responses.add((msg.address, subaddr, msg.src)) except Exception: - cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}") + cloudlog.exception("ECU addr scan exception") return ecu_responses @@ -71,6 +70,8 @@ if __name__ == "__main__": parser = argparse.ArgumentParser(description='Get addresses of all ECUs') parser.add_argument('--debug', action='store_true') + parser.add_argument('--bus', type=int, default=1) + parser.add_argument('--timeout', type=float, default=1.0) args = parser.parse_args() logcan = messaging.sub_sock('can') @@ -79,7 +80,7 @@ if __name__ == "__main__": time.sleep(1.0) print("Getting ECU addresses ...") - ecu_addrs = get_all_ecu_addrs(logcan, sendcan, 1, debug=args.debug) + ecu_addrs = get_all_ecu_addrs(logcan, sendcan, args.bus, args.timeout, debug=args.debug) print() print("Found ECUs on addresses:") diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 11d9bb3c7..f18014601 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,20 +1,26 @@ +import math from cereal import car from common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker from selfdrive.car.ford import fordcan -from selfdrive.car.ford.values import CarControllerParams +from selfdrive.car.ford.values import CANBUS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo): +def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo): # rate limit - steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last) - rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN + steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) + rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff)) + apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) - return apply_steer + # absolute limit (LatCtlPath_An_Actl) + apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO + apply_path_angle = clip(apply_path_angle, -0.5, 0.5235) + apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO + + return apply_angle class CarController: @@ -24,7 +30,7 @@ class CarController: self.packer = CANPacker(dbc_name) self.frame = 0 - self.apply_steer_last = 0 + self.apply_angle_last = 0 self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False @@ -38,52 +44,68 @@ class CarController: main_on = CS.out.cruiseState.available steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + ### acc buttons ### if CC.cruiseControl.cancel: - # cancel stock ACC - can_sends.append(fordcan.spam_cancel_button(self.packer)) + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True)) + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main)) + elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True)) + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main)) + # if stock lane centering isn't off, send a button press to toggle it off + # the stock system checks for steering pressed, and eventually disengages cruise control + elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0: + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True)) - # apply rate limits - new_steer = actuators.steeringAngleDeg - apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) + + ### lateral control ### + if CC.latActive: + lca_rq = 1 + apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo) + else: + lca_rq = 0 + apply_angle = 0. # send steering commands at 20Hz if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: - lca_rq = 1 if CC.latActive else 0 + # use LatCtlPath_An_Actl to actuate steering + path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO - # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented - path_angle = apply_steer + # set slower ramp type when small steering angle change + # 0=Slow, 1=Medium, 2=Fast, 3=Immediately + steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg) + if steer_change < 2.0: + ramp_type = 0 + elif steer_change < 4.0: + ramp_type = 1 + elif steer_change < 6.0: + ramp_type = 2 + else: + ramp_type = 3 + precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) - # convert steer angle to curvature - curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0) - - # TODO: get other actuators - curvature_rate = 0 - path_offset = 0 - - ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately - precision = 0 # 0=Comfortable, 1=Precise - - self.apply_steer_last = apply_steer - can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature)) + self.apply_angle_last = apply_angle + can_sends.append(fordcan.create_lka_command(self.packer, 0, 0)) can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, - path_offset, path_angle, curvature_rate, curvature)) + 0, path_angle, 0, 0)) + + ### 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 if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) + can_sends.append(fordcan.create_lkas_ui_command(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 if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values)) + can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive self.steer_alert_last = steer_alert new_actuators = actuators.copy() - new_actuators.steeringAngleDeg = apply_steer + new_actuators.steeringAngleDeg = self.apply_angle_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index aaa3af612..2276b1208 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -3,7 +3,7 @@ from common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.ford.values import CANBUS, DBC +from selfdrive.car.ford.values import CANBUS, DBC, CarControllerParams GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType @@ -20,9 +20,9 @@ class CarState(CarStateBase): ret = car.CarState.new_message() # car speed - ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS + ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG + ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 # gas pedal @@ -37,7 +37,7 @@ class CarState(CarStateBase): # steering wheel ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] - ret.steeringPressed = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0 + ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE 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 @@ -46,6 +46,8 @@ class CarState(CarStateBase): ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5) 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 # gear if self.CP.transmissionType == TransmissionType.automatic: @@ -65,6 +67,7 @@ class CarState(CarStateBase): # button presses ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 + # TODO: block this going to the camera otherwise it will enable stock TJA ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) # lock info @@ -77,6 +80,8 @@ class CarState(CarStateBase): ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 + # Stock steering buttons so that we can passthru blinkers etc. + self.buttons_stock_values = cp.vl["Steering_Data_FD1"] # Stock values from IPMA so that we can retain some stock functionality self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] @@ -87,7 +92,7 @@ class CarState(CarStateBase): def get_can_parser(CP): signals = [ # sig_name, sig_address - ("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph) + ("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 ("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status @@ -97,6 +102,8 @@ class CarState(CarStateBase): ("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) # The units might change with IPC settings? ("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status + ("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) # Calculates steering angle (and offset) from pinion # angle and driving measurements. @@ -104,7 +111,6 @@ class CarState(CarStateBase): # to zero at the beginning of the drive. ("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm) ("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status - ("LaHandsOff_B_Actl", "Lane_Assist_Data3_FD1"), # PSCM LKAS hands off wheel ("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch ("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle ("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver @@ -112,16 +118,49 @@ class CarState(CarStateBase): ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right ("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver + ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons + ("WiprFront_D_Stat", "Steering_Data_FD1"), + ("LghtAmb_D_Sns", "Steering_Data_FD1"), + ("AccButtnGapDecPress", "Steering_Data_FD1"), + ("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"), + ("CcAslButtnIndxDecPress", "Steering_Data_FD1"), + ("CcAslButtnIndxIncPress", "Steering_Data_FD1"), + ("CcAslButtnOffCnclPress", "Steering_Data_FD1"), + ("CcAslButtnOnOffCncl", "Steering_Data_FD1"), + ("CcAslButtnOnPress", "Steering_Data_FD1"), + ("CcAslButtnResDecPress", "Steering_Data_FD1"), + ("CcAslButtnResIncPress", "Steering_Data_FD1"), + ("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"), + ("CcButtnOnPress", "Steering_Data_FD1"), + ("HeadLghtHiFlash_D_Actl", "Steering_Data_FD1"), + ("HeadLghtHiOn_B_StatAhb", "Steering_Data_FD1"), + ("AhbStat_B_Dsply", "Steering_Data_FD1"), + ("AccButtnGapTogglePress", "Steering_Data_FD1"), + ("WiprFrontSwtch_D_Stat", "Steering_Data_FD1"), + ("HeadLghtHiCtrl_D_RqAhb", "Steering_Data_FD1"), ] checks = [ # sig_address, frequency - ("EngVehicleSpThrottle2", 50), + ("BrakeSysFeatures", 50), ("Yaw_Data_FD1", 100), ("DesiredTorqBrk", 50), ("EngVehicleSpThrottle", 100), ("BrakeSnData_4", 50), ("EngBrakeData", 10), + ("Cluster_Info1_FD1", 10), ("SteeringPinion_Data", 100), ("EPAS_INFO", 50), ("Lane_Assist_Data3_FD1", 33), @@ -203,7 +242,8 @@ class CarState(CarStateBase): ("DasStats_D_Dsply", "IPMA_Data"), # DAS status ("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning ("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status - ("Set_Me_X1", "IPMA_Data"), + ("Passthru_63", "IPMA_Data"), + ("Passthru_48", "IPMA_Data"), ] checks = [ diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 29fec1f2d..373ce096c 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -1,12 +1,14 @@ -from common.numpy_fast import clip +from cereal import car +from selfdrive.car.ford.values import CANBUS + +HUDControl = car.CarControl.HUDControl -def create_lkas_command(packer, angle_deg: float, curvature: float): +def create_lka_command(packer, angle_deg: float, curvature: float): """ Creates a CAN message for the Ford LKAS Command. - This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the - PSCM lockout. + This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout. Frequency is 20Hz. """ @@ -20,38 +22,46 @@ def create_lkas_command(packer, angle_deg: float, curvature: float): "LdwActvStats_D_Req": 0, # LDW status [0|7] "LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength } - return packer.make_can_msg("Lane_Assist_Data1", 0, values) + return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, values) def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float): """ Creates a CAN message for the Ford TJA/LCA Command. - This command can apply "Lane Centering" manoeuvres: continuous lane centering - for traffic jam assist and highway driving. It is not subject to the PSCM - lockout. + This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam + assist and highway driving. It is not subject to the PSCM lockout. - The PSCM should be configured to accept TJA/LCA commands before these - commands will be processed. This can be done using tools such as Forscan. + Ford lane centering command uses a third order polynomial to describe the road centerline. The + polynomial is defined by the following coefficients: + c0: lateral offset between the vehicle and the centerline + c1: heading angle between the vehicle and the centerline + c2: curvature of the centerline + c3: rate of change of curvature of the centerline + As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and + speed, the steering angle cannot be easily controlled. + + The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. + This can be done using tools such as Forscan. Frequency is 20Hz. """ values = { - "LatCtlRng_L_Max": 0, # Unknown [0|126] meter - "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] - "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] - "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] - "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] - "LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11), # Path offset [-5.12|5.11] meter - "LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235), # Path angle [-0.5|0.5235] radians - "LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375), # Curvature rate [-0.001024|0.00102375] 1/meter^2 - "LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094), # Curvature [-0.02|0.02094] 1/meter + "LatCtlRng_L_Max": 0, # Unknown [0|126] meter + "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] + "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter } - return packer.make_can_msg("LateralMotionControl", 0, values) + return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) -def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values): +def create_lkas_ui_command(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. @@ -63,75 +73,82 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo """ # LaActvStats_D_Dsply - # TODO: get LDW state from OP + # R Intvn Warn Supprs Avail No + # L + # Intvn 24 19 14 9 4 + # Warn 23 18 13 8 3 + # Supprs 22 17 12 7 2 + # Avail 21 16 11 6 1 + # No 20 15 10 5 0 + # + # TODO: test suppress state if enabled: - lines = 6 + lines = 0 # NoLeft_NoRight + if hud_control.leftLaneDepart: + lines += 4 + elif hud_control.leftLaneVisible: + lines += 1 + if hud_control.rightLaneDepart: + lines += 20 + elif hud_control.rightLaneVisible: + lines += 5 elif main_on: lines = 0 else: - lines = 30 + if hud_control.leftLaneDepart: + lines = 3 # WarnLeft_NoRight + elif hud_control.rightLaneDepart: + lines = 15 # NoLeft_WarnRight + 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 values = { - "FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535] - "FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535] - "PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7] - "AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3] - "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] - "LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"], # LKAS error [0|1] - "LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed - "CamraDefog_B_Req": stock_values["CamraDefog_B_Req"], # Windshield heater? [0|1] - "CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"], # Camera status [0|3] - "DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"], # DAS alert level [0|7] - "DasStats_D_Dsply": stock_values["DasStats_D_Dsply"], # DAS status [0|3] - "DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"], # DAS warning [0|3] - "AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"], # AHB status [0|3] - "Set_Me_X1": stock_values["Set_Me_X1"], # [0|15] + **stock_values, + "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", 0, values) + return packer.make_can_msg("IPMA_Data", CANBUS.main, values) -def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values): +def create_acc_ui_command(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. + 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 = { - "HaDsply_No_Cs": stock_values["HaDsply_No_Cs"], # [0|255] - "HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"], # [0|15] - "AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"], # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3] - "AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"], # ACC target distance [0|15] - "AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"], # [0|1] - "TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"], # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7] - "Tja_D_Stat": 2 if enabled else (1 if main_on else 0), # TJA status: 0=Off, 1=Standby, 2=Active, 3=InterventionLeft, 4=InterventionRight, 5=WarningLeft, 6=WarningRight, 7=NotUsed [0|7] - "TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"], # TJA text [0|7] - "IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"], # iACC status icon [0|3] - "AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"], # ACC text [0|15] - "FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"], # FCW disabled [0|1] - "FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"], # FCW enabled setting [0|1] - "AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"], # ACC time gap display setting [0|1] - "CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"], # Radar alignment? [0|1] - "AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"], # ACC follow mode display setting [0|1] - "CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"], # Radar blocked? [0|1] - "CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"], # AEB event status [0|1] - "AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"], # ACC stop mode display setting [0|1] - "FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"], # FCW sensitivity setting [0|3] - "FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"], # FCW text [0|7] - "AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"], # ACC warning [0|3] - "FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"], # FCW alert: 0=Off, 1=On [0|1] - "FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"], # FCW audio: 0=Off, 1=On [0|1] - "AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"], # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7] - "AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"], # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1] - "FdaMem_B_Stat": stock_values["FdaMem_B_Stat"], # FDA enabled setting [0|1] + **stock_values, + "Tja_D_Stat": status, } - return packer.make_can_msg("ACCDATA_3", 0, values) + return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) -def spam_cancel_button(packer, cancel=1): +def create_button_command(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. @@ -139,6 +156,9 @@ def spam_cancel_button(packer, cancel=1): """ values = { - "CcAslButtnCnclPress": cancel, # CC cancel button + **stock_values, + "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 } - return packer.make_can_msg("Steering_Data_FD1", 0, values) + 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 405579fa6..4943db076 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,70 +1,69 @@ #!/usr/bin/env python3 from cereal import car from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint -from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter from selfdrive.car.interfaces import CarInterfaceBase - -EventName = car.CarEvent.EventName +CarParams = car.CarParams class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + if car_fw is None: + car_fw = [] + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "ford" - #ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] ret.dashcamOnly = True + ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] # Angle-based steering - # TODO: use curvature control when ready - ret.steerControlType = car.CarParams.SteerControlType.angle - ret.steerActuatorDelay = 0.1 + ret.steerControlType = CarParams.SteerControlType.angle + ret.steerActuatorDelay = 0.4 ret.steerLimitTimer = 1.0 - - # TODO: detect stop-and-go vehicles - stop_and_go = False + tire_stiffness_factor = 1.0 if candidate == CAR.ESCAPE_MK4: ret.wheelbase = 2.71 ret.steerRatio = 14.3 # Copied from Focus - tire_stiffness_factor = 0.5328 # Copied from Focus ret.mass = 1750 + STD_CARGO_KG + elif candidate == CAR.EXPLORER_MK6: + ret.wheelbase = 3.025 + ret.steerRatio = 16.8 # learned + ret.mass = 2050 + STD_CARGO_KG + elif candidate == CAR.FOCUS_MK4: ret.wheelbase = 2.7 - ret.steerRatio = 14.3 - tire_stiffness_factor = 0.5328 + ret.steerRatio = 13.8 # learned ret.mass = 1350 + STD_CARGO_KG else: - raise ValueError(f"Unsupported car: ${candidate}") + raise ValueError(f"Unsupported car: {candidate}") - # Auto Transmission: Gear_Shift_by_Wire_FD1 - # TODO: detect transmission in car_fw? - if 0x5A in fingerprint[0]: + # 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]: ret.transmissionType = TransmissionType.automatic else: ret.transmissionType = TransmissionType.manual + ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS # BSM: Side_Detect_L_Stat, Side_Detect_R_Stat # TODO: detect bsm in car_fw? ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0] - # min speed to enable ACC. if car can do stop and go, then set enabling speed - # to a negative value, so it won't matter. - ret.minEnableSpeed = -1. if (stop_and_go) else 20. * CV.MPH_TO_MS # LCA can steer down to zero ret.minSteerSpeed = 0. - ret.centerToFront = ret.wheelbase * 0.44 - + ret.autoResumeSng = ret.minEnableSpeed == -1. ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.centerToFront = ret.wheelbase * 0.44 ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - return ret def _update(self, c): diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 825d515da..5114f8d06 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,9 +1,12 @@ -from collections import namedtuple +from collections import defaultdict, namedtuple +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 +from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu TransmissionType = car.CarParams.TransmissionType @@ -19,14 +22,15 @@ class CarControllerParams: 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_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) - STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + LKAS_STEER_RATIO = 2.75 # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians + # TODO: remove this once we understand how the EPS calculates the steering angle better + STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold in Nm - -class RADAR: - DELPHI_ESR = 'ford_fusion_2018_adas' - DELPHI_MRR = 'FORD_CADS' + RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) + RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) class CANBUS: @@ -37,38 +41,111 @@ class CANBUS: class CAR: ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" + EXPLORER_MK6 = "FORD EXPLORER 6TH GEN" FOCUS_MK4 = "FORD FOCUS 4TH GEN" +class RADAR: + DELPHI_ESR = 'ford_fusion_2018_adas' + DELPHI_MRR = 'FORD_CADS' + + +DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR)) + + +@dataclass +class FordCarInfo(CarInfo): + package: str = "Co-Pilot360 Assist+" + harness: Enum = Harness.ford_q3 + + CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { - CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"), - CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"), + CAR.ESCAPE_MK4: [ + FordCarInfo("Ford Escape 2020-21"), + FordCarInfo("Ford Kuga 2020-21", "Driver Assistance Pack"), + ], + CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-22"), + CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), } +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine], + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + bus=0, + whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire], + ), + ], +) FW_VERSIONS = { CAR.ESCAPE_MK4: { (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', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7E0, None): [ + 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', + ], + (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', + ], + }, + CAR.EXPLORER_MK6: { + (Ecu.eps, 0x730, None): [ + 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-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', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + 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', + ], + (Ecu.engine, 0x7E0, None): [ + 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'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + b'L1MP-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MP-14G395-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MP-14G395-JB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.FOCUS_MK4: { (Ecu.eps, 0x730, None): [ b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ @@ -80,11 +157,7 @@ FW_VERSIONS = { (Ecu.engine, 0x7E0, None): [ b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], + (Ecu.shiftByWire, 0x732, None): [ + ], }, } - - -DBC = { - CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), - CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), -} diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py new file mode 100755 index 000000000..c7e4d4eb3 --- /dev/null +++ b/selfdrive/car/fw_query_definitions.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +import capnp +from dataclasses import dataclass, field +import struct +from typing import Dict, List, Optional, Tuple + +import panda.python.uds as uds + + +def p16(val): + return struct.pack("!H", val) + + +class StdQueries: + # FW queries + TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0]) + TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0]) + + SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT]) + SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40]) + + DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, + uds.SESSION_TYPE.DEFAULT]) + DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, + uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4]) + + EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, + uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC]) + EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, + uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4]) + + MANUFACTURER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + + UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + + OBD_VERSION_REQUEST = b'\x09\x04' + OBD_VERSION_RESPONSE = b'\x49\x04' + + # VIN queries + OBD_VIN_REQUEST = b'\x09\x02' + OBD_VIN_RESPONSE = b'\x49\x02\x01' + + UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN) + UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN) + + +@dataclass +class Request: + request: List[bytes] + response: List[bytes] + whitelist_ecus: List[int] = field(default_factory=list) + rx_offset: int = 0x8 + bus: int = 1 + + +@dataclass +class FwQueryConfig: + requests: List[Request] + # Overrides and removes from essential ecus for specific models and ecus (exact matching) + 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) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 9d2fd11b4..f4d92ab96 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,9 +1,6 @@ #!/usr/bin/env python3 -import struct -import traceback from collections import defaultdict -from dataclasses import dataclass, field -from typing import Any, List, Optional, Set, Tuple +from typing import Any, Optional, Set, Tuple from tqdm import tqdm import panda.python.uds as uds @@ -12,224 +9,16 @@ from selfdrive.car.ecu_addrs import 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 -from selfdrive.car.toyota.values import CAR as TOYOTA from system.swaglog import cloudlog Ecu = car.CarParams.Ecu -ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] +ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] +FW_QUERY_CONFIGS = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True) +VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True) -def p16(val): - return struct.pack("!H", val) - - -TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0]) -TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0]) - -SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT]) -SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40]) - -DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, - uds.SESSION_TYPE.DEFAULT]) -DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, - uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4]) - -EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, - uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC]) -EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, - uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4]) - -UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) -UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) - - -HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(0xf100) # 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]) - - -TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01' -TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01' - -VOLKSWAGEN_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.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) -VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) - -OBD_VERSION_REQUEST = b'\x09\x04' -OBD_VERSION_RESPONSE = b'\x49\x04' - -DEFAULT_RX_OFFSET = 0x8 -VOLKSWAGEN_RX_OFFSET = 0x6a - -MAZDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) -MAZDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) - -NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xc0]) -NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xc0]) - -NISSAN_VERSION_REQUEST_KWP = b'\x21\x83' -NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83' - -NISSAN_VERSION_REQUEST_STANDARD = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) -NISSAN_VERSION_RESPONSE_STANDARD = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) - -NISSAN_RX_OFFSET = 0x20 - -SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) -SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) - -CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(0xf132) -CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(0xf132) - -CHRYSLER_RX_OFFSET = -0x280 - -FORD_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) -FORD_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) - - -@dataclass -class Request: - brand: str - request: List[bytes] - response: List[bytes] - whitelist_ecus: List[int] = field(default_factory=list) - rx_offset: int = DEFAULT_RX_OFFSET - bus: int = 1 - - -REQUESTS: List[Request] = [ - # Subaru - Request( - "subaru", - [TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], - ), - # Hyundai - Request( - "hyundai", - [HYUNDAI_VERSION_REQUEST_LONG], - [HYUNDAI_VERSION_RESPONSE], - ), - Request( - "hyundai", - [HYUNDAI_VERSION_REQUEST_MULTI], - [HYUNDAI_VERSION_RESPONSE], - ), - # Honda - Request( - "honda", - [UDS_VERSION_REQUEST], - [UDS_VERSION_RESPONSE], - ), - # Toyota - Request( - "toyota", - [SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST], - [SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE], - bus=0, - ), - Request( - "toyota", - [SHORT_TESTER_PRESENT_REQUEST, OBD_VERSION_REQUEST], - [SHORT_TESTER_PRESENT_RESPONSE, OBD_VERSION_RESPONSE], - bus=0, - ), - Request( - "toyota", - [TESTER_PRESENT_REQUEST, DEFAULT_DIAGNOSTIC_REQUEST, EXTENDED_DIAGNOSTIC_REQUEST, UDS_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, DEFAULT_DIAGNOSTIC_RESPONSE, EXTENDED_DIAGNOSTIC_RESPONSE, UDS_VERSION_RESPONSE], - bus=0, - ), - # Volkswagen - Request( - "volkswagen", - [VOLKSWAGEN_VERSION_REQUEST_MULTI], - [VOLKSWAGEN_VERSION_RESPONSE], - whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar], - rx_offset=VOLKSWAGEN_RX_OFFSET, - ), - Request( - "volkswagen", - [VOLKSWAGEN_VERSION_REQUEST_MULTI], - [VOLKSWAGEN_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine, Ecu.transmission], - ), - # Mazda - Request( - "mazda", - [MAZDA_VERSION_REQUEST], - [MAZDA_VERSION_RESPONSE], - ), - # Nissan - Request( - "nissan", - [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], - [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], - ), - Request( - "nissan", - [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], - [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], - rx_offset=NISSAN_RX_OFFSET, - ), - Request( - "nissan", - [NISSAN_VERSION_REQUEST_STANDARD], - [NISSAN_VERSION_RESPONSE_STANDARD], - rx_offset=NISSAN_RX_OFFSET, - ), - # Body - Request( - "body", - [TESTER_PRESENT_REQUEST, UDS_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, UDS_VERSION_RESPONSE], - bus=0, - ), - # Chrysler / FCA / Stellantis - Request( - "chrysler", - [CHRYSLER_VERSION_REQUEST], - [CHRYSLER_VERSION_RESPONSE], - rx_offset=CHRYSLER_RX_OFFSET, - ), - Request( - "chrysler", - [CHRYSLER_VERSION_REQUEST], - [CHRYSLER_VERSION_RESPONSE], - ), - # Ford - Request( - "ford", - [TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine], - ), - Request( - "ford", - [TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE], - bus=0, - whitelist_ecus=[Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera], - ), -] +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] def chunks(l, n=128): @@ -248,9 +37,8 @@ def build_fw_dict(fw_versions, filter_brand=None): def get_brand_addrs(): - versions = get_interface_attr('FW_VERSIONS', ignore_none=True) brand_addrs = defaultdict(set) - for brand, cars in versions.items(): + for brand, cars in VERSIONS.items(): for fw in cars.values(): brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in fw.keys()} return brand_addrs @@ -312,19 +100,19 @@ def match_fw_to_car_exact(fw_versions_dict): for candidate, fws in candidates.items(): for ecu, expected_versions in fws.items(): + config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]] ecu_type = ecu[0] addr = ecu[1:] + found_versions = fw_versions_dict.get(addr, set()) - if ecu_type == Ecu.esp and candidate in (TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER, TOYOTA.SIENNA, TOYOTA.LEXUS_IS) and not len(found_versions): - continue + if not len(found_versions): + # Some models can sometimes miss an ecu, or show on two different addresses + if candidate in config.non_essential_ecus.get(ecu_type, []): + continue - # On some Toyota models, the engine can show on two different addresses - if ecu_type == Ecu.engine and candidate in (TOYOTA.CAMRY, TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS) and not len(found_versions): - continue - - # Ignore non essential ecus - if ecu_type not in ESSENTIAL_ECUS and not len(found_versions): - continue + # Ignore non essential ecus + if ecu_type not in ESSENTIAL_ECUS: + continue # Virtual debug ecu doesn't need to match the database if ecu_type == Ecu.debug: @@ -345,11 +133,10 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True): if allow_fuzzy: exact_matches.append((False, match_fw_to_car_fuzzy)) - brands = get_interface_attr('FW_VERSIONS', ignore_none=True).keys() for exact_match, match_func in exact_matches: # For each brand, attempt to fingerprint using all FW returned from its queries matches = set() - for brand in brands: + for brand in VERSIONS.keys(): fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand) matches |= match_func(fw_versions_dict) @@ -363,13 +150,9 @@ def get_present_ecus(logcan, sendcan): queries = list() parallel_queries = list() responses = set() - versions = get_interface_attr('FW_VERSIONS', ignore_none=True) - for r in REQUESTS: - if r.brand not in versions: - continue - - for brand_versions in versions[r.brand].values(): + for brand, r in REQUESTS: + for brand_versions in VERSIONS[brand].values(): for ecu_type, addr, sub_addr in brand_versions: # Only query ecus in whitelist if whitelist is not empty if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus: @@ -398,9 +181,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 = {r.brand: set() for r in REQUESTS} + brand_matches = {brand: set() for brand, _ in REQUESTS} - brand_rx_offsets = set((r.brand, r.rx_offset) for 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: @@ -411,14 +194,14 @@ def get_brand_ecu_matches(ecu_rx_addrs): return brand_matches -def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, debug=False, progress=False): +def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False): """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" all_car_fw = [] brand_matches = get_brand_ecu_matches(ecu_rx_addrs) for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True): - car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, debug=debug, progress=progress) + 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)) @@ -428,8 +211,13 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, debug=Fa return all_car_fw -def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, debug=False, progress=False): - versions = get_interface_attr('FW_VERSIONS', ignore_none=True) +def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False): + versions = VERSIONS.copy() + + # Each brand can define extra ECUs to query for data collection + for brand, config in FW_QUERY_CONFIGS.items(): + versions[brand]["debug"] = {ecu: [] for ecu in config.extra_ecus} + if query_brand is not None: versions = {query_brand: versions[query_brand]} @@ -460,33 +248,37 @@ 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 = [r for r in REQUESTS if query_brand is None or r.brand == query_brand] + requests = [(brand, r) for brand, 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 r in requests: + for brand, r in requests: + # Skip query if no panda available + if r.bus > num_pandas * 4 - 1: + continue + try: - addrs = [(a, s) for (b, a, s) in addr_chunk if b in (r.brand, 'any') and + addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and (len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)] if addrs: query = IsoTpParallelQuery(sendcan, logcan, r.bus, addrs, r.request, r.response, r.rx_offset, debug=debug) - for (addr, rx_addr), version in query.get_data(timeout).items(): + for (tx_addr, sub_addr), version in query.get_data(timeout).items(): f = car.CarParams.CarFw.new_message() - f.ecu = ecu_types.get((r.brand, addr[0], addr[1]), Ecu.unknown) + f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown) f.fwVersion = version - f.address = addr[0] - f.responseAddress = rx_addr + f.address = tx_addr + f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset) f.request = r.request - f.brand = r.brand + f.brand = brand f.bus = r.bus - if addr[1] is not None: - f.subAddress = addr[1] + if sub_addr is not None: + f.subAddress = sub_addr car_fw.append(f) except Exception: - cloudlog.warning(f"FW query exception: {traceback.format_exc()}") + cloudlog.exception("FW query exception") return car_fw @@ -504,6 +296,7 @@ if __name__ == "__main__": args = parser.parse_args() logcan = messaging.sub_sock('can') + pandaStates_sock = messaging.sub_sock('pandaStates') sendcan = messaging.pub_sock('sendcan') extra: Any = None @@ -517,16 +310,17 @@ if __name__ == "__main__": extra = {"any": {"debug": extra}} time.sleep(1.) + num_pandas = len(messaging.recv_one_retry(pandaStates_sock).pandaStates) t = time.time() print("Getting vin...") - addr, vin_rx_addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug) - print(f'TX: {hex(addr)}, RX: {hex(vin_rx_addr)}, VIN: {vin}') + vin_rx_addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug) + print(f'RX: {hex(vin_rx_addr)}, VIN: {vin}') print(f"Getting VIN took {time.time() - t:.3f} s") print() t = time.time() - fw_vers = get_fw_versions(logcan, sendcan, query_brand=args.brand, extra=extra, debug=args.debug, progress=True) + fw_vers = get_fw_versions(logcan, sendcan, query_brand=args.brand, extra=extra, num_pandas=num_pandas, debug=args.debug, progress=True) _, candidates = match_fw_to_car(fw_vers) print() diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 977d20c5b..a6cd2f19b 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -5,10 +5,14 @@ from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan -from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, EV_CAR +from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons VisualAlert = car.CarControl.HUDControl.VisualAlert NetworkLocation = car.CarParams.NetworkLocation +LongCtrlState = car.CarControl.Actuators.LongControlState + +# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s +CAMERA_CANCEL_DELAY_FRAMES = 10 class CarController: @@ -19,12 +23,15 @@ class CarController: self.apply_gas = 0 self.apply_brake = 0 self.frame = 0 + self.last_steer_frame = 0 self.last_button_frame = 0 + self.cancel_counter = 0 - self.lka_steering_cmd_counter_last = -1 + self.lka_steering_cmd_counter = 0 + self.sent_lka_steering_cmd = False self.lka_icon_status_last = (False, False) - self.params = CarControllerParams() + self.params = CarControllerParams(self.CP) self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt']) self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) @@ -41,47 +48,59 @@ class CarController: # Send CAN commands. can_sends = [] - # Steering (50Hz) + # Steering (Active: 50Hz, inactive: 10Hz) + # Attempt to sync with camera on startup at 50Hz, first few msgs are blocked + init_lka_counter = not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera + steer_step = self.params.INACTIVE_STEER_STEP + if CC.latActive or init_lka_counter: + steer_step = self.params.ACTIVE_STEER_STEP + # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the # next Panda loopback confirmation in the current CS frame. - if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: - self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter - elif (self.frame % self.params.STEER_STEP) == 0: + if CS.loopback_lka_steering_cmd_updated: + self.lka_steering_cmd_counter += 1 + self.sent_lka_steering_cmd = True + elif (self.frame - self.last_steer_frame) >= steer_step: + # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus + if init_lka_counter: + self.lka_steering_cmd_counter = CS.camera_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) else: apply_steer = 0 + self.last_steer_frame = self.frame self.apply_steer_last = apply_steer - # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the - # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks. - idx = (CS.lka_steering_cmd_counter + 1) % 4 - + idx = self.lka_steering_cmd_counter % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive)) if self.CP.openpilotLongitudinalControl: # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: if not CC.longActive: - # Stock ECU sends max regen when not enabled - self.apply_gas = self.params.MAX_ACC_REGEN + # ASCM sends max regen when not enabled + self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = 0 else: - if self.CP.carFingerprint in EV_CAR: - self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) - else: - self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) idx = (self.frame // 4) % 4 at_full_stop = CC.longActive and CS.out.standstill near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) + friction_brake_bus = CanBus.CHASSIS + # GM Camera exceptions + # TODO: can we always check the longControlState? + if self.CP.networkLocation == NetworkLocation.fwdCamera: + at_full_stop = at_full_stop and actuators.longControlState == LongCtrlState.stopping + friction_brake_bus = CanBus.POWERTRAIN + # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) - can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, idx, CC.enabled, near_stop, at_full_stop, self.CP)) # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw @@ -108,12 +127,21 @@ class CarController: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) else: + # While car is braking, cancel button causes ECM to enter a soft disable state with a fault status. + # A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly + self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0 + # Stock longitudinal, integrated at camera if (self.frame - self.last_button_frame) * DT_CTRL > 0.04: - if CC.cruiseControl.cancel: + if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES: self.last_button_frame = self.frame can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL)) + if self.CP.networkLocation == NetworkLocation.fwdCamera: + # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1 + if self.frame % 10 == 0: + can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status)) + # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 0bba1d29b..df1b4b286 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -1,3 +1,4 @@ +import copy from cereal import car from common.conversions import Conversions as CV from common.numpy_fast import mean @@ -8,6 +9,7 @@ from selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation +STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS class CarState(CarStateBase): @@ -15,7 +17,8 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] - self.lka_steering_cmd_counter = 0 + self.loopback_lka_steering_cmd_updated = False + self.camera_lka_steering_cmd_counter = 0 self.buttons_counter = 0 def update(self, pt_cp, cam_cp, loopback_cp): @@ -24,6 +27,13 @@ class CarState(CarStateBase): self.prev_cruise_buttons = self.cruise_buttons self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] + self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"]) + self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0 + + # Variables used for avoiding LKAS faults + self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 + if self.CP.networkLocation == NetworkLocation.fwdCamera: + self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] ret.wheelSpeeds = self.get_wheel_speeds( pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], @@ -33,20 +43,27 @@ class CarState(CarStateBase): ) ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.vEgoRaw < 0.01 + # sample rear wheel speeds, standstill=True if ECM allows engagement with brake + ret.standstill = ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: ret.gearShifter = self.parse_gear_shifter("T") else: ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) - # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. - ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 - ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10 + ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] + if self.CP.networkLocation == NetworkLocation.fwdCamera: + ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0 + else: + # Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe + # that the brake is being intermittently pressed without user interaction. + # To avoid a cruise fault we need to use a conservative brake position threshold + # https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf + ret.brakePressed = ret.brake >= 8 # Regen braking is braking if self.CP.transmissionType == TransmissionType.direct: - ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 + ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. ret.gasPressed = ret.gas > 1e-5 @@ -56,7 +73,6 @@ class CarState(CarStateBase): ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"] ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] # 0 inactive, 1 active, 2 temporarily limited, 3 failed self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"] @@ -83,6 +99,7 @@ class CarState(CarStateBase): ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL if self.CP.networkLocation == NetworkLocation.fwdCamera: ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS + ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0 return ret @@ -91,8 +108,16 @@ class CarState(CarStateBase): signals = [] checks = [] if CP.networkLocation == NetworkLocation.fwdCamera: - signals.append(("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus")) - checks.append(("ASCMActiveCruiseControlStatus", 25)) + signals += [ + ("AEBCmdActive", "AEBCmd"), + ("RollingCounter", "ASCMLKASteeringCmd"), + ("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"), + ] + checks += [ + ("AEBCmd", 10), + ("ASCMLKASteeringCmd", 10), + ("ASCMActiveCruiseControlStatus", 25), + ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.CAMERA) @@ -100,7 +125,7 @@ class CarState(CarStateBase): def get_can_parser(CP): signals = [ # sig_name, sig_address - ("BrakePedalPosition", "EBCMBrakePedalPosition"), + ("BrakePedalPos", "ECMAcceleratorPos"), ("FrontLeftDoor", "BCMDoorBeltStatus"), ("FrontRightDoor", "BCMDoorBeltStatus"), ("RearLeftDoor", "BCMDoorBeltStatus"), @@ -118,14 +143,21 @@ class CarState(CarStateBase): ("FRWheelSpd", "EBCMWheelSpdFront"), ("RLWheelSpd", "EBCMWheelSpdRear"), ("RRWheelSpd", "EBCMWheelSpdRear"), + ("MovingBackward", "EBCMWheelSpdRear"), ("PRNDL2", "ECMPRDNL2"), ("ManualMode", "ECMPRDNL2"), ("LKADriverAppldTrq", "PSCMStatus"), ("LKATorqueDelivered", "PSCMStatus"), ("LKATorqueDeliveredStatus", "PSCMStatus"), + ("HandsOffSWlDetectionStatus", "PSCMStatus"), + ("HandsOffSWDetectionMode", "PSCMStatus"), + ("LKATotalTorqueDelivered", "PSCMStatus"), + ("PSCMStatusChecksum", "PSCMStatus"), + ("RollingCounter", "PSCMStatus"), ("TractionControlOn", "ESPStatus"), ("ParkBrake", "VehicleIgnitionAlt"), ("CruiseMainOn", "ECMEngineStatus"), + ("BrakePressed", "ECMEngineStatus"), ] checks = [ @@ -141,7 +173,7 @@ class CarState(CarStateBase): ("ASCMSteeringButton", 33), ("ECMEngineStatus", 100), ("PSCMSteeringAngle", 100), - ("EBCMBrakePedalPosition", 100), + ("ECMAcceleratorPos", 80), ] if CP.transmissionType == TransmissionType.direct: @@ -157,9 +189,7 @@ class CarState(CarStateBase): ] checks = [ - ("ASCMLKASteeringCmd", 10), # 10 Hz is the stock inactive rate (every 100ms). - # While active 50 Hz (every 20 ms) is normal - # EPS will tolerate around 200ms when active before faulting + ("ASCMLKASteeringCmd", 0), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK, enforce_checks=False) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 20e4c4ab6..56c981326 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -1,4 +1,6 @@ from selfdrive.car import make_can_msg +from selfdrive.car.gm.values import CAR + def create_buttons(packer, bus, idx, button): values = { @@ -7,8 +9,15 @@ def create_buttons(packer, bus, idx, button): } return packer.make_can_msg("ASCMSteeringButton", bus, values) -def create_steering_control(packer, bus, apply_steer, idx, lkas_active): +def create_pscm_status(packer, bus, pscm_status): + checksum_mod = int(1 - pscm_status["HandsOffSWlDetectionStatus"]) << 5 + pscm_status["HandsOffSWlDetectionStatus"] = 1 + pscm_status["PSCMStatusChecksum"] += checksum_mod + return packer.make_can_msg("PSCMStatus", bus, pscm_status) + + +def create_steering_control(packer, bus, apply_steer, idx, lkas_active): values = { "LKASteeringCmdActive": lkas_active, "LKASteeringCmd": apply_steer, @@ -18,15 +27,17 @@ def create_steering_control(packer, bus, apply_steer, idx, lkas_active): return packer.make_can_msg("ASCMLKASteeringCmd", bus, values) + def create_adas_keepalive(bus): dat = b"\x00\x00\x00\x00\x00\x00\x00" return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)] -def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_stop): + +def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop): values = { - "GasRegenCmdActive": acc_engaged, + "GasRegenCmdActive": enabled, "RollingCounter": idx, - "GasRegenCmdActiveInv": 1 - acc_engaged, + "GasRegenCmdActiveInv": 1 - enabled, "GasRegenCmd": throttle, "GasRegenFullStopActive": at_full_stop, "GasRegenAlwaysOne": 1, @@ -41,15 +52,21 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st return packer.make_can_msg("ASCMGasRegenCmd", bus, values) -def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop): + +def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_stop, at_full_stop, CP): mode = 0x1 + + # TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake + if enabled and CP.carFingerprint in (CAR.BOLT_EUV,): + mode = 0x9 + if apply_brake > 0: mode = 0xa if at_full_stop: mode = 0xd # TODO: this is to have GM bringing the car to complete stop, - # but currently it conflicts with OP controls, so turned off. + # but currently it conflicts with OP controls, so turned off. Not set by all cars #elif near_stop: # mode = 0xb @@ -57,44 +74,48 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_f checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff values = { - "RollingCounter" : idx, - "FrictionBrakeMode" : mode, + "RollingCounter": idx, + "FrictionBrakeMode": mode, "FrictionBrakeChecksum": checksum, - "FrictionBrakeCmd" : -apply_brake + "FrictionBrakeCmd": -apply_brake } return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) -def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lead_car_in_sight, fcw): + +def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw): target_speed = min(target_speed_kph, 255) values = { - "ACCAlwaysOne" : 1, - "ACCResumeButton" : 0, - "ACCSpeedSetpoint" : target_speed, - "ACCGapLevel" : 3 * acc_engaged, # 3 "far", 0 "inactive" - "ACCCmdActive" : acc_engaged, - "ACCAlwaysOne2" : 1, - "ACCLeadCar" : lead_car_in_sight, + "ACCAlwaysOne": 1, + "ACCResumeButton": 0, + "ACCSpeedSetpoint": target_speed, + "ACCGapLevel": 3 * enabled, # 3 "far", 0 "inactive" + "ACCCmdActive": enabled, + "ACCAlwaysOne2": 1, + "ACCLeadCar": lead_car_in_sight, "FCWAlert": 0x3 if fcw else 0 } return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values) + def create_adas_time_status(bus, tt, idx): dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff, - ((tt & 0xf) << 4) + (idx << 2)] + ((tt & 0xf) << 4) + (idx << 2)] chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] chksum = chksum & 0xfff dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] return make_can_msg(0xa1, bytes(dat), bus) + def create_adas_steering_status(bus, idx): dat = [idx << 6, 0xf0, 0x20, 0, 0, 0] chksum = 0x60 + sum(dat) dat += [chksum >> 8, chksum & 0xff] return make_can_msg(0x306, bytes(dat), bus) + def create_adas_accelerometer_speed_status(bus, speed_ms, idx): spd = int(speed_ms * 16) & 0xfff accel = 0 & 0xfff @@ -108,6 +129,7 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx): dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] return make_can_msg(0x308, bytes(dat), bus) + def create_adas_headlights_status(packer, bus): values = { "Always42": 0x42, @@ -115,6 +137,7 @@ def create_adas_headlights_status(packer, bus): } return packer.make_can_msg("ASCMHeadlight", bus, values) + def create_lka_icon_command(bus, active, critical, steer): if active and steer == 1: if critical: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 09d950ea8..eb5ab7329 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -20,8 +20,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D class CarInterface(CarInterfaceBase): @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): - params = CarControllerParams() - return params.ACCEL_MIN, params.ACCEL_MAX + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. @staticmethod @@ -45,52 +44,74 @@ class CarInterface(CarInterfaceBase): return CarInterfaceBase.get_steer_feedforward_default @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] + ret.autoResumeSng = False if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct else: ret.transmissionType = TransmissionType.automatic + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.15] + + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kiBP = [0.] + if candidate in CAMERA_ACC_CAR: - ret.openpilotLongitudinalControl = False + ret.experimentalLongitudinalAvailable = True ret.networkLocation = NetworkLocation.fwdCamera ret.radarOffCan = True # no radar ret.pcmCruise = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM + ret.minEnableSpeed = 5 * CV.KPH_TO_MS + + if experimental_long: + ret.pcmCruise = False + ret.openpilotLongitudinalControl = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG + + # Tuning + 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 + else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway ret.radarOffCan = False 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 + + # Tuning + ret.longitudinalTuning.kpV = [2.4, 1.5] + ret.longitudinalTuning.kiV = [0.36] # 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} + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX, CAR.BOLT_EV} # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. - ret.minSteerSpeed = 7 * CV.MPH_TO_MS + # Some GMs need some tolerance above 10 kph to avoid a fault + ret.minSteerSpeed = 10.1 * CV.KPH_TO_MS ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet tire_stiffness_factor = 0.444 # not optimized yet - ret.longitudinalTuning.kpBP = [5., 35.] - ret.longitudinalTuning.kpV = [2.4, 1.5] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.36] - ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz - # supports stop and go, but initial engage must (conservatively) be above 18mph - ret.minEnableSpeed = 18 * CV.MPH_TO_MS - if candidate == CAR.VOLT: ret.mass = 1607. + STD_CARGO_KG ret.wheelbase = 2.69 @@ -137,31 +158,29 @@ class CarInterface(CarInterfaceBase): ret.mass = 1601. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 15.3 - ret.centerToFront = ret.wheelbase * 0.49 + ret.centerToFront = ret.wheelbase * 0.5 elif candidate == CAR.ESCALADE_ESV: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 2739. + STD_CARGO_KG ret.wheelbase = 3.302 ret.steerRatio = 17.3 - ret.centerToFront = ret.wheelbase * 0.49 + ret.centerToFront = ret.wheelbase * 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]] ret.lateralTuning.pid.kf = 0.000045 tire_stiffness_factor = 1.0 - elif candidate == CAR.BOLT_EUV: - ret.minEnableSpeed = -1 + elif candidate in (CAR.BOLT_EV, CAR.BOLT_EUV): ret.mass = 1669. + STD_CARGO_KG - ret.wheelbase = 2.675 + ret.wheelbase = 2.63779 ret.steerRatio = 16.8 - ret.centerToFront = ret.wheelbase * 0.4 + ret.centerToFront = 2.15 # measured tire_stiffness_factor = 1.0 ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.SILVERADO: - ret.minEnableSpeed = -1 ret.mass = 2200. + STD_CARGO_KG ret.wheelbase = 3.75 ret.steerRatio = 16.3 @@ -169,6 +188,13 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 1.0 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.EQUINOX: + ret.mass = 3500. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.72 + ret.steerRatio = 14.4 + ret.centerToFront = ret.wheelbase * 0.4 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) @@ -185,24 +211,31 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: - be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS) + buttonEvents = [create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)] + # Handle ACCButtons changing buttons mid-press + if self.CS.cruise_buttons != CruiseButtons.UNPRESS and self.CS.prev_cruise_buttons != CruiseButtons.UNPRESS: + buttonEvents.append(create_button_event(CruiseButtons.UNPRESS, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)) - # Suppress resume button if we're resuming from stop so we don't adjust speed. - if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill): - be.type = ButtonType.unknown - - ret.buttonEvents = [be] + ret.buttonEvents = buttonEvents + # The ECM allows enabling on falling edge of set, but only rising edge of resume events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.eco, GearShifter.manumatic], - pcm_enable=self.CP.pcmCruise) + pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,)) + if not self.CP.pcmCruise: + if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents): + events.add(EventName.buttonEnable) - if ret.vEgo < self.CP.minEnableSpeed: + # Enabling at a standstill with brake is allowed + # TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs + below_min_enable_speed = ret.vEgo < self.CP.minEnableSpeed or self.CS.moving_backward + if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and + self.CP.networkLocation == NetworkLocation.fwdCamera): events.add(EventName.belowEngageSpeed) if ret.cruiseState.standstill: events.add(EventName.resumeRequired) if ret.vEgo < self.CP.minSteerSpeed: - events.add(car.CarEvent.EventName.belowSteerSpeed) + events.add(EventName.belowSteerSpeed) ret.events = events.to_msg() diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 21ede171e..03392ba0f 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -11,7 +11,8 @@ Ecu = car.CarParams.Ecu class CarControllerParams: STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output - STEER_STEP = 2 # Control frames per command (50hz) + ACTIVE_STEER_STEP = 2 # Active control frames per command (50hz) + INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz) STEER_DELTA_UP = 7 # Delta rates require review due to observed EPS weakness STEER_DELTA_DOWN = 17 STEER_DRIVER_ALLOWANCE = 50 @@ -23,13 +24,6 @@ class CarControllerParams: ADAS_KEEPALIVE_STEP = 100 CAMERA_KEEPALIVE_STEP = 100 - # Volt gasbrake lookups - # TODO: These values should be confirmed on non-Volt vehicles - MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. - ZERO_GAS = 2048 # Coasting - MAX_BRAKE = 350 # ~ -3.5 m/s^2 with regen - MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen - # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we # perform the closed loop control, and might need some # to apply some more braking if we're on a downhill slope. @@ -38,16 +32,32 @@ class CarControllerParams: ACCEL_MAX = 2. # m/s^2 ACCEL_MIN = -4. # m/s^2 - EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX] - EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.] + def __init__(self, CP): + # Gas/brake lookups + self.ZERO_GAS = 2048 # Coasting + self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen - # ICE has much less engine braking force compared to regen in EVs, - # lower threshold removes some braking deadzone - GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX] - BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1] + if CP.carFingerprint in CAMERA_ACC_CAR: + self.MAX_GAS = 3400 + self.MAX_ACC_REGEN = 1514 + self.INACTIVE_REGEN = 1554 + # Camera ACC vehicles have no regen while enabled. + # Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly + max_regen_acceleration = 0. - GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] - BRAKE_LOOKUP_V = [MAX_BRAKE, 0.] + else: + self.MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. + self.MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen + self.INACTIVE_REGEN = 1404 + # ICE has much less engine braking force compared to regen in EVs, + # lower threshold removes some braking deadzone + max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1 + + self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX] + self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS] + + self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration] + self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] class CAR: @@ -58,8 +68,10 @@ class CAR: ACADIA = "GMC ACADIA DENALI 2018" BUICK_REGAL = "BUICK REGAL ESSENCE 2018" ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" + BOLT_EV = "CHEVROLET BOLT EV 2022" BOLT_EUV = "CHEVROLET BOLT EUV 2022" SILVERADO = "CHEVROLET SILVERADO 1500 2020" + EQUINOX = "CHEVROLET EQUINOX 2019" class Footnote(Enum): @@ -71,24 +83,26 @@ class Footnote(Enum): @dataclass class GMCarInfo(CarInfo): - package: str = "Adaptive Cruise Control" + package: str = "Adaptive Cruise Control (ACC)" harness: Enum = Harness.obd_ii footnotes: List[Enum] = field(default_factory=lambda: [Footnote.OBD_II]) CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"), - CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0), + CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ"), 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_REGAL: GMCarInfo("Buick Regal Essence 2018"), 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", video_link="https://youtu.be/xvwzGMUA210", footnotes=[], harness=Harness.gm), + CAR.BOLT_EV: GMCarInfo("Chevrolet Bolt EV 2022-23", footnotes=[], harness=Harness.gm), + CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210", footnotes=[], harness=Harness.gm), CAR.SILVERADO: [ GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II", footnotes=[], harness=Harness.gm), GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", footnotes=[], harness=Harness.gm), ], + CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22", footnotes=[], harness=Harness.gm), } @@ -166,13 +180,17 @@ FINGERPRINTS = { { 190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 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, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 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 }], + CAR.EQUINOX: [ + { + 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 + }], } 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')) -EV_CAR = {CAR.VOLT, CAR.BOLT_EUV} +EV_CAR = {CAR.VOLT, CAR.BOLT_EV, 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} +CAMERA_ACC_CAR = {CAR.BOLT_EV, CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX} STEER_THRESHOLD = 1.0 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 5fa475fe0..790dce181 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -99,6 +99,12 @@ HUDData = namedtuple("HUDData", "lanes_visible", "fcw", "acc_alert", "steer_required"]) +def rate_limit_steer(new_steer, last_steer): + # TODO just hardcoded ramp to min/max in 0.33s for all Honda + MAX_DELTA = 3 * DT_CTRL + return clip(new_steer, last_steer - MAX_DELTA, last_steer + MAX_DELTA) + + class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -116,6 +122,7 @@ class CarController: self.speed = 0.0 self.gas = 0.0 self.brake = 0.0 + self.last_steer = 0.0 def update(self, CC, CS): actuators = CC.actuators @@ -130,6 +137,10 @@ class CarController: accel = 0.0 gas, brake = 0.0, 0.0 + # *** rate limit steer *** + limited_steer = rate_limit_steer(actuators.steer, self.last_steer) + self.last_steer = limited_steer + # *** apply brake hysteresis *** pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady, CS.out.vEgo, self.CP.carFingerprint) @@ -143,7 +154,7 @@ class CarController: # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) - apply_steer = int(interp(-actuators.steer * self.params.STEER_MAX, + apply_steer = int(interp(-limited_steer * self.params.STEER_MAX, self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V)) # Send CAN commands @@ -250,6 +261,7 @@ class CarController: new_actuators.accel = self.accel new_actuators.gas = self.gas new_actuators.brake = self.brake + new_actuators.steer = self.last_steer self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 4696bec82..a37667fd3 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -24,6 +24,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"), ("STEER_TORQUE_SENSOR", "STEER_STATUS"), ("IMPERIAL_UNIT", "CAR_SPEED"), + ("ROUGH_CAR_SPEED_2", "CAR_SPEED"), ("LEFT_BLINKER", "SCM_FEEDBACK"), ("RIGHT_BLINKER", "SCM_FEEDBACK"), ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"), @@ -150,6 +151,10 @@ class CarState(CarStateBase): self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 + # When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster + # However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX) + self.dash_speed_seen = False + def update(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() @@ -203,6 +208,11 @@ class CarState(CarStateBase): ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + self.dash_speed_seen = self.dash_speed_seen or cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] > 1e-3 + if self.dash_speed_seen: + conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + ret.vEgoCluster = cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] * conversion + ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"] ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"] @@ -237,9 +247,9 @@ class CarState(CarStateBase): ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252. # on certain cars, CRUISE_SPEED changes to imperial with car's unit setting - conversion_factor = CV.MPH_TO_MS if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS and not self.is_metric else CV.KPH_TO_MS + conversion = CV.MPH_TO_MS if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS and not self.is_metric else CV.KPH_TO_MS # 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_factor + 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 else: ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index a235ba6cd..e397f0283 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -29,7 +29,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "honda" @@ -40,7 +40,8 @@ class CarInterface(CarInterfaceBase): if candidate not in HONDA_BOSCH_RADARLESS: # Disable the radar and let openpilot control longitudinal # WARNING: THIS DISABLES AEB! - ret.openpilotLongitudinalControl = disable_radar + ret.experimentalLongitudinalAvailable = True + ret.openpilotLongitudinalControl = experimental_long ret.pcmCruise = not ret.openpilotLongitudinalControl else: @@ -219,23 +220,17 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] tire_stiffness_factor = 0.677 - elif candidate == CAR.ODYSSEY: - ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG + elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN): + ret.mass = 1900. + STD_CARGO_KG ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - tire_stiffness_factor = 0.82 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] - - elif candidate == CAR.ODYSSEY_CHN: - ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg - ret.wheelbase = 2.90 - ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY - ret.steerRatio = 14.35 - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] + if candidate == CAR.ODYSSEY_CHN: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end elif candidate in (CAR.PILOT, CAR.PASSPORT): ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index ad4f73c01..2510f2e1f 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -6,6 +6,7 @@ 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.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -104,44 +105,51 @@ class Footnote(Enum): @dataclass class HondaCarInfo(CarInfo): package: str = "Honda Sensing" - min_steer_speed: float = 12. * CV.MPH_TO_MS CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { CAR.ACCORD: [ - HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + HondaCarInfo("Honda Accord 2018-22", "All", "https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), ], CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), - CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", harness=Harness.nidec, video_link="https://youtu.be/-IkImTe1NYE"), + CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec, video_link="https://youtu.be/-IkImTe1NYE"), CAR.CIVIC_BOSCH: [ - 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, harness=Harness.bosch_a), - HondaCarInfo("Honda Civic Hatchback 2017-21", harness=Harness.bosch_a), + HondaCarInfo("Honda Civic 2019-21", "All", "https://www.youtube.com/watch?v=4Iz1Mz5LGF8", [Footnote.CIVIC_DIESEL], 2. * CV.MPH_TO_MS, harness=Harness.bosch_a), + HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a), ], CAR.CIVIC_BOSCH_DIESEL: None, # same platform CAR.CIVIC_2022: [ - HondaCarInfo("Honda Civic 2022", "All", min_steer_speed=0., harness=Harness.bosch_b), - HondaCarInfo("Honda Civic Hatchback 2022", "All", min_steer_speed=0., harness=Harness.bosch_b), + HondaCarInfo("Honda Civic 2022", "All", harness=Harness.bosch_b), + HondaCarInfo("Honda Civic Hatchback 2022", "All", harness=Harness.bosch_b), ], CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", harness=Harness.nidec), - CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", harness=Harness.bosch_a), + CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), + CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a), CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring - CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", harness=Harness.bosch_a), - CAR.FIT: HondaCarInfo("Honda Fit 2018-20", harness=Harness.nidec), - CAR.FREED: HondaCarInfo("Honda Freed 2020", harness=Harness.nidec), - CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", harness=Harness.nidec), - CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", min_steer_speed=0., harness=Harness.nidec), + CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a), + CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), + CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), + CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), + CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", harness=Harness.nidec), CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey - CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", harness=Harness.nidec), + CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), - CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", harness=Harness.nidec), - CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", harness=Harness.nidec), - CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", harness=Harness.nidec), + CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), + CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), + CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), } +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.UDS_VERSION_REQUEST], + [StdQueries.UDS_VERSION_RESPONSE], + ), + ], +) FW_VERSIONS = { CAR.ACCORD: { @@ -438,7 +446,7 @@ FW_VERSIONS = { b'78109-TED-Q510\x00\x00', b'78109-TEG-A310\x00\x00', ], - (Ecu.fwdCamera, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-TBA-A020\x00\x00', b'36161-TBA-A030\x00\x00', b'36161-TBA-A040\x00\x00', @@ -956,7 +964,7 @@ FW_VERSIONS = { b'77959-THR-A110\x00\x00', b'77959-THR-X010\x00\x00', ], - (Ecu.fwdCamera, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-THR-A020\x00\x00', b'36161-THR-A030\x00\x00', b'36161-THR-A110\x00\x00', @@ -1023,6 +1031,23 @@ FW_VERSIONS = { b'54008-THR-A020\x00\x00', ], }, + CAR.ODYSSEY_CHN: { + (Ecu.eps, 0x18da30f1, None): [ + b'39990-T6D-H220\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T6A-J010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T6A-F310\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T6A-P040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T6A-P110\x00\x00', + ], + }, CAR.PILOT: { (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TG7-A520\x00\x00', @@ -1060,7 +1085,7 @@ FW_VERSIONS = { b'39990-TG7-A070\x00\x00', b'39990-TGS-A230\x00\x00', ], - (Ecu.fwdCamera, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-TG7-A310\x00\x00', b'36161-TG7-A520\x00\x00', b'36161-TG7-A630\x00\x00', @@ -1168,7 +1193,7 @@ FW_VERSIONS = { b'57114-TX5-A220\x00\x00', b'57114-TX4-A220\x00\x00', ], - (Ecu.fwdCamera, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-TX5-A030\x00\x00', b'36161-TX4-A030\x00\x00', ], @@ -1265,7 +1290,7 @@ FW_VERSIONS = { b'39990-T6Z-A030\x00\x00', b'39990-T6Z-A050\x00\x00', ], - (Ecu.fwdCamera, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-T6Z-A020\x00\x00', b'36161-T6Z-A310\x00\x00', b'36161-T6Z-A420\x00\x00', diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 971330a33..e5fdbfd57 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car from common.conversions import Conversions as CV -from common.numpy_fast import clip, interp +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 @@ -10,11 +10,18 @@ from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerPar VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState +# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second +# All slightly below EPS thresholds to avoid fault +MAX_ANGLE = 85 +MAX_ANGLE_FRAMES = 89 +MAX_ANGLE_CONSECUTIVE_FRAMES = 2 + def process_hud_alert(enabled, fingerprint, hud_control): sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)) # initialize to no line visible + # TODO: this is not accurate for all cars sys_state = 1 if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active sys_state = 3 if enabled or sys_warning else 4 @@ -39,22 +46,22 @@ class CarController: self.CP = CP self.params = CarControllerParams(CP) self.packer = CANPacker(dbc_name) + self.angle_limit_counter = 0 self.frame = 0 + self.accel_last = 0 self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 - self.accel = 0 def update(self, CC, CS): actuators = CC.actuators hud_control = CC.hudControl - # Steering Torque - - # These cars have significantly more torque than most HKG. Limit to 70% of max. + # steering torque steer = actuators.steer if self.CP.carFingerprint in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): + # these cars have significantly more torque than most HKG; limit to 70% of max steer = clip(steer, -0.7, 0.7) new_steer = int(round(steer * self.params.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) @@ -64,51 +71,90 @@ class CarController: self.apply_steer_last = apply_steer + # accel + longitudinal + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + stopping = actuators.longControlState == LongCtrlState.stopping + set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) + + # HUD messages sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint, hud_control) can_sends = [] - if self.CP.carFingerprint in CANFD_CAR: - # steering control - can_sends.append(hyundaicanfd.create_lkas(self.packer, self.CP, CC.enabled, CC.latActive, apply_steer)) + # *** common hyundai stuff *** - # block LFA on HDA2 - if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.CANFD_HDA2): + # tester present - w/ no response (keeps relevant ECU disabled) + if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl: + addr, bus = 0x7d0, 0 + if self.CP.flags & HyundaiFlags.CANFD_HDA2.value: + addr, bus = 0x730, 5 + can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus]) + + # >90 degree steering fault prevention + # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault + if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE: + self.angle_limit_counter += 1 + else: + self.angle_limit_counter = 0 + + # Cut steer actuation bit for two frames and hold torque with induced temporary fault + torque_fault = CC.latActive and self.angle_limit_counter > MAX_ANGLE_FRAMES + lat_active = CC.latActive and not torque_fault + + if self.angle_limit_counter >= MAX_ANGLE_FRAMES + MAX_ANGLE_CONSECUTIVE_FRAMES: + self.angle_limit_counter = 0 + + # CAN-FD platforms + if self.CP.carFingerprint in CANFD_CAR: + hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2 + 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)) + + # disable LFA on HDA2 + if self.frame % 5 == 0 and hda2: can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, CS.cam_0x2a4)) # LFA and HDA icons - if self.frame % 2 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_HDA2): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, CC.enabled)) + if self.frame % 5 == 0 and (not hda2 or hda2_long): + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled)) - # button presses - if (self.frame - self.last_button_frame) * DT_CTRL > 0.25: - # cruise cancel - if CC.cruiseControl.cancel: - if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - can_sends.append(hyundaicanfd.create_cruise_info(self.packer, CS.cruise_info_copy, True)) - self.last_button_frame = self.frame - else: - for _ in range(20): - can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.CANCEL)) - self.last_button_frame = self.frame - - # cruise standstill resume - elif CC.cruiseControl.resume: - if not (self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS): - can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.RES_ACCEL)) - self.last_button_frame = self.frame - else: - - # tester present - w/ no response (keeps radar disabled) if self.CP.openpilotLongitudinalControl: - if self.frame % 100 == 0: - can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0]) + if hda2: + can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, 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, + set_speed_in_units)) + self.accel_last = accel + else: + # button presses + if (self.frame - self.last_button_frame) * DT_CTRL > 0.25: + # 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)) + 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)) + self.last_button_frame = self.frame - can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive, - CS.lkas11, sys_warning, sys_state, CC.enabled, - hud_control.leftLaneVisible, hud_control.rightLaneVisible, - left_lane_warning, right_lane_warning)) + # cruise standstill resume + elif CC.cruiseControl.resume: + if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: + # TODO: resume for alt button cars + pass + else: + for _ in range(20): + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, 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, + torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, + hud_control.leftLaneVisible, hud_control.rightLaneVisible, + left_lane_warning, right_lane_warning)) if not self.CP.openpilotLongitudinalControl: if CC.cruiseControl.cancel: @@ -121,27 +167,16 @@ class CarController: self.last_button_frame = self.frame if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: - accel = actuators.accel - jerk = 0 - - if CC.longActive: - jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) - if accel < 0: - accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel]) - - accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) - - stopping = actuators.longControlState == LongCtrlState.stopping - set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) + # TODO: unclear if this is needed + jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), - hud_control.leadVisible, set_speed_in_units, stopping, CS.out.gasPressed)) - self.accel = accel + hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.KONA_EV_2022, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, - CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022): + CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022): can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) # 5 Hz ACC options @@ -154,7 +189,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.params.STEER_MAX - new_actuators.accel = self.accel + new_actuators.accel = accel self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index eab6e73f1..2c309fa0d 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -1,5 +1,6 @@ from collections import deque import copy +import math from cereal import car from common.conversions import Conversions as CV @@ -9,6 +10,7 @@ from selfdrive.car.hyundai.values import HyundaiFlags, DBC, FEATURES, CAMERA_SCC from selfdrive.car.interfaces import CarStateBase PREV_BUTTON_SAMPLES = 8 +CLUSTER_SAMPLE_RATE = 20 # frames class CarState(CarStateBase): @@ -19,8 +21,9 @@ class CarState(CarStateBase): self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) + self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else "GEAR_SHIFTER" if CP.carFingerprint in CANFD_CAR: - self.shifter_values = can_define.dv["GEAR_SHIFTER"]["GEAR"] + self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"] elif self.CP.carFingerprint in FEATURES["use_cluster_gears"]: self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"] elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: @@ -28,10 +31,16 @@ class CarState(CarStateBase): else: # preferred and elect gear methods use same definition self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"] + self.is_metric = False self.brake_error = False - self.park_brake = False self.buttons_counter = 0 + self.cruise_info = {} + + # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz + self.cluster_speed = 0 + self.cluster_speed_counter = CLUSTER_SAMPLE_RATE + self.params = CarControllerParams(CP) def update(self, cp, cp_cam): @@ -39,8 +48,9 @@ class CarState(CarStateBase): return self.update_canfd(cp, cp_cam) ret = car.CarState.new_message() - cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp + self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0 + speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"], cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]]) @@ -55,9 +65,19 @@ class CarState(CarStateBase): ) ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.vEgoRaw < 0.1 + self.cluster_speed_counter += 1 + if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE: + self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] + self.cluster_speed_counter = 0 + + # mimic how dash converts to imperial + if not self.is_metric: + self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH) + + ret.vEgoCluster = self.cluster_speed * speed_conv + ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"] ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"] ret.yawRate = cp.vl["ESP12"]["YAW_RATE"] @@ -78,7 +98,6 @@ class CarState(CarStateBase): ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1 ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0 ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4. - speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv # TODO: Find brake pressure @@ -111,12 +130,12 @@ class CarState(CarStateBase): ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) if not self.CP.openpilotLongitudinalControl: - if self.CP.carFingerprint in FEATURES["use_fca"]: - ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0 - ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2 - else: - ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0 - ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2 + aeb_src = "FCA11" if self.CP.carFingerprint in FEATURES["use_fca"] else "SCC12" + aeb_sig = "FCA_CmdAct" if self.CP.carFingerprint in FEATURES["use_fca"] else "AEB_CmdAct" + aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0 + aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0 + ret.stockFcw = aeb_warning and not aeb_braking + ret.stockAeb = aeb_warning and aeb_braking if self.CP.enableBsm: ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 @@ -136,17 +155,21 @@ class CarState(CarStateBase): def update_canfd(self, cp, cp_cam): ret = car.CarState.new_message() - if self.CP.flags & HyundaiFlags.CANFD_HDA2: - ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255. + if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR): + if self.CP.carFingerprint in EV_CAR: + ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255. + else: + ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023. + ret.gasPressed = ret.gas > 1e-5 else: - ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023. - ret.gasPressed = ret.gas > 1e-5 - ret.brakePressed = cp.vl["BRAKE"]["BRAKE_PRESSED"] == 1 + ret.gasPressed = bool(cp.vl["ACCELERATOR_BRAKE_ALT"]["ACCELERATOR_PEDAL_PRESSED"]) + + ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1 ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR_OPEN"] == 1 ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT_LATCHED"] == 0 - gear = cp.vl["GEAR_SHIFTER"]["GEAR"] + gear = cp.vl[self.gear_msg_canfd]["GEAR"] ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) # TODO: figure out positions @@ -165,22 +188,29 @@ class CarState(CarStateBase): ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], cp.vl["BLINKERS"]["RIGHT_LAMP"]) + if self.CP.enableBsm: + ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0 + ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0 ret.cruiseState.available = True - ret.cruiseState.enabled = cp.vl["SCC1"]["CRUISE_ACTIVE"] == 1 - cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam - speed_factor = CV.MPH_TO_MS if cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] == 1 else CV.KPH_TO_MS - ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor - ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1 + self.is_metric = cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] != 1 + if not self.CP.openpilotLongitudinalControl: + speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp + ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor + ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1 + ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2) + self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"]) cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" + self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"]) self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"]) self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"] - self.cruise_info_copy = copy.copy(cp_cruise_info.vl["CRUISE_INFO"]) if self.CP.flags & HyundaiFlags.CANFD_HDA2: self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"]) @@ -227,6 +257,8 @@ class CarState(CarStateBase): ("CF_Clu_AmpInfo", "CLU11"), ("CF_Clu_AliveCnt1", "CLU11"), + ("CF_Clu_VehicleSpeed", "CLU15"), + ("ACCEnable", "TCS13"), ("ACC_REQ", "TCS13"), ("DriverBraking", "TCS13"), @@ -251,6 +283,7 @@ class CarState(CarStateBase): ("TCS13", 50), ("TCS15", 10), ("CLU11", 50), + ("CLU15", 5), ("ESP12", 100), ("CGW1", 10), ("CGW2", 5), @@ -276,12 +309,14 @@ class CarState(CarStateBase): signals += [ ("FCA_CmdAct", "FCA11"), ("CF_VSM_Warn", "FCA11"), + ("CF_VSM_DecCmdAct", "FCA11"), ] checks.append(("FCA11", 50)) else: signals += [ ("AEB_CmdAct", "SCC12"), ("CF_VSM_Warn", "SCC12"), + ("CF_VSM_DecCmdAct", "SCC12"), ] if CP.enableBsm: @@ -309,7 +344,6 @@ class CarState(CarStateBase): if CP.carFingerprint in FEATURES["use_cluster_gears"]: signals.append(("CF_Clu_Gear", "CLU15")) - checks.append(("CLU15", 5)) elif CP.carFingerprint in FEATURES["use_tcu_gears"]: signals.append(("CUR_GR", "TCU12")) checks.append(("TCU12", 100)) @@ -366,12 +400,14 @@ class CarState(CarStateBase): signals += [ ("FCA_CmdAct", "FCA11"), ("CF_VSM_Warn", "FCA11"), + ("CF_VSM_DecCmdAct", "FCA11"), ] checks.append(("FCA11", 50)) else: signals += [ ("AEB_CmdAct", "SCC12"), ("CF_VSM_Warn", "SCC12"), + ("CF_VSM_DecCmdAct", "SCC12"), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) @@ -380,21 +416,23 @@ class CarState(CarStateBase): def get_can_parser_canfd(CP): cruise_btn_msg = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" + gear_msg = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else "GEAR_SHIFTER" signals = [ ("WHEEL_SPEED_1", "WHEEL_SPEEDS"), ("WHEEL_SPEED_2", "WHEEL_SPEEDS"), ("WHEEL_SPEED_3", "WHEEL_SPEEDS"), ("WHEEL_SPEED_4", "WHEEL_SPEEDS"), - ("GEAR", "GEAR_SHIFTER"), - ("BRAKE_PRESSED", "BRAKE"), + ("GEAR", gear_msg), ("STEERING_RATE", "STEERING_SENSORS"), ("STEERING_ANGLE", "STEERING_SENSORS"), ("STEERING_COL_TORQUE", "MDPS"), ("STEERING_OUT_TORQUE", "MDPS"), + ("LKA_FAULT", "MDPS"), + + ("DriverBraking", "TCS"), - ("CRUISE_ACTIVE", "SCC1"), ("COUNTER", cruise_btn_msg), ("CRUISE_BUTTONS", cruise_btn_msg), ("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg), @@ -410,64 +448,83 @@ class CarState(CarStateBase): checks = [ ("WHEEL_SPEEDS", 100), - ("GEAR_SHIFTER", 100), - ("BRAKE", 100), + (gear_msg, 100), ("STEERING_SENSORS", 100), ("MDPS", 100), - ("SCC1", 50), + ("TCS", 50), (cruise_btn_msg, 50), ("CLUSTER_INFO", 4), ("BLINKERS", 4), ("DOORS_SEATBELTS", 4), ] - if CP.flags & HyundaiFlags.CANFD_HDA2: + if CP.enableBsm: signals += [ - ("ACCELERATOR_PEDAL", "ACCELERATOR"), - ("GEAR", "ACCELERATOR"), - ("SET_SPEED", "CRUISE_INFO"), - ("CRUISE_STANDSTILL", "CRUISE_INFO"), + ("FL_INDICATOR", "BLINDSPOTS_REAR_CORNERS"), + ("FR_INDICATOR", "BLINDSPOTS_REAR_CORNERS"), + ] + checks += [ + ("BLINDSPOTS_REAR_CORNERS", 20), + ] + + if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl: + signals += [ + ("ACCMode", "SCC_CONTROL"), + ("VSetDis", "SCC_CONTROL"), + ("CRUISE_STANDSTILL", "SCC_CONTROL"), + ] + checks += [ + ("SCC_CONTROL", 50), + ] + + if CP.carFingerprint in EV_CAR: + signals += [ + ("ACCELERATOR_PEDAL", "ACCELERATOR"), ] checks += [ - ("CRUISE_INFO", 50), ("ACCELERATOR", 100), ] - else: + elif CP.carFingerprint in HYBRID_CAR: signals += [ ("ACCELERATOR_PEDAL", "ACCELERATOR_ALT"), ] checks += [ ("ACCELERATOR_ALT", 100), ] + else: + signals += [ + ("ACCELERATOR_PEDAL_PRESSED", "ACCELERATOR_BRAKE_ALT"), + ] + checks += [ + ("ACCELERATOR_BRAKE_ALT", 100), + ] bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, bus) @staticmethod def get_cam_can_parser_canfd(CP): + signals = [] + checks = [] if CP.flags & HyundaiFlags.CANFD_HDA2: - signals = [(f"BYTE{i}", "CAM_0x2a4") for i in range(3, 24)] - checks = [("CAM_0x2a4", 20)] - else: - signals = [ - ("COUNTER", "CRUISE_INFO"), - ("NEW_SIGNAL_1", "CRUISE_INFO"), - ("CRUISE_MAIN", "CRUISE_INFO"), - ("CRUISE_STATUS", "CRUISE_INFO"), - ("CRUISE_INACTIVE", "CRUISE_INFO"), - ("NEW_SIGNAL_2", "CRUISE_INFO"), - ("CRUISE_STANDSTILL", "CRUISE_INFO"), - ("NEW_SIGNAL_3", "CRUISE_INFO"), - ("BYTE11", "CRUISE_INFO"), - ("SET_SPEED", "CRUISE_INFO"), - ("NEW_SIGNAL_4", "CRUISE_INFO"), + signals += [(f"BYTE{i}", "CAM_0x2a4") for i in range(3, 24)] + checks += [("CAM_0x2a4", 20)] + elif CP.flags & HyundaiFlags.CANFD_CAMERA_SCC: + signals += [ + ("COUNTER", "SCC_CONTROL"), + ("NEW_SIGNAL_1", "SCC_CONTROL"), + ("MainMode_ACC", "SCC_CONTROL"), + ("ACCMode", "SCC_CONTROL"), + ("CRUISE_INACTIVE", "SCC_CONTROL"), + ("ZEROS_9", "SCC_CONTROL"), + ("CRUISE_STANDSTILL", "SCC_CONTROL"), + ("ZEROS_5", "SCC_CONTROL"), + ("DISTANCE_SETTING", "SCC_CONTROL"), + ("VSetDis", "SCC_CONTROL"), ] - signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(3, 7)] - signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(13, 31)] - - checks = [ - ("CRUISE_INFO", 50), + checks += [ + ("SCC_CONTROL", 50), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 8a5e33f11..c2ffffbf2 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -4,7 +4,7 @@ from selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, - lkas11, sys_warning, sys_state, enabled, + torque_fault, lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_depart, right_lane_depart): values = lkas11 @@ -14,12 +14,14 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_LdwsRHWarning"] = right_lane_depart values["CR_Lkas_StrToqReq"] = apply_steer values["CF_Lkas_ActToi"] = steer_req + values["CF_Lkas_ToiFlt"] = torque_fault # seems to allow actuation on CR_Lkas_StrToqReq values["CF_Lkas_MsgCount"] = frame % 0x10 if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, - CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): + CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, + CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 @@ -37,12 +39,26 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, # Note: the warning is hidden while the blinkers are on values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 + # Likely cars lacking the ability to show individual lane lines in the dash + elif car_fingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): + # SysWarning 4 = keep hands on wheel + beep + values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 + + # SysState 0 = no icons + # SysState 1-2 = white car + lanes + # SysState 3 = green car + lanes, green steering wheel + # SysState 4 = green car + lanes + values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1 + values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition + + # these have no effect + values["CF_Lkas_LdwsActivemode"] = 0 + values["CF_Lkas_FcwOpt_USM"] = 0 + elif car_fingerprint == CAR.HYUNDAI_GENESIS: # This field is actually LdwsActivemode # Genesis and Optima fault when forwarding while engaged values["CF_Lkas_LdwsActivemode"] = 2 - elif car_fingerprint == CAR.KIA_OPTIMA: - values["CF_Lkas_LdwsActivemode"] = 0 dat = packer.make_can_msg("LKAS11", 0, values)[2] @@ -80,7 +96,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0): } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping, gas_pressed): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override): commands = [] scc11_values = { @@ -88,16 +104,16 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe "TauGapSet": 4, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, - "ObjValid": 0, # TODO: these two bits may allow for better longitudinal control - "ACC_ObjStatus": 0, + "ObjValid": 1, # close lead makes controls tighter + "ACC_ObjStatus": 1, # close lead makes controls tighter "ACC_ObjLatPos": 0, "ACC_ObjRelSpd": 0, - "ACC_ObjDist": 0, - } + "ACC_ObjDist": 1, # close lead makes controls tighter + } commands.append(packer.make_can_msg("SCC11", 0, scc11_values)) scc12_values = { - "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 0, + "ACCMode": 2 if enabled and long_override else 1 if enabled else 0, "StopReq": 1 if stopping else 0, "aReqRaw": accel, "aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw @@ -111,25 +127,23 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe scc14_values = { "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values - "JerkUpperLimit": max(jerk, 1.0) if not stopping else 0, # stock usually is 1.0 but sometimes uses higher values - "JerkLowerLimit": max(-jerk, 1.0), # stock usually is 0.5 but sometimes uses higher values - "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage + "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values + "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values + "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead } commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) + # note that some vehicles most likely have an alternate checksum/counter definition + # https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602 fca11_values = { - # seems to count 2,1,0,3,2,1,0,3,2,1,0,3,2,1,0,repeat... - # (where first value is aligned to Supplemental_Counter == 0) - # test: [(idx % 0xF, -((idx % 0xF) + 2) % 4) for idx in range(0x14)] - "CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1, - "Supplemental_Counter": idx % 0xF, + "CR_FCA_Alive": idx % 0xF, "PAINT1_Status": 1, "FCA_DrvSetStatus": 1, "FCA_Status": 1, # AEB disabled } fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] - fca11_values["CR_FCA_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in fca11_dat) % 0x10 + fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7]) commands.append(packer.make_can_msg("FCA11", 0, fca11_values)) return commands diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index a53be7627..8b53e7c37 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,7 +1,18 @@ +from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags -def create_lkas(packer, CP, enabled, lat_active, apply_steer): +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 + + +def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): + + ret = [] + values = { "LKA_MODE": 2, "LKA_ICON": 2 if enabled else 1, @@ -14,8 +25,14 @@ def create_lkas(packer, CP, enabled, lat_active, apply_steer): "NEW_SIGNAL_2": 0, } - msg = "LKAS" if CP.flags & HyundaiFlags.CANFD_HDA2 else "LFA" - return packer.make_can_msg(msg, 4, values) + 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)) + else: + ret.append(packer.make_can_msg("LFA", 4, values)) + + return ret def create_cam_0x2a4(packer, camera_values): camera_values.update({ @@ -23,24 +40,111 @@ def create_cam_0x2a4(packer, camera_values): }) return packer.make_can_msg("CAM_0x2a4", 4, camera_values) -def create_buttons(packer, cnt, btn): +def create_buttons(packer, CP, cnt, btn): values = { "COUNTER": cnt, "SET_ME_1": 1, "CRUISE_BUTTONS": btn, } - return packer.make_can_msg("CRUISE_BUTTONS", 5, values) -def create_cruise_info(packer, cruise_info_copy, cancel): + bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 6 + return packer.make_can_msg("CRUISE_BUTTONS", bus, values) + +def create_acc_cancel(packer, CP, cruise_info_copy): values = cruise_info_copy - if cancel: - values["CRUISE_STATUS"] = 0 - values["CRUISE_INACTIVE"] = 1 - return packer.make_can_msg("CRUISE_INFO", 4, values) + values.update({ + "ACCMode": 4, + }) + return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) -def create_lfahda_cluster(packer, enabled): +def create_lfahda_cluster(packer, CP, enabled): values = { "HDA_ICON": 1 if enabled else 0, "LFA_ICON": 2 if enabled else 0, } - return packer.make_can_msg("LFAHDA_CLUSTER", 4, values) + return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values) + + +def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_override, set_speed): + jerk = 5 + jn = jerk / 50 + if not enabled or gas_override: + a_val, a_raw = 0, 0 + else: + a_raw = accel + a_val = clip(accel, accel_last - jn, accel_last + jn) + if stopping: + a_raw = 0 + + values = { + "ACCMode": 0 if not enabled else (2 if gas_override else 1), + "MainMode_ACC": 1, + "StopReq": 1 if stopping else 0, + "aReqValue": a_val, + "aReqRaw": a_raw, + "VSetDis": set_speed, + "JerkLowerLimit": jerk if enabled else 1, + + "ACC_ObjDist": 1, + "ObjValid": 0, + "OBJ_STATUS": 2, + "SET_ME_2": 0x4, + "SET_ME_3": 0x3, + "SET_ME_TMP_64": 0x64, + "NEW_SIGNAL_10": 4, + "DISTANCE_SETTING": 4, + } + + return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) + + + +def create_adrv_messages(packer, frame): + # messages needed to car happy after disabling + # the ADAS Driving ECU to do longitudinal control + + ret = [] + + values = { + } + ret.append(packer.make_can_msg("ADRV_0x51", 4, values)) + + if frame % 2 == 0: + values = { + 'AEB_SETTING': 0x1, # show AEB disabled icon + 'SET_ME_2': 0x2, + 'SET_ME_FF': 0xff, + 'SET_ME_FC': 0xfc, + 'SET_ME_9': 0x9, + } + ret.append(packer.make_can_msg("ADRV_0x160", 5, values)) + + if frame % 5 == 0: + values = { + 'SET_ME_1C': 0x1c, + 'SET_ME_FF': 0xff, + 'SET_ME_TMP_F': 0xf, + 'SET_ME_TMP_F_2': 0xf, + } + ret.append(packer.make_can_msg("ADRV_0x1ea", 5, values)) + + values = { + 'SET_ME_E1': 0xe1, + 'SET_ME_3A': 0x3a, + } + ret.append(packer.make_can_msg("ADRV_0x200", 5, values)) + + if frame % 20 == 0: + values = { + 'SET_ME_15': 0x15, + } + ret.append(packer.make_can_msg("ADRV_0x345", 5, values)) + + if frame % 100 == 0: + values = { + 'SET_ME_22': 0x22, + 'SET_ME_41': 0x41, + } + ret.append(packer.make_can_msg("ADRV_0x1da", 5, values)) + + return ret diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index fbf233498..8738aabd1 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,12 +2,13 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV -from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams +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, CarControllerParams from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from selfdrive.car import STD_CARGO_KG, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu +Ecu = car.CarParams.Ecu ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) @@ -21,168 +22,124 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "hyundai" ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None - # WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB) - ret.openpilotLongitudinalControl = disable_radar and (candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)) - - ret.pcmCruise = not ret.openpilotLongitudinalControl - # 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.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} + ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, } + + if candidate in CANFD_CAR: + # detect HDA2 with ADAS Driving ECU + if Ecu.adas in [fw.ecu for fw in car_fw]: + ret.flags |= HyundaiFlags.CANFD_HDA2.value + else: + # non-HDA2 + if 0x1cf not in fingerprint[4]: + ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value + # ICE cars do not have 0x130; GEARS message on 0x40 instead + if 0x130 not in fingerprint[4]: + ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value + if candidate not in CANFD_RADAR_SCC_CAR: + ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value ret.steerActuatorDelay = 0.1 # Default delay ret.steerLimitTimer = 0.4 tire_stiffness_factor = 1. + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - ret.stoppingControl = True - ret.vEgoStopping = 1.0 - - ret.longitudinalTuning.kpV = [0.1] - ret.longitudinalTuning.kiV = [0.0] - ret.stopAccel = 0.0 - - ret.longitudinalActuatorDelayUpperBound = 1.0 # s if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.766 # Values from optimizer ret.steerRatio = 16.55 # 13.8 is spec end-to-end tire_stiffness_factor = 0.82 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): ret.mass = 1513. + STD_CARGO_KG ret.wheelbase = 2.84 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable tire_stiffness_factor = 0.65 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.SONATA_LF: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 4497. * CV.LB_TO_KG ret.wheelbase = 2.804 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.PALISADE: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1999. + STD_CARGO_KG ret.wheelbase = 2.90 ret.steerRatio = 15.6 * 1.15 tire_stiffness_factor = 0.63 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): - ret.lateralTuning.pid.kf = 0.00006 + elif candidate == CAR.ELANTRA: ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 tire_stiffness_factor = 0.385 # stiffnessFactor settled on 1.0081302973865127 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.ELANTRA_2021: ret.mass = (2800. * CV.LB_TO_KG) + STD_CARGO_KG ret.wheelbase = 2.72 ret.steerRatio = 12.9 tire_stiffness_factor = 0.65 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.ELANTRA_HEV_2021: ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG ret.wheelbase = 2.72 ret.steerRatio = 12.9 tire_stiffness_factor = 0.65 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.HYUNDAI_GENESIS: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 - ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGainBP = [0.] - ret.lateralTuning.indi.innerLoopGainV = [3.5] - ret.lateralTuning.indi.outerLoopGainBP = [0.] - ret.lateralTuning.indi.outerLoopGainV = [2.0] - ret.lateralTuning.indi.timeConstantBP = [0.] - ret.lateralTuning.indi.timeConstantV = [1.4] - ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] - ret.lateralTuning.indi.actuatorEffectivenessV = [2.3] ret.minSteerSpeed = 60 * CV.KPH_TO_MS elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743.}.get(candidate, 1275.) + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.42 # Spec tire_stiffness_factor = 0.385 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): - ret.lateralTuning.pid.kf = 0.00006 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 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate not in (CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): 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.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGainBP = [0.] - ret.lateralTuning.indi.innerLoopGainV = [2.5] - ret.lateralTuning.indi.outerLoopGainBP = [0.] - ret.lateralTuning.indi.outerLoopGainV = [3.5] - ret.lateralTuning.indi.timeConstantBP = [0.] - ret.lateralTuning.indi.timeConstantV = [1.4] - ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] - ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.VELOSTER: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 * 1.15 tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.TUCSON: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3520. * CV.LB_TO_KG ret.wheelbase = 2.67 ret.steerRatio = 14.00 * 1.15 tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.TUCSON_HYBRID_4TH_GEN: ret.mass = 1680. + STD_CARGO_KG # average of all 3 trims ret.wheelbase = 2.756 ret.steerRatio = 16. tire_stiffness_factor = 0.385 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.SANTA_CRUZ_1ST_GEN: + ret.mass = 1870. + STD_CARGO_KG # weight from Limited trim - the only supported trim + ret.wheelbase = 3.000 + ret.steerRatio = 14.2 # steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf # Kia elif candidate == CAR.KIA_SORENTO: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1985. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021): - ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1737. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate == CAR.KIA_NIRO_PHEV: ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.KIA_SELTOS: @@ -190,136 +147,128 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.63 ret.steerRatio = 14.56 tire_stiffness_factor = 1 - ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGainBP = [0.] - ret.lateralTuning.indi.innerLoopGainV = [4.] - ret.lateralTuning.indi.outerLoopGainBP = [0.] - ret.lateralTuning.indi.outerLoopGainV = [3.] - ret.lateralTuning.indi.timeConstantBP = [0.] - ret.lateralTuning.indi.timeConstantV = [1.4] - ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] - ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] - elif candidate in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H): + elif candidate == CAR.KIA_SPORTAGE_5TH_GEN: + ret.mass = 1700. + STD_CARGO_KG # weight from SX and above trims, average of FWD and AWD versions + ret.wheelbase = 2.756 + ret.steerRatio = 13.6 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications + elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H): ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.KIA_STINGER: - ret.lateralTuning.pid.kf = 0.00005 + if candidate == CAR.KIA_OPTIMA_G4: + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + elif candidate in (CAR.KIA_STINGER, CAR.KIA_STINGER_2022): ret.mass = 1825. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_FORTE: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_CEED: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1450. + STD_CARGO_KG ret.wheelbase = 2.65 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kf = 0.00005 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_K5_2021: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3228. * CV.LB_TO_KG ret.wheelbase = 2.85 ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_EV6: ret.mass = 2055 + STD_CARGO_KG ret.wheelbase = 2.9 ret.steerRatio = 16. tire_stiffness_factor = 0.65 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.IONIQ_5: ret.mass = 2012 + STD_CARGO_KG ret.wheelbase = 3.0 ret.steerRatio = 16. tire_stiffness_factor = 0.65 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: + ret.mass = 1767. + STD_CARGO_KG # SX Prestige trim support only + ret.wheelbase = 2.756 + ret.steerRatio = 13.6 # Genesis elif candidate == CAR.GENESIS_G70: - ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGainBP = [0.] - ret.lateralTuning.indi.innerLoopGainV = [2.5] - ret.lateralTuning.indi.outerLoopGainBP = [0.] - ret.lateralTuning.indi.outerLoopGainV = [3.5] - ret.lateralTuning.indi.timeConstantBP = [0.] - ret.lateralTuning.indi.timeConstantV = [1.4] - ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] - ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] ret.steerActuatorDelay = 0.1 ret.mass = 1640.0 + STD_CARGO_KG ret.wheelbase = 2.84 ret.steerRatio = 13.56 elif candidate == CAR.GENESIS_G70_2020: - ret.lateralTuning.pid.kf = 0. - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.112], [0.004]] ret.mass = 3673.0 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.83 ret.steerRatio = 12.9 + elif candidate == CAR.GENESIS_GV70_1ST_GEN: + ret.mass = 1950. + STD_CARGO_KG + ret.wheelbase = 2.87 + ret.steerRatio = 14.6 elif candidate == CAR.GENESIS_G80: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] elif candidate == CAR.GENESIS_G90: ret.mass = 2200 ret.wheelbase = 3.15 ret.steerRatio = 12.069 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - # panda safety config + # *** longitudinal control *** + if candidate in CANFD_CAR: + ret.longitudinalTuning.kpV = [0.1] + ret.longitudinalTuning.kiV = [0.0] + ret.experimentalLongitudinalAvailable = candidate in (HYBRID_CAR | EV_CAR) and candidate not in CANFD_RADAR_SCC_CAR + else: + ret.longitudinalTuning.kpV = [0.5] + ret.longitudinalTuning.kiV = [0.0] + ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR) + ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable + ret.pcmCruise = not ret.openpilotLongitudinalControl + + ret.stoppingControl = True + ret.startingState = True + ret.vEgoStarting = 0.1 + ret.startAccel = 2.0 + ret.longitudinalActuatorDelayLowerBound = 0.5 + ret.longitudinalActuatorDelayUpperBound = 0.5 + + # *** feature detection *** + if candidate in CANFD_CAR: + bus = 5 if ret.flags & HyundaiFlags.CANFD_HDA2 else 4 + ret.enableBsm = 0x1e5 in fingerprint[bus] + 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)] - # detect HDA2 with LKAS message - if 0x50 in fingerprint[6]: - ret.flags |= HyundaiFlags.CANFD_HDA2.value + if ret.flags & HyundaiFlags.CANFD_HDA2: ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 - else: - # non-HDA2 - if 0x1cf not in fingerprint[4]: - ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value + if ret.flags & HyundaiFlags.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 else: - ret.enableBsm = 0x58b in fingerprint[0] - if candidate in LEGACY_SAFETY_MODE_CAR: # these cars require a special panda safety mode due to missing counters and checksums in the messages ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundaiLegacy)] else: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundai, 0)] - # set appropriate safety param for gas signal - if candidate in HYBRID_CAR: - ret.safetyConfigs[0].safetyParam = 2 - elif candidate in EV_CAR: - ret.safetyConfigs[0].safetyParam = 1 - - if ret.openpilotLongitudinalControl: - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LONG - if candidate in CAMERA_SCC_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + if ret.openpilotLongitudinalControl: + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG + if candidate in HYBRID_CAR: + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS + elif candidate in EV_CAR: + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS + ret.centerToFront = ret.wheelbase * 0.4 # TODO: get actual value, for now starting with reasonable value for @@ -330,13 +279,15 @@ class CarInterface(CarInterfaceBase): # 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, tire_stiffness_factor=tire_stiffness_factor) - return ret @staticmethod def init(CP, logcan, sendcan): - if CP.openpilotLongitudinalControl: - disable_ecu(logcan, sendcan, addr=0x7d0, com_cont_req=b'\x28\x83\x01') + 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 + disable_ecu(logcan, sendcan, bus=bus, addr=addr, 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 aa1823522..ecba7b749 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -3,9 +3,12 @@ from dataclasses import dataclass from typing import Dict, List, Optional, Union 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 CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 + Ecu = car.CarParams.Ecu @@ -26,12 +29,14 @@ class CarControllerParams: self.STEER_DRIVER_ALLOWANCE = 250 self.STEER_DRIVER_MULTIPLIER = 2 self.STEER_THRESHOLD = 250 + self.STEER_DELTA_UP = 2 + self.STEER_DELTA_DOWN = 3 # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. - elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ, + elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, - CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO, CAR.KIA_STINGER): + CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO): self.STEER_MAX = 255 # Default for most HKG @@ -42,6 +47,8 @@ class CarControllerParams: class HyundaiFlags(IntFlag): CANFD_HDA2 = 1 CANFD_ALT_BUTTONS = 2 + CANFD_ALT_GEARS = 4 + CANFD_CAMERA_SCC = 8 class CAR: @@ -49,7 +56,6 @@ class CAR: ELANTRA = "HYUNDAI ELANTRA 2017" ELANTRA_2021 = "HYUNDAI ELANTRA 2021" ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021" - ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019" IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022" @@ -73,6 +79,7 @@ class CAR: SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021" IONIQ_5 = "HYUNDAI IONIQ 5 2022" TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN" + SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN" # Kia KIA_FORTE = "KIA FORTE E 2018 & GT 2021" @@ -80,97 +87,112 @@ class CAR: KIA_NIRO_EV = "KIA NIRO EV 2020" KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" - KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016" + KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN" + KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT" KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" KIA_SELTOS = "KIA SELTOS 2021" + KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN" KIA_SORENTO = "KIA SORENTO GT LINE 2018" + KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN" KIA_STINGER = "KIA STINGER GT2 2018" + KIA_STINGER_2022 = "KIA STINGER 2022" KIA_CEED = "KIA CEED INTRO ED 2019" KIA_EV6 = "KIA EV6 2022" # Genesis GENESIS_G70 = "GENESIS G70 2018" GENESIS_G70_2020 = "GENESIS G70 2020" + GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN" GENESIS_G80 = "GENESIS G80 2017" GENESIS_G90 = "GENESIS G90 2017" @dataclass class HyundaiCarInfo(CarInfo): - # TODO: we can probably remove LKAS. LKAS is standard on many - # HKG and for others, it's likely packaged together with SCC - package: str = "Smart Cruise Control (SCC) & LKAS" + package: str = "Smart Cruise Control (SCC)" 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), + 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 2019", harness=Harness.hyundai_e), + ], CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), - CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-22", "Smart Cruise Control (SCC)", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), - CAR.ELANTRA_GT_I30: None, # dashcamOnly and same platform as CAR.ELANTRA - CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), + CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=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 CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c), - CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "Smart Cruise Control (SCC) & LFA", harness=Harness.hyundai_h), + 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", harness=Harness.hyundai_h), + 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", "Smart Cruise Control (SCC)", harness=Harness.hyundai_h), - CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", "Smart Cruise Control (SCC)", harness=Harness.hyundai_b), + 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", "Smart Cruise Control (SCC)", harness=Harness.hyundai_o), - CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness=Harness.hyundai_i), + 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", video_link="https://youtu.be/VnHzSTygTS4", harness=Harness.hyundai_l), + 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-22", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a), + CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "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.TUCSON: [ - HyundaiCarInfo("Hyundai Tucson 2021", "Smart Cruise Control (SCC)", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l), - HyundaiCarInfo("Hyundai Tucson Diesel 2019", "Smart Cruise Control (SCC)", harness=Harness.hyundai_l), + 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), ], CAR.PALISADE: [ - HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h), - HyundaiCarInfo("Kia Telluride 2020", "All", harness=Harness.hyundai_h), + 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), ], - CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", "Smart Cruise Control (SCC)", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e), + 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.IONIQ_5: HyundaiCarInfo("Hyundai Ioniq 5 2022", "Highway Driving Assist II", harness=Harness.hyundai_q), + CAR.IONIQ_5: [ + 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), + ], 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", "Smart Cruise Control (SCC)", harness=Harness.hyundai_n), # Kia - CAR.KIA_FORTE: [ - HyundaiCarInfo("Kia Forte 2018", harness=Harness.hyundai_b), - HyundaiCarInfo("Kia Forte 2019-21", "All", harness=Harness.hyundai_g), - ], - CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "Smart Cruise Control (SCC)", harness=Harness.hyundai_a), + 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_NIRO_EV: [ - HyundaiCarInfo("Kia Niro Electric 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), - HyundaiCarInfo("Kia Niro Electric 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f), - HyundaiCarInfo("Kia Niro Electric 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), - HyundaiCarInfo("Kia Niro Electric 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), + 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_NIRO_PHEV: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), + 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), 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), ], - CAR.KIA_OPTIMA: [ - HyundaiCarInfo("Kia Optima 2017", min_steer_speed=32. * CV.MPH_TO_MS, harness=Harness.hyundai_b), - HyundaiCarInfo("Kia Optima 2019", harness=Harness.hyundai_g), + 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_OPTIMA_H: [ + HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years + HyundaiCarInfo("Kia Optima Hybrid 2019"), ], - CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima Hybrid 2017, 2019"), # TODO: info may be incorrect - CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", "Smart Cruise Control (SCC)", harness=Harness.hyundai_a), + CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a), + CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", "Smart Cruise Control (SCC)", harness=Harness.hyundai_n), CAR.KIA_SORENTO: [ - HyundaiCarInfo("Kia Sorento 2018", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c), + 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), ], + 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_EV6: HyundaiCarInfo("Kia EV6 2022", "Highway Driving Assist II", harness=Harness.hyundai_p), + CAR.KIA_EV6: [ + 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) + ], # Genesis 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 2017-19", "All", harness=Harness.hyundai_h), CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c), } @@ -186,15 +208,6 @@ FINGERPRINTS = { CAR.ELANTRA: [{ 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }], - CAR.ELANTRA_GT_I30: [{ - 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1193: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1952: 8, 1960: 8, 1988: 8, 2000: 8, 2001: 8, 2005: 8, 2008: 8, 2009: 8, 2013: 8, 2017: 8, 2025: 8 - }, - { - 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8 - }, - { - 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1960: 8, 1990: 8, 1998: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], CAR.HYUNDAI_GENESIS: [{ 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 }, @@ -225,9 +238,6 @@ FINGERPRINTS = { CAR.SONATA_LF: [ {66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8}, ], - CAR.KIA_OPTIMA: [{ - 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8, 1371: 8, 1397: 8, 1961: 8 - }], CAR.KIA_SORENTO: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 }], @@ -272,6 +282,47 @@ FINGERPRINTS = { }], } +HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf100) # 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( + requests=[ + # TODO: minimize shared whitelists for CAN and cornerRadar for CAN-FD + # CAN queries (OBD-II port) + Request( + [HYUNDAI_VERSION_REQUEST_LONG], + [HYUNDAI_VERSION_RESPONSE], + whitelist_ecus=[Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera], + ), + Request( + [HYUNDAI_VERSION_REQUEST_MULTI], + [HYUNDAI_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar], + ), + # CAN-FD queries (camera) + Request( + [HYUNDAI_VERSION_REQUEST_LONG], + [HYUNDAI_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar], + bus=4, + ), + Request( + [HYUNDAI_VERSION_REQUEST_LONG], + [HYUNDAI_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar], + bus=5, + ), + ], + extra_ecus=[ + (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms + (Ecu.cornerRadar, 0x7b7, None), + ], +) FW_VERSIONS = { CAR.IONIQ: { @@ -398,7 +449,7 @@ FW_VERSIONS = { 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.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300', b'\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300', @@ -419,6 +470,7 @@ FW_VERSIONS = { b'\xf1\x82DNBWN5TMDCXXXG2E', b'\xf1\x82DNCVN5GMCCXXXF0A', 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\x87391162M003', b'\xf1\x87391162M013', @@ -443,6 +495,7 @@ FW_VERSIONS = { b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101', b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101', 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', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422', @@ -463,6 +516,7 @@ 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\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', + b'\xf1\x00T02601BL T02832A1 VDN8T25XXX832NS8G\x0e\xfeE', 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', @@ -518,7 +572,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00LF__ SCC F-CUP 1.00 1.00 96401-C2200 ', ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00LF ESC \f 11 \x17\x01\x13 58920-C2610', b'\xf1\x00LF ESC \t 11 \x17\x01\x13 58920-C2610', ], @@ -565,7 +619,7 @@ FW_VERSIONS = { b'\xf1\x00TM__ SCC F-CUP 1.00 1.02 99110-S2000 ', b'\xf1\x00TM__ SCC F-CUP 1.00 1.03 99110-S2000 ', ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650', b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650', b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650', @@ -623,7 +677,7 @@ FW_VERSIONS = { 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.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ 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', @@ -710,6 +764,7 @@ FW_VERSIONS = { b'\xf1\x81640E0051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x82CKJN3TMSDE0B\x00\x00\x00\x00', b'\xf1\x82CKKN3TMD_H0A\x00\x00\x00\x00', + b'\xe0\x19\xff\xe7\xe7g\x01\xa2\x00\x0f\x00\x9e\x00\x06\x00\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\x00\x00\x0f\x0e\x0f\x0f\x0e\r\x00\x00\x7f\x02.\xff\x00\x00~p\x00\x00\x00\x00u\xff\xf9\xff\x00\x00\x00\x00V\t\xd5\x01\xc0\x00\x00\x00\x007\xfb\xfc\x0b\x8d\x00', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5200 4C2CL104', @@ -732,10 +787,31 @@ FW_VERSIONS = { b'\xf1\x87VDKLJ18675252DK6\x89vhgwwwwveVU\x88w\x87w\x99vgf\x97vXfgw_\xff\xc2\xfb\xf1\x89E25\x00\x00\x00\x00\x00\x00\x00\xf1\x82TCK0T33NB2', b'\xf1\x87WAJTE17552812CH4vfFffvfVeT5DwvvVVdFeegeg\x88\x88o\xff\x1a]\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00TCK2T20NB1\x19\xd2\x00\x94', b'\xf1\x87VDHLG17274082DK2wfFf\x89x\x98wUT5T\x88v\x97xgeGefTGTVvO\xff\x1c\x14\xf1\x81E19\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E19\x00\x00\x00\x00\x00\x00\x00SCK0T33UB2\xee[\x97S', + b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\t\xb7\x17\xf5', + b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\t\xb7\x17\xf5', + b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + ], + }, + CAR.KIA_STINGER_2022: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00CK__ SCC F-CUP 1.00 1.00 99110-J5500 ', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640R0051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5380 4C2VR503', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CK MFC AT AUS RHD 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', ], }, CAR.PALISADE: { (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00LX2_ SCC F-CUP 1.00 1.04 99110-S8100 ', b'\xf1\x00LX2_ SCC F-CUP 1.00 1.05 99110-S8100 ', b'\xf1\x00LX2 SCC FHCUP 1.00 1.04 99110-S8100 ', b'\xf1\x00LX2_ SCC FHCU- 1.00 1.05 99110-S8100 ', @@ -744,11 +820,12 @@ FW_VERSIONS = { b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ', b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ', ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360', b'\xf1\x00LX ESC \x01 1031\t\x10 58910-S8360', b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330', b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330', + b'\xf1\x00LX ESC \x0b 103\x19\t\t 58910-S8350', b'\xf1\x00LX ESC \x0b 103\x19\t\x07 58910-S8330', b'\xf1\x00LX ESC \x0b 103\x19\t\x10 58910-S8360', b'\xf1\x00LX ESC \x0b 104 \x10\x16 58910-S8360', @@ -763,6 +840,7 @@ FW_VERSIONS = { ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00LX2 MDPS C 1,00 1,03 56310-S8020 4LXDC103', + b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8000 4LXDC103', b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103', b'\xf1\x00LX2 MDPS C 1.00 1.04 56310-S8020 4LXDC104', b'\xf1\x00ON MDPS C 1.00 1.00 56340-S9000 8B13', @@ -779,6 +857,7 @@ FW_VERSIONS = { ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', + b'\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1', b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', @@ -834,7 +913,7 @@ FW_VERSIONS = { b'\xf1\x00JS__ SCC H-CUP 1.00 1.02 95650-J3200 ', b'\xf1\x00JS__ SCC HNCUP 1.00 1.02 95650-J3100 ', ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816V8RAC00121.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ], @@ -895,7 +974,7 @@ FW_VERSIONS = { }, CAR.KONA: { (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP 1.00 1.00 95655-J9200 ', ], - (Ecu.esp, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ], + (Ecu.abs, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [b'"\x01TOS-0NU06F301J02', ], (Ecu.eps, 0x7d4, None): [b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105', ], (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ], @@ -910,7 +989,7 @@ FW_VERSIONS = { b'\xf1\x816U2V7051\000\000\xf1\0006U2V0_C2\000\0006U2V7051\000\000DCD0T14US1\000\000\000\000', b'\xf1\x816U2V7051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V7051\x00\x00DCD0T14US1U\x867Z', ], - (Ecu.esp, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ], + (Ecu.abs, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ], }, CAR.KIA_FORTE: { (Ecu.eps, 0x7D4, None): [ @@ -928,7 +1007,7 @@ FW_VERSIONS = { b'\x01TBDM1NU06F200H01', b'391182B945\x00', ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ @@ -952,7 +1031,7 @@ FW_VERSIONS = { b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915', b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.04 99210-L3000 210208', ], - (Ecu.esp, 0x7D1, None): [ + (Ecu.abs, 0x7D1, None): [ b'\xf1\000DL ESC \006 101 \004\002 58910-L3200', b'\xf1\x8758910-L3200\xf1\000DL ESC \006 101 \004\002 58910-L3200', b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800', @@ -972,7 +1051,7 @@ FW_VERSIONS = { ], }, CAR.KONA_EV: { - (Ecu.esp, 0x7D1, None): [ + (Ecu.abs, 0x7D1, None): [ b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000', b'\xf1\x00OS IEB \x01 212 \x11\x13 58520-K4000', b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000', @@ -999,10 +1078,11 @@ FW_VERSIONS = { ], }, CAR.KONA_EV_2022: { - (Ecu.esp, 0x7D1, None): [ + (Ecu.abs, 0x7D1, None): [ b'\xf1\x8758520-K4010\xf1\x00OS IEB \x02 101 \x11\x13 58520-K4010', b'\xf1\x8758520-K4010\xf1\x00OS IEB \x04 101 \x11\x13 58520-K4010', b'\xf1\x8758520-K4010\xf1\x00OS IEB \x03 101 \x11\x13 58520-K4010', + b'\xf1\x00OS IEB \r 102"\x05\x16 58520-K4010', # TODO: these return from the MULTI request, above return from LONG b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xd3\x00\x00\x00\x00\xff\xb7\xff\xee\xff\xe0\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xffP\xf5\xff\xfd\x00\x00\x00\x00\xfc\x00\x01', b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xdb\x00\x00\x00\x00\xff\xb1\xff\xd9\xff\xd2\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xff\xd6\xf5\x00\x06\x00\x00\x00\x14\xfd\x00\x04', @@ -1012,10 +1092,12 @@ FW_VERSIONS = { b'\xf1\x00OSP LKA AT CND LHD 1.00 1.02 99211-J9110 802', b'\xf1\x00OSP LKA AT EUR RHD 1.00 1.02 99211-J9110 802', b'\xf1\x00OSP LKA AT AUS RHD 1.00 1.04 99211-J9200 904', + b'\xf1\x00OSP LKA AT EUR LHD 1.00 1.04 99211-J9200 904', ], (Ecu.eps, 0x7D4, None): [ b'\xf1\x00OSP MDPS C 1.00 1.02 56310K4260\x00 4OEPC102', b'\xf1\x00OSP MDPS C 1.00 1.02 56310/K4970 4OEPC102', + b'\xf1\x00OSP MDPS C 1.00 1.02 56310/K4271 4OEPC102', ], (Ecu.fwdRadar, 0x7D0, None): [ b'\xf1\x00YB__ FCA ----- 1.00 1.01 99110-K4500 \x00\x00\x00', @@ -1093,7 +1175,7 @@ FW_VERSIONS = { }, CAR.KIA_SELTOS: { (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 ',], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x8758910-Q5450\xf1\000SP ESC \a 101\031\t\005 58910-Q5450', b'\xf1\x8758910-Q5450\xf1\000SP ESC \t 101\031\t\005 58910-Q5450', ], @@ -1117,27 +1199,59 @@ FW_VERSIONS = { b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS8\r\xfe\x9c\x8b', ], }, - CAR.KIA_OPTIMA: { + CAR.KIA_OPTIMA_G4: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4100 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00JF ESC \x0f 16 \x16\x06\x17 58920-D5080', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00JFWGN LDWS AT USA LHD 1.00 1.02 95895-D4100 G21', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6J0051\x00\x00\xf1\x006T6J0_C2\x00\x006T6J0051\x00\x00TJF0T20NSB\x00\x00\x00\x00', + ], + }, + CAR.KIA_OPTIMA_G4_FL: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 ', ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00JF ESC \x0b 11 \x18\x030 58920-D5180', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x89F1JF600AISEIU702\xf1\x82F1JF600AISEIU702', - ], - (Ecu.eps, 0x7d4, None): [ - b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409', + b"\xf1\x00JF ESC \t 11 \x18\x03' 58920-D5260", ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.00 95895-D5001 h32', - b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.02 95895-D5000 h31', + b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.00 95895-D5100 h32', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW', + b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\xca3\xeb.', + b'\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DJF0T16NL2\x9eA\x80\x01', + b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\x00\x00\x00\x00', b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW', b'\xf1\x816U2VA051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\xca3\xeb.', + b'\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DJF0T16NL2\x9eA\x80\x01', b'\xf1\x816U2VA051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\x00\x00\x00\x00', + b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B8051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B8051\x00\x00TJFSG24NH27\xa7\xc2\xb4', + ], + }, + CAR.ELANTRA: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00PD LKAS AT USA LHD 1.01 1.01 95740-G3100 A54', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DPD0H16NS0e\x0e\xcd\x8e', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00PD MDPS C 1.00 1.04 56310/G3300 4PDDC104', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00PD ESC \x0b 104\x18\t\x03 58920-G3350', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00PD__ SCC F-CUP 1.00 1.00 96400-G3300 ', ], }, CAR.ELANTRA_2021: { @@ -1151,6 +1265,7 @@ FW_VERSIONS = { b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106', b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106', b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', + b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106\xf1\xa01.06', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819', @@ -1158,11 +1273,12 @@ FW_VERSIONS = { b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.06 99210-AA000 220111', ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800', b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', + b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800\xf1\xa01.01', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1175,36 +1291,39 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x82CNCWD0AMFCXCSFFA', - b'\xf1\x82CNCWD0AMFCXCSFFB', + b'\xf1\x81HM6M2_0a0_FF0', b'\xf1\x82CNCVD0AMFCXCSFFB', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_G80', ], }, CAR.ELANTRA_HEV_2021: { - (Ecu.fwdCamera, 0x7c4, None) : [ + (Ecu.fwdCamera, 0x7c4, None): [ 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', ], - (Ecu.fwdRadar, 0x7d0, None) : [ + (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\000CNhe 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) :[ + (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', ], - (Ecu.transmission, 0x7e1, None) :[ + (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', ], - (Ecu.engine, 0x7e0, None) : [ + (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', ] }, CAR.KONA_HEV: { - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00OS IEB \x01 104 \x11 58520-CM000', ], (Ecu.fwdRadar, 0x7d0, None): [ @@ -1258,7 +1377,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00UMP LKAS AT USA LHD 1.01 1.01 95740-C6550 d01' ], - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00UM ESC \x0c 12 \x18\x05\x06 58910-C6330' ], (Ecu.fwdRadar, 0x7D0, None): [ @@ -1272,50 +1391,69 @@ FW_VERSIONS = { ], }, CAR.KIA_EV6: { - (Ecu.esp, 0x7d1, None): [ - b'\xf1\x8758520CV100\xf1\x00CV IEB \x02 101!\x10\x18 58520-CV100', - ], - (Ecu.eps, 0x7d4, None): [ - b'\xf1\x00CV1 MDPS R 1.00 1.04 57700-CV000 1B30', - ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ', b'\xf1\x8799110CV000\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.05 99210-CV000 211027', + b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.06 99210-CV000 220328', + b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.05 99210-CV000 211027', + b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.06 99210-CV000 220328', ], }, CAR.IONIQ_5: { - (Ecu.esp, 0x7d1, None): [ - b'\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010', - b'\xf1\x8758520GI010\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010', - ], - (Ecu.eps, 0x7d4, None): [ - b'\xf1\x00NE MDPS R 1.00 1.06 57700GI000 4NEDR106', - b'\xf1\x8757700GI000 \xf1\x00NE MDPS R 1.00 1.06 57700GI000 4NEDR106', - ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NE1_ RDR ----- 1.00 1.00 99110-GI000 ', b'\xf1\x8799110GI000\xf1\x00NE1_ RDR ----- 1.00 1.00 99110-GI000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ 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', ], }, CAR.TUCSON_HYBRID_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ 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', ], - (Ecu.eps, 0x7d4, None): [ - b'\xf1\x00NX4 MDPS C 1.00 1.01 56300-P0100 2228', + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x87391312MND0', + }, + CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1060 665', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00PSBG2441 G19_Rev\x00\x00\x00SNX4T16XXHS01NS2lS\xdfa', - b'\xf1\x8795441-3D220\x00\xf1\x81G19_Rev\x00\x00\x00\xf1\x00PSBG2441 G19_Rev\x00\x00\x00SNX4T16XXHS01NS2lS\xdfa', + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NQ5__ 1.01 1.03 99110-CH000 ', + ], + }, + CAR.SANTA_CRUZ_1ST_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW000 14M', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NX4__ 1.00 1.00 99110-K5000 ', + ], + }, + CAR.KIA_SPORTAGE_5TH_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1030 662', + b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1040 663', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NQ5__ 1.00 1.02 99110-P1000 ', + b'\xf1\x00NQ5__ 1.00 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', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ', ], }, } @@ -1327,24 +1465,27 @@ CHECKSUM = { FEATURES = { # which message has the gear - "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA}, - "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, + "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}, # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 - "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022}, + "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022}, } -CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_HYBRID_4TH_GEN} +CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, 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} + +# The radar does SCC on these cars when HDA I, rather than the camera +CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, } # 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} # 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} +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} # 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} # 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, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} +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, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py @@ -1352,44 +1493,49 @@ DBC = { CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None), CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None), CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None), CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None), CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None), CAR.IONIQ_PHEV_2019: dbc_dict('hyundai_kia_generic', None), CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None), CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.IONIQ: dbc_dict('hyundai_kia_generic', None), CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None), CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), - CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), + CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_STINGER_2022: dbc_dict('hyundai_kia_generic', None), CAR.KONA: dbc_dict('hyundai_kia_generic', None), CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None), CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None), CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format CAR.TUCSON: dbc_dict('hyundai_kia_generic', None), - CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None), CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None), CAR.KIA_EV6: dbc_dict('hyundai_canfd', None), - CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.TUCSON_HYBRID_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.IONIQ_5: dbc_dict('hyundai_canfd', None), + CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None), + CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None), + CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: dbc_dict('hyundai_canfd', None), + CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None), } diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 9da6aabd6..8e8872a53 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -2,24 +2,28 @@ import yaml import os import time from abc import abstractmethod, ABC -from typing import Any, Dict, Optional, Tuple, List +from typing import Any, Dict, Optional, Tuple, List, Callable 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 interp from common.realtime import DT_CTRL -from selfdrive.car import create_button_enable_events, gen_empty_fingerprint -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from selfdrive.car import apply_hysteresis, gen_empty_fingerprint +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_deadzone from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel +ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName +TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, bool], float] MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 ACCEL_MIN = -3.5 +FRICTION_THRESHOLD = 0.3 TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yaml') TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml') @@ -85,7 +89,7 @@ class CarInterfaceBase(ABC): @staticmethod @abstractmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): pass @staticmethod @@ -101,17 +105,34 @@ class CarInterfaceBase(ABC): def get_steer_feedforward_function(self): 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): + # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) + friction_interp = interp( + apply_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 + + def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: + return self.torque_from_lateral_accel_linear + # returns a set of default params to avoid repetition in car specific params @staticmethod def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate + # Car docs fields + ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED'] + ret.autoResumeSng = True # describes whether car can resume from a stop automatically + # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque ret.minSteerSpeed = 0. ret.wheelSpeedFactor = 1.0 - ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED'] ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this @@ -141,10 +162,12 @@ class CarInterfaceBase(ABC): tune.init('torque') tune.torque.useSteeringAngle = use_steering_angle - tune.torque.kp = 1.0 / params['LAT_ACCEL_FACTOR'] - tune.torque.kf = 1.0 / params['LAT_ACCEL_FACTOR'] - tune.torque.ki = 0.1 / params['LAT_ACCEL_FACTOR'] + tune.torque.kp = 1.0 + tune.torque.kf = 1.0 + tune.torque.ki = 0.1 tune.torque.friction = params['FRICTION'] + tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] + tune.torque.latAccelOffset = 0.0 tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg @abstractmethod @@ -168,6 +191,12 @@ class CarInterfaceBase(ABC): else: self.v_ego_cluster_seen = True + # Many cars apply hysteresis to the ego dash speed + if self.CS is not None: + ret.vEgoCluster = apply_hysteresis(ret.vEgoCluster, self.CS.out.vEgoCluster, self.CS.cluster_speed_hyst_gap) + if abs(ret.vEgo) < self.CS.cluster_min_speed: + ret.vEgoCluster = 0.0 + if ret.cruiseState.speedCluster == 0: ret.cruiseState.speedCluster = ret.cruiseState.speed @@ -182,7 +211,8 @@ class CarInterfaceBase(ABC): def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]: pass - def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True): + def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True, + enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): events = Events() if cs_out.doorOpen: @@ -212,9 +242,17 @@ class CarInterfaceBase(ABC): events.add(EventName.parkBrake) if cs_out.accFaulted: events.add(EventName.accFaulted) + if cs_out.steeringPressed: + events.add(EventName.steerOverride) # Handle button presses - events.events.extend(create_button_enable_events(cs_out.buttonEvents, pcm_cruise=self.CP.pcmCruise)) + for b in cs_out.buttonEvents: + # Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port) + if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed): + events.add(EventName.buttonEnable) + # Disable on rising and falling edge of cancel for both stock and OP long + if b.type == ButtonType.cancel: + events.add(EventName.buttonCancel) # Handle permanent and temporary steering faults self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1 @@ -267,13 +305,15 @@ class CarStateBase(ABC): self.right_blinker_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False + self.cluster_speed_hyst_gap = 0.0 + self.cluster_min_speed = 0.0 # min speed before dropping to 0 - # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) - # R = 1e3 + # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) + # R = 0.3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], A=[[1.0, DT_CTRL], [0.0, 1.0]], C=[1.0, 0.0], - K=[[0.12287673], [0.29666309]]) + K=[[0.17406039], [1.65925647]]) def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 65122ab89..4b4bdcc0c 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -1,7 +1,6 @@ import time from collections import defaultdict from functools import partial -from typing import Optional import cereal.messaging as messaging from system.swaglog import cloudlog @@ -10,24 +9,21 @@ from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_a class IsoTpParallelQuery: - def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addr=False, debug=False, response_pending_timeout=10): + def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addrs=None, debug=False, response_pending_timeout=10): self.sendcan = sendcan self.logcan = logcan self.bus = bus self.request = request self.response = response + self.functional_addrs = functional_addrs or [] self.debug = debug - self.functional_addr = functional_addr self.response_pending_timeout = response_pending_timeout - self.real_addrs = [] - for a in addrs: - if isinstance(a, tuple): - self.real_addrs.append(a) - else: - self.real_addrs.append((a, None)) + real_addrs = [a if isinstance(a, tuple) else (a, None) for a in addrs] + for tx_addr, _ in real_addrs: + assert tx_addr not in FUNCTIONAL_ADDRS, f"Functional address should be defined in functional_addrs: {hex(tx_addr)}" - self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in self.real_addrs} + self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in real_addrs} self.msg_buffer = defaultdict(list) def rx(self): @@ -36,13 +32,8 @@ class IsoTpParallelQuery: for packet in can_packets: for msg in packet.can: - if msg.src == self.bus: - if self.functional_addr: - if (0x7E8 <= msg.address <= 0x7EF) or (0x18DAF100 <= msg.address <= 0x18DAF1FF): - fn_addr = next(a for a in FUNCTIONAL_ADDRS if msg.address - a <= 32) - self.msg_buffer[fn_addr].append((msg.address, msg.busTime, msg.dat, msg.src)) - elif msg.address in self.msg_addrs.values(): - self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src)) + if msg.src == self.bus and msg.address in self.msg_addrs.values(): + self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src)) def _can_tx(self, tx_addr, dat, bus): """Helper function to send single message""" @@ -72,6 +63,16 @@ class IsoTpParallelQuery: messaging.drain_sock(self.logcan) self.msg_buffer = defaultdict(list) + def _create_isotp_msg(self, tx_addr, sub_addr, rx_addr): + can_client = CanClient(self._can_tx, partial(self._can_rx, rx_addr, sub_addr=sub_addr), tx_addr, rx_addr, + self.bus, sub_addr=sub_addr, debug=self.debug) + + max_len = 8 if sub_addr is None else 7 + # uses iso-tp frame separation time of 10 ms + # TODO: use single_frame_mode so ECUs can send as fast as they want, + # as well as reduces chances we process messages from previous queries + return IsoTpMessage(can_client, timeout=0, separation_time=0.01, debug=self.debug, max_len=max_len) + def get_data(self, timeout, total_timeout=60.): self._drain_rx() @@ -80,22 +81,19 @@ class IsoTpParallelQuery: request_counter = {} request_done = {} for tx_addr, rx_addr in self.msg_addrs.items(): - # rx_addr not set when using functional tx addr - id_addr = rx_addr or tx_addr[0] - sub_addr = tx_addr[1] - - can_client = CanClient(self._can_tx, partial(self._can_rx, id_addr, sub_addr=sub_addr), tx_addr[0], rx_addr, - self.bus, sub_addr=sub_addr, debug=self.debug) - - max_len = 8 if sub_addr is None else 7 - - msg = IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug) - msg.send(self.request[0]) - - msgs[tx_addr] = msg + msgs[tx_addr] = self._create_isotp_msg(*tx_addr, rx_addr) request_counter[tx_addr] = 0 request_done[tx_addr] = False + # Send first request to functional addrs, subsequent responses are handled on physical addrs + if len(self.functional_addrs): + for addr in self.functional_addrs: + self._create_isotp_msg(addr, None, -1).send(self.request[0]) + + # If querying functional addrs, set up physical IsoTpMessages to send consecutive frames + for msg in msgs.values(): + msg.send(self.request[0], setup_only=len(self.functional_addrs) > 0) + results = {} start_time = time.monotonic() response_timeouts = {tx_addr: start_time + timeout for tx_addr in self.msg_addrs} @@ -107,12 +105,15 @@ class IsoTpParallelQuery: for tx_addr, msg in msgs.items(): try: - dat: Optional[bytes] = msg.recv() + dat, updated = msg.recv() except Exception: - cloudlog.exception("Error processing UDS response") + cloudlog.exception(f"Error processing UDS response: {tx_addr}") request_done[tx_addr] = True continue + if updated: + response_timeouts[tx_addr] = time.monotonic() + timeout + if not dat: continue @@ -121,12 +122,11 @@ class IsoTpParallelQuery: response_valid = dat[:len(expected_response)] == expected_response if response_valid: - response_timeouts[tx_addr] = time.monotonic() + timeout if counter + 1 < len(self.request): msg.send(self.request[counter + 1]) request_counter[tx_addr] += 1 else: - results[(tx_addr, msg._can_client.rx_addr)] = dat[len(expected_response):] + results[tx_addr] = dat[len(expected_response):] request_done[tx_addr] = True else: error_code = dat[2] if len(dat) > 2 else -1 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 864090344..7c42431e3 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -11,11 +11,7 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod - def compute_gb(accel, speed): - return float(accel) / 4.0 - - @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "mazda" diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index ed246f15d..129273efe 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -2,9 +2,11 @@ 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 cereal import car +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + Ecu = car.CarParams.Ecu @@ -41,7 +43,7 @@ CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"), CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-22", video_link="https://youtu.be/dA3duO4a0O4"), - CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022"), + CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-23"), } @@ -50,6 +52,7 @@ class LKAS_LIMITS: DISABLE_SPEED = 45 # kph ENABLE_SPEED = 52 # kph + class Buttons: NONE = 0 SET_PLUS = 1 @@ -58,8 +61,17 @@ class Buttons: CANCEL = 4 +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + ), + ], +) + FW_VERSIONS = { - CAR.CX5_2022 : { + CAR.CX5_2022: { (Ecu.eps, 0x730, None): [ b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -72,11 +84,12 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x764, None): [ b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -86,13 +99,15 @@ FW_VERSIONS = { }, CAR.CX5: { (Ecu.eps, 0x730, None): [ + b'K319-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KCB8-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KJ01-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KJ01-3210X-M-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', - b'K319-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'PA53-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PAR4-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYFA-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -116,7 +131,7 @@ FW_VERSIONS = { b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KBJ5-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KL2K-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -134,6 +149,7 @@ FW_VERSIONS = { ], (Ecu.transmission, 0x7e1, None): [ b'PA66-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PA66-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX39-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX39-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -173,7 +189,7 @@ FW_VERSIONS = { b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'TK80-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'TK79-437K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -217,7 +233,7 @@ FW_VERSIONS = { b'GHP9-67Y10---41\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'B45A-437AS-0-08\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ @@ -248,7 +264,7 @@ FW_VERSIONS = { b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-E\000\000\000\000\000\000\000\000\000\000\000\000', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'GBVH-437K2-B\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', ], @@ -275,7 +291,7 @@ FW_VERSIONS = { b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x760, None): [ + (Ecu.abs, 0x760, None): [ b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 715850c22..a3194cd79 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -20,8 +20,7 @@ class CarInterface(CarInterfaceBase): cloudlog.debug("Using Mock Car Interface") - self.sensor = messaging.sub_sock('sensorEvents') - self.gps = messaging.sub_sock('gpsLocationExternal') + self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal']) self.speed = 0. self.prev_speed = 0. @@ -29,11 +28,7 @@ class CarInterface(CarInterfaceBase): self.yaw_rate_meas = 0. @staticmethod - def compute_gb(accel, speed): - return accel - - @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "mock" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)] @@ -49,17 +44,16 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): - # get basic data from phone and gps since CAN isn't connected - sensors = messaging.recv_sock(self.sensor) - if sensors is not None: - for sensor in sensors.sensorEvents: - if sensor.type == 4: # gyro - self.yaw_rate_meas = -sensor.gyro.v[0] + self.sm.update(0) - gps = messaging.recv_sock(self.gps) - if gps is not None: + # get basic data from phone and gps since CAN isn't connected + if self.sm.updated['gyroscope']: + self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0] + + gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' + if self.sm.updated[gps_sock]: self.prev_speed = self.speed - self.speed = gps.gpsLocationExternal.speed + self.speed = self.sm[gps_sock].speed # create message ret = car.CarState.new_message() diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index a5ca23711..d6b6d17d5 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -44,7 +44,7 @@ class CarState(CarStateBase): ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.vEgoRaw < 0.01 + ret.standstill = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] == 0.0 and cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] == 0.0 if self.CP.carFingerprint == CAR.ALTIMA: ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 9c04d975f..e095ceb46 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -8,33 +8,32 @@ from selfdrive.car.nissan.values import CAR class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] + ret.autoResumeSng = False ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.1 + ret.steerRatio = 17 if candidate in (CAR.ROGUE, CAR.XTRAIL): ret.mass = 1610 + STD_CARGO_KG ret.wheelbase = 2.705 ret.centerToFront = ret.wheelbase * 0.44 - ret.steerRatio = 17 elif candidate in (CAR.LEAF, CAR.LEAF_IC): ret.mass = 1610 + STD_CARGO_KG ret.wheelbase = 2.705 ret.centerToFront = ret.wheelbase * 0.44 - ret.steerRatio = 17 elif candidate == CAR.ALTIMA: # Altima has EPS on C-CAN unlike the others that have it on V-CAN ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus ret.mass = 1492 + STD_CARGO_KG ret.wheelbase = 2.824 ret.centerToFront = ret.wheelbase * 0.44 - ret.steerRatio = 17 ret.steerControlType = car.CarParams.SteerControlType.angle ret.radarOffCan = True diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index f59714b8d..01fb3463a 100644 --- a/selfdrive/car/nissan/nissancan.py +++ b/selfdrive/car/nissan/nissancan.py @@ -24,10 +24,7 @@ def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torqu def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg): values = copy.copy(cruise_throttle_msg) - can_bus = 2 - - if car_fingerprint == CAR.ALTIMA: - can_bus = 1 + can_bus = 1 if car_fingerprint == CAR.ALTIMA else 2 values["CANCEL_BUTTON"] = 1 values["NO_BUTTON_PRESSED"] = 0 diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 2126e550e..09bd7ca83 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -2,9 +2,12 @@ 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 dbc_dict from selfdrive.car.docs_definitions import CarInfo, Harness -from cereal import car +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + Ecu = car.CarParams.Ecu @@ -75,6 +78,33 @@ FINGERPRINTS = { ] } +NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xc0]) +NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xc0]) + +NISSAN_VERSION_REQUEST_KWP = b'\x21\x83' +NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83' + +NISSAN_RX_OFFSET = 0x20 + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], + [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], + ), + Request( + [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], + [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], + rx_offset=NISSAN_RX_OFFSET, + ), + Request( + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + rx_offset=NISSAN_RX_OFFSET, + ), + ], +) + FW_VERSIONS = { CAR.ALTIMA: { (Ecu.fwdCamera, 0x707, None): [ @@ -95,7 +125,7 @@ FW_VERSIONS = { 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', ], - (Ecu.esp, 0x740, None): [ + (Ecu.abs, 0x740, None): [ b'476605SH1D', b'476605SK2A', ], @@ -112,7 +142,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x707, None): [ b'284N86FR2A', ], - (Ecu.esp, 0x740, None): [ + (Ecu.abs, 0x740, None): [ b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80', b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80', ], diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 7fc5456d9..ba873c48d 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -32,9 +32,8 @@ class CarState(CarStateBase): cp_wheels.vl["Wheel_Speeds"]["RR"], ) ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. - # Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.vEgoRaw < 0.01 + ret.standstill = ret.vEgoRaw == 0 # continuous blinker signals for assisted lane change ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"], @@ -50,7 +49,7 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"] ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"] ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"] - + steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80 ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold @@ -59,9 +58,6 @@ class CarState(CarStateBase): ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS - if self.car_fingerprint not in PREGLOBAL_CARS: - ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 - if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1): ret.cruiseState.speed *= CV.MPH_TO_KPH @@ -79,6 +75,8 @@ class CarState(CarStateBase): else: ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1 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"]) cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam @@ -313,4 +311,4 @@ class CarState(CarStateBase): checks += CarState.get_global_es_distance_signals()[1] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) - return None \ No newline at end of file + return None diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index a464734a3..a920c0253 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -9,12 +9,13 @@ from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "subaru" ret.radarOffCan = True ret.dashcamOnly = candidate in PREGLOBAL_CARS + ret.autoResumeSng = False if candidate in PREGLOBAL_CARS: ret.enableBsm = 0x25c in fingerprint[0] diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 7d8365a11..7a1e9a8a3 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -2,9 +2,11 @@ from dataclasses import dataclass from enum import Enum 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 cereal import car +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -71,10 +73,23 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { CAR.OUTBACK_PREGLOBAL_2018: SubaruCarInfo("Subaru Outback 2018-19"), } +SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) +SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], + ), + ], +) FW_VERSIONS = { CAR.ASCENT: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\xa5 \x19\x02\x00', b'\xa5 !\002\000', b'\xf1\x82\xa5 \x19\x02\x00', @@ -89,6 +104,7 @@ FW_VERSIONS = { b'\000\000e~\037@ \'', b'\x00\x00e@\x1f@ $', b'\x00\x00d\xb9\x00\x00\x00\x00', + b'\x00\x00e@\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\xbb,\xa0t\a', @@ -96,32 +112,42 @@ FW_VERSIONS = { b'\xf1\x82\xbb,\xa0t\a', b'\xf1\x82\xd9,\xa0@\a', b'\xf1\x82\xd1,\xa0q\x07', + b'\xd1,\xa0q\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\x00\xfe\xf7\x00\x00', b'\001\xfe\xf9\000\000', b'\x01\xfe\xf7\x00\x00', + b'\x01\xfe\xfa\x00\x00', ], }, CAR.LEGACY: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\xa1\\ x04\x01', + b'\xa1 \x03\x03', + b'\xa1 \x02\x01', ], (Ecu.eps, 0x746, None): [ b'\x9b\xc0\x11\x00', + b'\x9b\xc0\x11\x02', ], (Ecu.fwdCamera, 0x787, None): [ b'\x00\x00e\x80\x00\x1f@ \x19\x00', + b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\xde\"a0\x07', + b'\xe2"aq\x07', + b'\xde,\xa0@\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xa5\xf6\x05@\x00', + b'\xa7\xf6\x04@\x00', + b'\xa5\xfe\xc7@\x00', ], }, CAR.IMPREZA: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x7a\x94\x3f\x90\x00', b'\xa2 \x185\x00', b'\xa2 \x193\x00', @@ -170,6 +196,7 @@ FW_VERSIONS = { b'\xaa!dt\a', b'\xc5!ar\a', b'\xbe!as\a', + b'\xc5!as\x07', b'\xc5!ds\a', b'\xc5!`s\a', b'\xaa!au\a', @@ -196,7 +223,7 @@ FW_VERSIONS = { ], }, CAR.IMPREZA_2020: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\xa2 \0314\000', b'\xa2 \0313\000', b'\xa2 !i\000', @@ -234,7 +261,7 @@ FW_VERSIONS = { ], }, CAR.FORESTER: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\xa3 \x18\x14\x00', b'\xa3 \024\000', b'\xa3 \031\024\000', @@ -269,7 +296,7 @@ FW_VERSIONS = { ], }, CAR.FORESTER_PREGLOBAL: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x7d\x97\x14\x40', b'\xf1\x00\xbb\x0c\x04', ], @@ -298,7 +325,7 @@ FW_VERSIONS = { ], }, CAR.LEGACY_PREGLOBAL: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'k\x97D\x00', b'[\xba\xc4\x03', b'{\x97D\x00', @@ -328,7 +355,7 @@ FW_VERSIONS = { ], }, CAR.OUTBACK_PREGLOBAL: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'{\x9a\xac\x00', b'k\x97\xac\x00', b'\x5b\xf7\xbc\x03', @@ -382,7 +409,7 @@ FW_VERSIONS = { ], }, CAR.OUTBACK_PREGLOBAL_2018: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x8b\x97\xac\x00', b'\x8b\x9a\xac\x00', b'\x9b\x97\xac\x00', @@ -425,7 +452,7 @@ FW_VERSIONS = { ], }, CAR.OUTBACK: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\xa1 \x06\x01', b'\xa1 \a\x00', b'\xa1 \b\001', diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 5e78c72b6..2eb29efb4 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(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "tesla" diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 296169587..e28666c62 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -1,9 +1,12 @@ from collections import namedtuple from typing import Dict, List, Union +from cereal import car from selfdrive.car import dbc_dict from selfdrive.car.docs_definitions import CarInfo -from cereal import car +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + +Ecu = car.CarParams.Ecu Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) @@ -20,11 +23,6 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { } FINGERPRINTS = { - CAR.AP2_MODELS: [ - { - 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 6, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2015: 8, 2043: 5, 2045: 4 - }, - ], CAR.AP1_MODELS: [ { 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4 @@ -37,6 +35,42 @@ DBC = { CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), } +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], + whitelist_ecus=[Ecu.eps], + rx_offset=0x08, + bus=0, + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], + whitelist_ecus=[Ecu.adas, Ecu.electricBrakeBooster, Ecu.fwdRadar], + rx_offset=0x10, + bus=0, + ), + ] +) + +FW_VERSIONS = { + CAR.AP2_MODELS: { + (Ecu.adas, 0x649, None): [ + b'\x01\x00\x8b\x07\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x11', + ], + (Ecu.electricBrakeBooster, 0x64d, None): [ + b'1037123-00-A', + ], + (Ecu.fwdRadar, 0x671, None): [ + b'\x01\x00W\x00\x00\x00\x07\x00\x00\x00\x00\x08\x01\x00\x00\x00\x07\xff\xfe', + ], + (Ecu.eps, 0x730, None): [ + b'\x10#\x01', + ], + }, +} + class CANBUS: # Lateral harness chassis = 0 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index aabf652c8..8d2217367 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -34,15 +34,18 @@ class TestCarInterfaces(unittest.TestCase): self.assertGreater(car_params.maxLateralAccel, 0) if car_params.steerControlType != car.CarParams.SteerControlType.angle: - tuning = car_params.lateralTuning.which() - if tuning == 'pid': - self.assertTrue(len(car_params.lateralTuning.pid.kpV)) - elif tuning == 'torque': - kf = car_params.lateralTuning.torque.kf - self.assertTrue(not math.isnan(kf) and kf > 0) - self.assertTrue(not math.isnan(car_params.lateralTuning.torque.friction)) - elif tuning == 'indi': - self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) + tune = car_params.lateralTuning + if tune.which() == 'pid': + self.assertTrue(not math.isnan(tune.pid.kf) and tune.pid.kf > 0) + self.assertTrue(len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP)) + self.assertTrue(len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP)) + + 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)) + + elif tune.which() == 'indi': + self.assertTrue(len(tune.indi.outerLoopGainV)) # Run car interface CC = car.CarControl.new_message() diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 14ee7e223..6ec782444 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -13,6 +13,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan] # Guess FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan] +FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan] FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] ### @@ -20,14 +21,19 @@ FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] COMMA BODY: [.nan, 1000, .nan] # Totally new cars -KIA EV6 2022: [3.5, 2.5, 0.0] RAM 1500 5TH GEN: [2.0, 2.0, 0.0] RAM HD 5TH GEN: [1.4, 1.4, 0.0] SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11] +CHEVROLET BOLT EV 2022: [2.0, 2.0, 0.05] CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05] CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112] +CHEVROLET EQUINOX 2019: [2.0, 2.0, 0.05] VOLKSWAGEN PASSAT NMS: [2.5, 2.5, 0.1] -HYUNDAI TUCSON HYBRID 4TH GEN: [2.5, 2.5, 0.0] +VOLKSWAGEN SHARAN 2ND GEN: [2.5, 2.5, 0.1] +HYUNDAI SANTA CRUZ 1ST GEN: [2.7, 2.7, 0.1] +KIA SPORTAGE 5TH GEN: [2.7, 2.7, 0.1] +KIA SPORTAGE HYBRID 5TH GEN: [2.5, 2.5, 0.1] +GENESIS GV70 1ST GEN: [2.42, 2.42, 0.1] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index 160f60548..a9023b4ed 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -25,6 +25,7 @@ HONDA ODYSSEY 2018: [1.8774809275211801, 0.8394431662987996, 0.2096978613792822] HONDA PASSPORT 2021: [1.5305538930036766, 0.7956063674638759, 0.19599407381531284] HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] +HYUNDAI ELANTRA 2021: [3.169, 2.1259108157250735, 0.0819] HYUNDAI GENESIS 2015-2016: [1.8466226943929824, 1.5552063647830634, 0.0984484465421171] HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276] HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] @@ -35,21 +36,23 @@ HYUNDAI SANTA FE 2019: [3.0787027729757632, 2.6173437483495565, 0.12070193418239 HYUNDAI SANTA FE HYBRID 2022: [3.501877602644835, 2.729064118456137, 0.10384068104538963] HYUNDAI SANTA FE PlUG-IN HYBRID 2022: [1.6953050513611045, 1.5837614296206861, 0.12672855941458458] HYUNDAI SONATA 2019: [2.2200457811703953, 1.2967330275895228, 0.14039920986586393] -HYUNDAI SONATA 2020: [3.284505627881726, 2.1259108157250735, 0.08452048323586728] +HYUNDAI SONATA 2020: [2.9638737459977467, 2.1259108157250735, 0.07813665616927593] HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488717382] +HYUNDAI TUCSON HYBRID 4TH GEN: [2.035545, 2.035545, 0.110272] JEEP GRAND CHEROKEE 2019: [1.7321233388827006, 1.289689569171081, 0.15046331002097185] JEEP GRAND CHEROKEE V6 2018: [1.8776598027756923, 1.4057367824262523, 0.11725947414922003] +KIA EV6 2022: [3.2, 3.0, 0.05] KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716] KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267] KIA SORENTO GT LINE 2018: [2.464854685101844, 1.5335274218367956, 0.12056170567599558] KIA STINGER GT2 2018: [2.7499043387418967, 1.849652021986449, 0.12048334239559202] -LEXUS ES 2019: [2.0203086922726112, 2.134803912579666, 0.12757526789308554] -LEXUS ES HYBRID 2019: [2.392442298703042, 1.863360677810788, 0.17690002108856212] +LEXUS ES 2019: [1.935835, 2.134803912579666, 0.093439] +LEXUS ES HYBRID 2019: [2.135678, 1.863360677810788, 0.109627] LEXUS NX 2018: [2.302625600642627, 2.1382378491466625, 0.14986840878892838] LEXUS NX 2020: [2.4331999786982936, 2.1045680431705414, 0.14099899317761067] LEXUS NX HYBRID 2018: [2.4025593501080955, 1.8080446063815507, 0.15349361249519017] LEXUS RX 2016: [1.5876816543130423, 1.0427699298523752, 0.21334066732397142] -LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.2093316728710659] +LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.164117] LEXUS RX HYBRID 2017: [1.6984261557042386, 1.3211501880159107, 0.1820354534928893] LEXUS RX HYBRID 2020: [1.5522309889823778, 1.255230465866663, 0.2220954003055114] MAZDA CX-9 2021: [1.7601682915983443, 1.0889677335154337, 0.17713792194297195] @@ -62,31 +65,31 @@ TOYOTA AVALON 2019: [1.7036141952825095, 1.239619084240008, 0.08459830394899492] TOYOTA AVALON 2022: [2.3154403649717357, 2.7777922854327124, 0.11453999639164605] TOYOTA C-HR 2018: [1.5591084333664578, 1.271271459066948, 0.20259087058453193] TOYOTA C-HR 2021: [1.7678810166088303, 1.3742176337919942, 0.2319674583741509] -TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.13519250664782062] -TOYOTA CAMRY 2021: [2.6922769557433055, 2.3476510120007434, 0.1450430192989234] -TOYOTA CAMRY HYBRID 2018: [2.0974120828287774, 1.7996193116697359, 0.13823613467632756] -TOYOTA CAMRY HYBRID 2021: [2.6426668350384457, 2.3901492458927986, 0.16103875108816076] +TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.105192506] +TOYOTA CAMRY 2021: [2.446083, 2.3476510120007434, 0.121615] +TOYOTA CAMRY HYBRID 2018: [1.996333, 1.7996193116697359, 0.112565] +TOYOTA CAMRY HYBRID 2021: [2.263582, 2.3901492458927986, 0.115257] TOYOTA COROLLA 2017: [3.117154369115421, 1.8438132575043773, 0.12289685869250652] -TOYOTA COROLLA HYBRID TSS2 2019: [2.3287672277252005, 1.8118712531729109, 0.2215868445753317] -TOYOTA COROLLA TSS2 2019: [2.4204464833010175, 1.9258612322678952, 0.20670411068012526] +TOYOTA COROLLA HYBRID TSS2 2019: [1.9079729107361805, 1.8118712531729109, 0.22251440891543514] +TOYOTA COROLLA TSS2 2019: [2.0742917676766712, 1.9258612322678952, 0.16888685704519352] TOYOTA HIGHLANDER 2017: [1.8696367437248915, 1.626293990451463, 0.17485372210240796] TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054457] -TOYOTA HIGHLANDER HYBRID 2018: [1.9421825202382728, 1.6433903296845025, 0.16928956792275918] -TOYOTA HIGHLANDER HYBRID 2020: [2.103373061114133, 2.104015182965606, 0.14447040132184993] +TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600] +TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993] TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565] -TOYOTA PRIUS 2017: [2.0183401513314294, 1.5023147650693636, 0.20856908464957724] -TOYOTA PRIUS TSS2 2021: [2.327639738920072, 1.9104337425537743, 0.2030762265549664] +TOYOTA PRIUS 2017: [1.746445, 1.5023147650693636, 0.151515] +TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968] TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975] -TOYOTA RAV4 2019: [2.5038362866776835, 2.0993589721530252, 0.1552425356342368] +TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822] TOYOTA RAV4 2019 8965: [2.5084506298290377, 2.4216520504763475, 0.11992835265067918] TOYOTA RAV4 2019 x02: [2.7209621987605024, 2.2148637653781593, 0.10862567142268198] TOYOTA RAV4 HYBRID 2017: [1.9796257271652042, 1.7503987331707576, 0.14628860048885406] TOYOTA RAV4 HYBRID 2019: [2.2271858492309153, 2.074844961405639, 0.14382216826893632] TOYOTA RAV4 HYBRID 2019 8965: [2.1077397198131336, 1.8162215166877735, 0.13891369391200137] TOYOTA RAV4 HYBRID 2019 x02: [2.803624333289342, 2.272367966572498, 0.11364569214387774] -TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.1565442715453653] +TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.112174] TOYOTA RAV4 HYBRID 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736] -TOYOTA SIENNA 2018: [1.8660896232147548, 1.3208264576110418, 0.18799149615227198] +TOYOTA SIENNA 2018: [1.689726, 1.3208264576110418, 0.140456] VOLKSWAGEN ARTEON 1ST GEN: [1.45136518053819, 1.3639364049316804, 0.23806361745695032] VOLKSWAGEN ATLAS 1ST GEN: [1.4677006726964945, 1.6733266634075656, 0.12959584092073367] VOLKSWAGEN GOLF 7TH GEN: [1.3750394140491293, 1.5814743077200641, 0.2018321939386586] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index de64a5544..77236e393 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -17,7 +17,8 @@ LEXUS RC 2020: LEXUS NX 2020 TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019 TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022 -KIA OPTIMA SX 2019 & 2016: HYUNDAI SONATA 2020 +KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020 +KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020 KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020 KIA FORTE E 2018 & GT 2021: HYUNDAI SONATA 2020 KIA CEED INTRO ED 2019: HYUNDAI SONATA 2020 @@ -25,7 +26,6 @@ KIA SELTOS 2021: HYUNDAI SONATA 2020 KIA NIRO HYBRID 2019: KIA NIRO EV 2020 KIA NIRO HYBRID 2021: KIA NIRO EV 2020 HYUNDAI VELOSTER 2019: HYUNDAI SONATA 2019 -HYUNDAI I30 N LINE 2019 & GT 2018 DCT: HYUNDAI SONATA 2019 HYUNDAI KONA 2020: HYUNDAI KONA ELECTRIC 2019 HYUNDAI KONA HYBRID 2020: HYUNDAI KONA ELECTRIC 2019 HYUNDAI KONA ELECTRIC 2022: HYUNDAI KONA ELECTRIC 2019 @@ -35,9 +35,9 @@ HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019 HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020 -HYUNDAI ELANTRA 2021: HYUNDAI SONATA 2020 HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019 HYUNDAI SANTA FE 2022: HYUNDAI SANTA FE HYBRID 2022 +KIA STINGER 2022: KIA STINGER GT2 2018 GENESIS G90 2017: GENESIS G70 2018 GENESIS G80 2017: GENESIS G70 2018 GENESIS G70 2020: HYUNDAI SONATA 2020 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 3326d7c96..4ec950017 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -5,11 +5,19 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma create_accel_command, create_acc_cancel_command, \ create_fcw_command, create_lta_steer_command from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ - MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams + MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, \ + UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert +# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long +MAX_STEER_RATE = 100 # deg/s +MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut + +# EPS allows user torque above threshold for 50 frames before permanently faulting +MAX_USER_TORQUE = 500 + class CarController: def __init__(self, dbc_name, CP, VM): @@ -20,6 +28,7 @@ class CarController: self.alert_active = False self.last_standstill = False self.standstill_req = False + self.steer_rate_counter = 0 self.packer = CANPacker(dbc_name) self.gas = 0 @@ -29,6 +38,7 @@ class CarController: actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel + lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE # gas and brake if self.CP.enableGasInterceptor and CC.longActive: @@ -52,11 +62,19 @@ class CarController: 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) - if not CC.latActive: + # 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: + self.steer_rate_counter += 1 + else: + self.steer_rate_counter = 0 + + apply_steer_req = 1 + if not lat_active: apply_steer = 0 apply_steer_req = 0 - else: - apply_steer_req = 1 + elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES: + apply_steer_req = 0 + self.steer_rate_counter = 0 # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different @@ -64,7 +82,7 @@ class CarController: pcm_cancel_cmd = 1 # on entering standstill, send standstill request - if CS.out.standstill and not self.last_standstill and self.CP.carFingerprint not in NO_STOP_TIMER_CAR: + if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor): self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled @@ -95,7 +113,7 @@ class CarController: lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged # Lexus IS uses a different cancellation message - if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): + if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) @@ -109,28 +127,29 @@ class CarController: can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) self.gas = interceptor_gas_cmd - # ui mesg is at 1Hz but we send asap if: - # - there is something to display - # - there is something to stop displaying - fcw_alert = hud_control.visualAlert == VisualAlert.fcw - steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + if self.CP.carFingerprint != CAR.PRIUS_V: + # ui mesg is at 1Hz but we send asap if: + # - there is something to display + # - there is something to stop displaying + fcw_alert = hud_control.visualAlert == VisualAlert.fcw + steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) - send_ui = False - if ((fcw_alert or steer_alert) and not self.alert_active) or \ - (not (fcw_alert or steer_alert) and self.alert_active): - send_ui = True - self.alert_active = not self.alert_active - elif pcm_cancel_cmd: - # forcing the pcm to disengage causes a bad fault sound so play a good sound instead - send_ui = True + send_ui = False + if ((fcw_alert or steer_alert) and not self.alert_active) or \ + (not (fcw_alert or steer_alert) and self.alert_active): + send_ui = True + self.alert_active = not self.alert_active + elif pcm_cancel_cmd: + # forcing the pcm to disengage causes a bad fault sound so play a good sound instead + send_ui = True - if (self.frame % 100 == 0 or send_ui) and (self.CP.carFingerprint != CAR.PRIUS_V): - can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, - hud_control.rightLaneVisible, hud_control.leftLaneDepart, - hud_control.rightLaneDepart, CC.enabled)) + if self.frame % 100 == 0 or send_ui: + can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, + hud_control.rightLaneVisible, hud_control.leftLaneDepart, + hud_control.rightLaneDepart, CC.enabled)) - if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu: - can_sends.append(create_fcw_command(self.packer, fcw_alert)) + if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu: + can_sends.append(create_fcw_command(self.packer, fcw_alert)) # *** static msgs *** for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 537915919..8efb2c79e 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -6,7 +6,7 @@ from common.realtime import DT_CTRL from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE +from selfdrive.car.toyota.values import ToyotaFlags, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR class CarState(CarStateBase): @@ -15,6 +15,8 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. + self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. + self.cluster_min_speed = CV.KPH_TO_MS / 2. # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # the signal is zeroed to where the steering angle is at start. @@ -52,8 +54,9 @@ class CarState(CarStateBase): ) ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars - ret.standstill = ret.vEgoRaw < 0.001 + ret.standstill = ret.vEgoRaw == 0 ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] @@ -87,25 +90,32 @@ class CarState(CarStateBase): # 17 is a fault from a prolonged high torque delta between cmd and user ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] == 17 - if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): + if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS + cluster_set_speed = cp.vl["PCM_CRUISE_ALT"]["UI_SET_SPEED"] else: ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS + cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"] - if self.CP.carFingerprint in RADAR_ACC_CAR: - self.acc_type = cp.vl["ACC_CONTROL"]["ACC_TYPE"] - ret.stockFcw = bool(cp.vl["ACC_HUD"]["FCW"]) - elif self.CP.carFingerprint in TSS2_CAR: - self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"] - ret.stockFcw = bool(cp_cam.vl["ACC_HUD"]["FCW"]) + # UI_SET_SPEED is always non-zero when main is on, hide until first enable + if ret.cruiseState.speed != 0: + is_metric = cp.vl["BODY_CONTROL_STATE_2"]["UNITS"] in (1, 2) + conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS + ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor + + 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"] + ret.stockFcw = bool(cp_acc.vl["ACC_HUD"]["FCW"]) # some TSS2 cars have low speed lockout permanently set, so ignore on those cars # these cars are identified by an ACC_TYPE value of 2. # TODO: it is possible to avoid the lockout and gain stop and go if you # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1 - if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in (CAR.LEXUS_IS, CAR.LEXUS_RC)) or \ + if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR) or \ (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1): self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 @@ -120,10 +130,11 @@ class CarState(CarStateBase): ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6) ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) - ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 + if not self.CP.enableDsu: + ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + if self.CP.enableBsm: ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) @@ -147,6 +158,7 @@ class CarState(CarStateBase): ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"), ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"), ("PARKING_BRAKE", "BODY_CONTROL_STATE"), + ("UNITS", "BODY_CONTROL_STATE_2"), ("TC_DISABLED", "ESP_CONTROL"), ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"), ("STEER_FRACTION", "STEER_ANGLE_SENSOR"), @@ -154,6 +166,7 @@ class CarState(CarStateBase): ("CRUISE_ACTIVE", "PCM_CRUISE"), ("CRUISE_STATE", "PCM_CRUISE"), ("GAS_RELEASED", "PCM_CRUISE"), + ("UI_SET_SPEED", "PCM_CRUISE_SM"), ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"), ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"), ("STEER_ANGLE", "STEER_TORQUE_SENSOR"), @@ -168,12 +181,14 @@ class CarState(CarStateBase): ("LIGHT_STALK", 1), ("BLINKERS_STATE", 0.15), ("BODY_CONTROL_STATE", 3), + ("BODY_CONTROL_STATE_2", 2), ("ESP_CONTROL", 3), ("EPS_STATUS", 25), ("BRAKE_MODULE", 40), ("WHEEL_SPEEDS", 80), ("STEER_ANGLE_SENSOR", 80), ("PCM_CRUISE", 33), + ("PCM_CRUISE_SM", 1), ("STEER_TORQUE_SENSOR", 50), ] @@ -184,10 +199,12 @@ class CarState(CarStateBase): signals.append(("GAS_PEDAL", "GAS_PEDAL")) checks.append(("GAS_PEDAL", 33)) - if CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): + if CP.carFingerprint in UNSUPPORTED_DSU_CAR: signals.append(("MAIN_ON", "DSU_CRUISE")) signals.append(("SET_SPEED", "DSU_CRUISE")) + signals.append(("UI_SET_SPEED", "PCM_CRUISE_ALT")) checks.append(("DSU_CRUISE", 5)) + checks.append(("PCM_CRUISE_ALT", 1)) else: signals.append(("MAIN_ON", "PCM_CRUISE_2")) signals.append(("SET_SPEED", "PCM_CRUISE_2")) @@ -219,25 +236,31 @@ class CarState(CarStateBase): ("ACC_HUD", 1), ] + if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu: + signals += [ + ("FORCE", "PRE_COLLISION"), + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), + ] + checks += [ + ("PRE_COLLISION", 33), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): - signals = [ - ("FORCE", "PRE_COLLISION"), - ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), - ] - - checks = [ - ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent - ] + signals = [] + checks = [] if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): signals += [ + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), + ("FORCE", "PRE_COLLISION"), ("ACC_TYPE", "ACC_CONTROL"), ("FCW", "ACC_HUD"), ] checks += [ + ("PRE_COLLISION", 33), ("ACC_CONTROL", 33), ("ACC_HUD", 1), ] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 642c39952..3f4edb36d 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,8 +2,7 @@ from cereal import car from common.conversions import Conversions as CV from panda import Panda -from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune -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, CarControllerParams +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 import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -16,7 +15,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "toyota" @@ -74,7 +73,6 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeedFactor = 1.035 tire_stiffness_factor = 0.5533 ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.CHR, CAR.CHRH): stop_and_go = True @@ -82,7 +80,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_F) elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): stop_and_go = True @@ -90,8 +87,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - if candidate not in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): stop_and_go = True @@ -99,7 +94,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4516. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_G) elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2): # starting from 2019, all Avalon variants have stop and go @@ -109,7 +103,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_H) elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022): stop_and_go = True @@ -117,13 +110,20 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kiBP = [0.0] + ret.lateralTuning.pid.kpBP = [0.0] + ret.lateralTuning.pid.kpV = [0.6] + ret.lateralTuning.pid.kiV = [0.1] + ret.lateralTuning.pid.kf = 0.00007818594 # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891 for fw in car_fw: if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']): - set_lat_tune(ret.lateralTuning, LatTunes.PID_I) + ret.lateralTuning.pid.kpV = [0.15] + ret.lateralTuning.pid.kiV = [0.05] + ret.lateralTuning.pid.kf = 0.00004 break elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2): @@ -139,7 +139,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max - set_lat_tune(ret.lateralTuning, LatTunes.PID_D) elif candidate == CAR.SIENNA: stop_and_go = True @@ -147,14 +146,12 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.5 tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_J) elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC): ret.wheelbase = 2.79908 ret.steerRatio = 13.3 tire_stiffness_factor = 0.444 ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_L) elif candidate == CAR.LEXUS_CTH: stop_and_go = True @@ -162,7 +159,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 18.6 tire_stiffness_factor = 0.517 ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max - set_lat_tune(ret.lateralTuning, LatTunes.PID_M) elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NXH, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2): stop_and_go = True @@ -170,7 +166,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.7 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate == CAR.PRIUS_TSS2: stop_and_go = True @@ -178,7 +173,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.4 # True steerRatio from older prius tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_N) elif candidate == CAR.MIRAI: stop_and_go = True @@ -186,7 +180,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.8 tire_stiffness_factor = 0.8 ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2): stop_and_go = True @@ -194,7 +187,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.2 tire_stiffness_factor = 0.444 ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_J) ret.centerToFront = ret.wheelbase * 0.44 @@ -212,10 +204,11 @@ class CarInterface(CarInterfaceBase): smartDsu = 0x2FF in fingerprint[0] # 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) 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 smartDsu 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.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR if not ret.openpilotLongitudinalControl: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL @@ -229,13 +222,21 @@ class CarInterface(CarInterfaceBase): # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED - if ret.enableGasInterceptor: - set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL) - elif candidate in TSS2_CAR: - set_long_tune(ret.longitudinalTuning, LongTunes.TSS2) - ret.stoppingDecelRate = 0.3 # reach stopping target smoothly + tune = ret.longitudinalTuning + tune.deadzoneBP = [0., 9.] + tune.deadzoneV = [.0, .15] + if candidate in TSS2_CAR or ret.enableGasInterceptor: + tune.kpBP = [0., 5., 20.] + tune.kpV = [1.3, 1.0, 0.7] + tune.kiBP = [0., 5., 12., 20., 27.] + tune.kiV = [.35, .23, .20, .17, .1] + if candidate in TSS2_CAR: + ret.stoppingDecelRate = 0.3 # reach stopping target smoothly else: - set_long_tune(ret.longitudinalTuning, LongTunes.TSS) + tune.kpBP = [0., 5., 35.] + tune.kiBP = [0., 35.] + tune.kpV = [3.6, 2.4, 1.5] + tune.kiV = [0.54, 0.36] return ret diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py deleted file mode 100644 index a8b8758d8..000000000 --- a/selfdrive/car/toyota/tunes.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python3 -from enum import Enum - -class LongTunes(Enum): - PEDAL = 0 - TSS2 = 1 - TSS = 2 - -class LatTunes(Enum): - INDI_PRIUS = 0 - LQR_RAV4 = 1 - PID_A = 2 - PID_B = 3 - PID_C = 4 - PID_D = 5 - PID_E = 6 - PID_F = 7 - PID_G = 8 - PID_I = 9 - PID_H = 10 - PID_J = 11 - PID_K = 12 - PID_L = 13 - PID_M = 14 - PID_N = 15 - - -###### LONG ###### -def set_long_tune(tune, name): - # Improved longitudinal tune - if name == LongTunes.TSS2 or name == LongTunes.PEDAL: - tune.deadzoneBP = [0., 8.05] - tune.deadzoneV = [.0, .14] - tune.kpBP = [0., 5., 20.] - tune.kpV = [1.3, 1.0, 0.7] - tune.kiBP = [0., 5., 12., 20., 27.] - tune.kiV = [.35, .23, .20, .17, .1] - # Default longitudinal tune - elif name == LongTunes.TSS: - tune.deadzoneBP = [0., 9.] - tune.deadzoneV = [0., .15] - tune.kpBP = [0., 5., 35.] - tune.kiBP = [0., 35.] - tune.kpV = [3.6, 2.4, 1.5] - tune.kiV = [0.54, 0.36] - else: - raise NotImplementedError('This longitudinal tune does not exist') - - -###### LAT ###### -def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True): - if 'PID' in str(name): - tune.init('pid') - tune.pid.kiBP = [0.0] - tune.pid.kpBP = [0.0] - if name == LatTunes.PID_A: - tune.pid.kpV = [0.2] - tune.pid.kiV = [0.05] - tune.pid.kf = 0.00003 - elif name == LatTunes.PID_C: - tune.pid.kpV = [0.6] - tune.pid.kiV = [0.1] - tune.pid.kf = 0.00006 - elif name == LatTunes.PID_D: - tune.pid.kpV = [0.6] - tune.pid.kiV = [0.1] - tune.pid.kf = 0.00007818594 - elif name == LatTunes.PID_F: - tune.pid.kpV = [0.723] - tune.pid.kiV = [0.0428] - tune.pid.kf = 0.00006 - elif name == LatTunes.PID_G: - tune.pid.kpV = [0.18] - tune.pid.kiV = [0.015] - tune.pid.kf = 0.00012 - elif name == LatTunes.PID_H: - tune.pid.kpV = [0.17] - tune.pid.kiV = [0.03] - tune.pid.kf = 0.00006 - elif name == LatTunes.PID_I: - tune.pid.kpV = [0.15] - tune.pid.kiV = [0.05] - tune.pid.kf = 0.00004 - elif name == LatTunes.PID_J: - tune.pid.kpV = [0.19] - tune.pid.kiV = [0.02] - tune.pid.kf = 0.00007818594 - elif name == LatTunes.PID_L: - tune.pid.kpV = [0.3] - tune.pid.kiV = [0.05] - tune.pid.kf = 0.00006 - elif name == LatTunes.PID_M: - tune.pid.kpV = [0.3] - tune.pid.kiV = [0.05] - tune.pid.kf = 0.00007 - elif name == LatTunes.PID_N: - tune.pid.kpV = [0.35] - tune.pid.kiV = [0.15] - tune.pid.kf = 0.00007818594 - else: - raise NotImplementedError('This PID tune does not exist') - else: - raise NotImplementedError('This lateral tune does not exist') diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 7b4031ca6..ba26c7e03 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -7,6 +7,7 @@ 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.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu MIN_ACC_SPEED = 19. * CV.MPH_TO_MS @@ -86,10 +87,6 @@ class CAR: class Footnote(Enum): - DSU = CarFootnote( - "When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace " + - "stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).", - Column.LONGITUDINAL) CAMRY = CarFootnote( "openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.", Column.FSR_LONGITUDINAL) @@ -106,11 +103,11 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.ALPHARD_TSS2: ToyotaCarInfo("Toyota Alphard 2019-20"), CAR.ALPHARDH_TSS2: ToyotaCarInfo("Toyota Alphard Hybrid 2021"), CAR.AVALON: [ - ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P", footnotes=[Footnote.DSU]), - ToyotaCarInfo("Toyota Avalon 2017-18", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"), + ToyotaCarInfo("Toyota Avalon 2017-18"), ], - CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21", footnotes=[Footnote.DSU]), - CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21", footnotes=[Footnote.DSU]), + CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21"), + CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"), CAR.AVALON_TSS2: ToyotaCarInfo("Toyota Avalon 2022"), CAR.AVALONH_TSS2: ToyotaCarInfo("Toyota Avalon Hybrid 2022"), CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), @@ -119,10 +116,10 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-22"), CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-21"), CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-19"), - CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19", footnotes=[Footnote.DSU]), + CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"), CAR.COROLLA_TSS2: [ ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), - ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-21", min_enable_speed=7.5), + ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5), ToyotaCarInfo("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), ], CAR.COROLLAH_TSS2: [ @@ -130,48 +127,54 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), ToyotaCarInfo("Lexus UX Hybrid 2019-22"), ], - CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo", footnotes=[Footnote.DSU]), + CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"), CAR.HIGHLANDER_TSS2: ToyotaCarInfo("Toyota Highlander 2020-22"), - CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19", footnotes=[Footnote.DSU]), + CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"), CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-22"), CAR.PRIUS: [ - ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), - ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), - ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", "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"), ], - CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED, footnotes=[Footnote.DSU]), + CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED), CAR.PRIUS_TSS2: [ ToyotaCarInfo("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), ToyotaCarInfo("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), ], CAR.RAV4: [ - ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P", footnotes=[Footnote.DSU]), - ToyotaCarInfo("Toyota RAV4 2017-18", footnotes=[Footnote.DSU]) + ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P"), + ToyotaCarInfo("Toyota RAV4 2017-18") ], CAR.RAV4H: [ - ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26", footnotes=[Footnote.DSU]), - ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26", footnotes=[Footnote.DSU]) + ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", "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.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"), - CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", footnotes=[Footnote.DSU], min_enable_speed=MIN_ACC_SPEED), + 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+", footnotes=[Footnote.DSU]), - CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "Lexus Safety System+", footnotes=[Footnote.DSU]), + 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_TSS2: ToyotaCarInfo("Lexus ES 2019-22"), CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-22", video_link="https://youtu.be/BZ29osRVJeg?t=12"), CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"), - CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19", footnotes=[Footnote.DSU]), - CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19", footnotes=[Footnote.DSU]), + CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19"), + 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_RX: ToyotaCarInfo("Lexus RX 2016-19", footnotes=[Footnote.DSU]), - CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]), + CAR.LEXUS_RX: [ + ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"), + ToyotaCarInfo("Lexus RX 2017-19"), + ], + CAR.LEXUS_RXH: [ + ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+"), + ToyotaCarInfo("Lexus RX Hybrid 2017-19"), + ], CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"), CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"), } @@ -198,9 +201,38 @@ STATIC_DSU_MSGS = [ (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'), ] +TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01' +TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01' + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST], + [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE], + bus=0, + ), + Request( + [StdQueries.SHORT_TESTER_PRESENT_REQUEST, StdQueries.OBD_VERSION_REQUEST], + [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, StdQueries.OBD_VERSION_RESPONSE], + bus=0, + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.EXTENDED_DIAGNOSTIC_REQUEST, StdQueries.UDS_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.EXTENDED_DIAGNOSTIC_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], + bus=0, + ), + ], + non_essential_ecus={ + # FIXME: On some models, abs can sometimes be missing + Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS], + # On some models, the engine can show on two different addresses + Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.LEXUS_IS], + } +) + FW_VERSIONS = { CAR.AVALON: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152607060\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ @@ -225,7 +257,7 @@ FW_VERSIONS = { ], }, CAR.AVALON_2019: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152607140\x00\x00\x00\x00\x00\x00', b'F152607171\x00\x00\x00\x00\x00\x00', b'F152607110\x00\x00\x00\x00\x00\x00', @@ -253,7 +285,7 @@ FW_VERSIONS = { ], }, CAR.AVALONH_2019: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152641040\x00\x00\x00\x00\x00\x00', b'F152641061\x00\x00\x00\x00\x00\x00', b'F152641050\x00\x00\x00\x00\x00\x00', @@ -280,7 +312,7 @@ FW_VERSIONS = { ], }, CAR.AVALON_TSS2: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F152607240\x00\x00\x00\x00\x00\x00', b'\x01F152607280\x00\x00\x00\x00\x00\x00', ], @@ -300,7 +332,7 @@ FW_VERSIONS = { ], }, CAR.AVALONH_TSS2: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152641080\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -356,7 +388,7 @@ FW_VERSIONS = { b'8821F0608200 ', b'8821F0609100 ', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152606210\x00\x00\x00\x00\x00\x00', b'F152606230\x00\x00\x00\x00\x00\x00', b'F152606270\x00\x00\x00\x00\x00\x00', @@ -420,7 +452,7 @@ FW_VERSIONS = { b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966306S1100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152633214\x00\x00\x00\x00\x00\x00', b'F152633660\x00\x00\x00\x00\x00\x00', b'F152633712\x00\x00\x00\x00\x00\x00', @@ -486,7 +518,7 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B33630\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (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', @@ -511,13 +543,16 @@ FW_VERSIONS = { CAR.CAMRYH_TSS2: { (Ecu.eps, 0x7a1, None): [ b'8965B33630\x00\x00\x00\x00\x00\x00', + b'8965B33650\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152633D00\x00\x00\x00\x00\x00\x00', + b'F152633D60\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x018966306Q6000\x00\x00\x00\x00', b'\x018966306Q7000\x00\x00\x00\x00', + b'\x01896633T20000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 15): [ b'\x018821F6201200\x00\x00\x00\x00', @@ -547,7 +582,7 @@ FW_VERSIONS = { b'8821FF406000 ', b'8821FF407100 ', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152610020\x00\x00\x00\x00\x00\x00', b'F152610153\x00\x00\x00\x00\x00\x00', b'F152610210\x00\x00\x00\x00\x00\x00', @@ -599,7 +634,7 @@ FW_VERSIONS = { b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x0189663F438000\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152610012\x00\x00\x00\x00\x00\x00', b'F152610013\x00\x00\x00\x00\x00\x00', b'F152610014\x00\x00\x00\x00\x00\x00', @@ -658,7 +693,7 @@ FW_VERSIONS = { b'881510201100\x00\x00\x00\x00', b'881510201200\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152602190\x00\x00\x00\x00\x00\x00', b'F152602191\x00\x00\x00\x00\x00\x00', ], @@ -678,6 +713,7 @@ FW_VERSIONS = { }, CAR.COROLLA_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x01896630A22000\x00\x00\x00\x00', b'\x01896630ZG2000\x00\x00\x00\x00', b'\x01896630ZG5000\x00\x00\x00\x00', b'\x01896630ZG5100\x00\x00\x00\x00', @@ -718,6 +754,8 @@ FW_VERSIONS = { b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02312U5000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -734,7 +772,7 @@ FW_VERSIONS = { b'\x018965B12510\x00\x00\x00\x00\x00\x00', b'\x018965B1256000\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F152602280\x00\x00\x00\x00\x00\x00', b'\x01F152602560\x00\x00\x00\x00\x00\x00', b'\x01F152602590\x00\x00\x00\x00\x00\x00', @@ -755,6 +793,7 @@ FW_VERSIONS = { b'F152602191\x00\x00\x00\x00\x00\x00', b'\x01F152612862\x00\x00\x00\x00\x00\x00', b'\x01F152612B91\x00\x00\x00\x00\x00\x00', + b'\x01F15260A070\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -772,6 +811,7 @@ FW_VERSIONS = { b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, CAR.COROLLAH_TSS2: { @@ -783,6 +823,7 @@ FW_VERSIONS = { b'\x01896637626000\x00\x00\x00\x00', b'\x01896637648000\x00\x00\x00\x00', b'\x01896637643000\x00\x00\x00\x00', + b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630A21000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -812,7 +853,7 @@ FW_VERSIONS = { b'\x018965B12520\x00\x00\x00\x00\x00\x00', b'\x018965B12530\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152612590\x00\x00\x00\x00\x00\x00', b'F152612691\x00\x00\x00\x00\x00\x00', b'F152612692\x00\x00\x00\x00\x00\x00', @@ -884,7 +925,7 @@ FW_VERSIONS = { b'8965B48150\x00\x00\x00\x00\x00\x00', b'8965B48210\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [b'F15260E011\x00\x00\x00\x00\x00\x00'], + (Ecu.abs, 0x7b0, None): [b'F15260E011\x00\x00\x00\x00\x00\x00'], (Ecu.dsu, 0x791, None): [ b'881510E01100\x00\x00\x00\x00', b'881510E01200\x00\x00\x00\x00', @@ -902,7 +943,7 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B48160\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152648541\x00\x00\x00\x00\x00\x00', b'F152648542\x00\x00\x00\x00\x00\x00', ], @@ -927,7 +968,7 @@ FW_VERSIONS = { b'8965B48310\x00\x00\x00\x00\x00\x00', b'8965B48320\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F15260E051\x00\x00\x00\x00\x00\x00', b'\x01F15260E061\x00\x00\x00\x00\x00\x00', b'\x01F15260E110\x00\x00\x00\x00\x00\x00', @@ -946,7 +987,9 @@ FW_VERSIONS = { b'\x01896630EB2200\x00\x00\x00\x00', b'\x01896630EC4000\x00\x00\x00\x00', b'\x01896630ED9000\x00\x00\x00\x00', + b'\x01896630ED9100\x00\x00\x00\x00', b'\x01896630EE1000\x00\x00\x00\x00', + b'\x01896630EE1100\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', @@ -964,7 +1007,7 @@ FW_VERSIONS = { b'8965B48241\x00\x00\x00\x00\x00\x00', b'8965B48310\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F15264872300\x00\x00\x00\x00', b'\x01F15264872400\x00\x00\x00\x00', b'\x01F15264872500\x00\x00\x00\x00', @@ -974,15 +1017,16 @@ FW_VERSIONS = { b'\x01F152648J6000\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ - b'\x01896630EE4000\x00\x00\x00\x00', - b'\x01896630EE6000\x00\x00\x00\x00', b'\x01896630E67000\x00\x00\x00\x00', b'\x01896630EA1000\x00\x00\x00\x00', - b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x01896630EE4000\x00\x00\x00\x00', + b'\x01896630EE4100\x00\x00\x00\x00', + b'\x01896630EE6000\x00\x00\x00\x00', b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', - b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', @@ -1011,7 +1055,7 @@ FW_VERSIONS = { b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02353P9000\x00\x00\x00\x00\x00\x00\x00\x00553C1000\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152653300\x00\x00\x00\x00\x00\x00', b'F152653301\x00\x00\x00\x00\x00\x00', b'F152653310\x00\x00\x00\x00\x00\x00', @@ -1095,7 +1139,7 @@ FW_VERSIONS = { b'8965B47050\x00\x00\x00\x00\x00\x00', b'8965B47060\x00\x00\x00\x00\x00\x00', # This is the EPS with good angle sensor ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152647290\x00\x00\x00\x00\x00\x00', b'F152647300\x00\x00\x00\x00\x00\x00', b'F152647310\x00\x00\x00\x00\x00\x00', @@ -1136,7 +1180,7 @@ FW_VERSIONS = { ], }, CAR.PRIUS_V: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152647280\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ @@ -1169,7 +1213,7 @@ FW_VERSIONS = { b'8965B42082\x00\x00\x00\x00\x00\x00', b'8965B42083\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F15260R102\x00\x00\x00\x00\x00\x00', b'F15260R103\x00\x00\x00\x00\x00\x00', b'F152642493\x00\x00\x00\x00\x00\x00', @@ -1206,7 +1250,7 @@ FW_VERSIONS = { b'8965B42162\x00\x00\x00\x00\x00\x00', b'8965B42163\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152642090\x00\x00\x00\x00\x00\x00', b'F152642110\x00\x00\x00\x00\x00\x00', b'F152642120\x00\x00\x00\x00\x00\x00', @@ -1266,8 +1310,9 @@ FW_VERSIONS = { b'\x02896634A18100\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', + b'\x028966342Z8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F15260R210\x00\x00\x00\x00\x00\x00', b'\x01F15260R220\x00\x00\x00\x00\x00\x00', b'\x01F15260R290\x00\x00\x00\x00\x00\x00', @@ -1308,7 +1353,7 @@ FW_VERSIONS = { ], }, CAR.RAV4_TSS2_2022: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F15260R350\x00\x00\x00\x00\x00\x00', b'\x01F15260R361\x00\x00\x00\x00\x00\x00', ], @@ -1317,9 +1362,11 @@ FW_VERSIONS = { ], (Ecu.engine, 0x700, None): [ b'\x01896634AA0000\x00\x00\x00\x00', + b'\x01896634AA0100\x00\x00\x00\x00', b'\x01896634AA1000\x00\x00\x00\x00', b'\x01896634A88000\x00\x00\x00\x00', b'\x01896634A89000\x00\x00\x00\x00', + b'\x01896634A89100\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R01100\x00\x00\x00\x00', @@ -1345,10 +1392,11 @@ FW_VERSIONS = { b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A23000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x02896634A23101\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152642291\x00\x00\x00\x00\x00\x00', b'F152642290\x00\x00\x00\x00\x00\x00', b'F152642322\x00\x00\x00\x00\x00\x00', @@ -1387,7 +1435,7 @@ FW_VERSIONS = { ], }, CAR.RAV4H_TSS2_2022: { - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F15264283100\x00\x00\x00\x00', b'\x01F15264286200\x00\x00\x00\x00', b'\x01F15264286100\x00\x00\x00\x00', @@ -1435,7 +1483,7 @@ FW_VERSIONS = { b'8965B45080\x00\x00\x00\x00\x00\x00', b'8965B45082\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152608130\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ @@ -1454,7 +1502,7 @@ FW_VERSIONS = { (Ecu.dsu, 0x791, None): [ b'881517601100\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152676144\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ @@ -1476,7 +1524,7 @@ FW_VERSIONS = { b'\x018966333X6000\x00\x00\x00\x00', b'\x01896633T07000\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F152606281\x00\x00\x00\x00\x00\x00', b'\x01F152606340\x00\x00\x00\x00\x00\x00', b'\x01F152606461\x00\x00\x00\x00\x00\x00', @@ -1513,7 +1561,7 @@ FW_VERSIONS = { b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', b'\x01896633T38000\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152633423\x00\x00\x00\x00\x00\x00', b'F152633680\x00\x00\x00\x00\x00\x00', b'F152633681\x00\x00\x00\x00\x00\x00', @@ -1546,7 +1594,7 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\x02333M4200\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152633171\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ @@ -1572,7 +1620,7 @@ FW_VERSIONS = { b'\x01896637854000\x00\x00\x00\x00', b'\x01896637878000\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152678130\x00\x00\x00\x00\x00\x00', b'F152678140\x00\x00\x00\x00\x00\x00', ], @@ -1599,7 +1647,7 @@ FW_VERSIONS = { b'\x018966378B3000\x00\x00\x00\x00', b'\x018966378G3000\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F152678221\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -1619,7 +1667,7 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152678210\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -1640,7 +1688,7 @@ FW_VERSIONS = { b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152678160\x00\x00\x00\x00\x00\x00', b'F152678170\x00\x00\x00\x00\x00\x00', b'F152678171\x00\x00\x00\x00\x00\x00', @@ -1667,7 +1715,7 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\x0232484000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152624221\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ @@ -1703,7 +1751,7 @@ FW_VERSIONS = { b'\x018966348R8500\x00\x00\x00\x00', b'\x018966348W1300\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152648472\x00\x00\x00\x00\x00\x00', b'F152648473\x00\x00\x00\x00\x00\x00', b'F152648492\x00\x00\x00\x00\x00\x00', @@ -1750,7 +1798,7 @@ FW_VERSIONS = { b'\x02348V6000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348Z3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152648361\x00\x00\x00\x00\x00\x00', b'F152648501\x00\x00\x00\x00\x00\x00', b'F152648502\x00\x00\x00\x00\x00\x00', @@ -1798,7 +1846,7 @@ FW_VERSIONS = { b'\x01896634D43000\x00\x00\x00\x00', b'\x01896634D44000\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'\x01F15260E031\x00\x00\x00\x00\x00\x00', b'\x01F15260E041\x00\x00\x00\x00\x00\x00', b'\x01F152648781\x00\x00\x00\x00\x00\x00', @@ -1828,7 +1876,7 @@ FW_VERSIONS = { 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.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152648831\x00\x00\x00\x00\x00\x00', b'F152648891\x00\x00\x00\x00\x00\x00', b'F152648D00\x00\x00\x00\x00\x00\x00', @@ -1858,7 +1906,7 @@ FW_VERSIONS = { b'\x038966347C5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00', b'\x038966347C5100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152647500\x00\x00\x00\x00\x00\x00', b'F152647510\x00\x00\x00\x00\x00\x00', b'F152647520\x00\x00\x00\x00\x00\x00', @@ -1878,8 +1926,8 @@ FW_VERSIONS = { ], }, CAR.MIRAI: { - (Ecu.esp, 0x7D1, None): [b'\x01898A36203000\x00\x00\x00\x00',], - (Ecu.esp, 0x7B0, None): [ # a second ESP ECU + (Ecu.abs, 0x7D1, None): [b'\x01898A36203000\x00\x00\x00\x00',], + (Ecu.abs, 0x7B0, None): [ # a second ABS ECU b'\x01F15266203200\x00\x00\x00\x00', b'\x01F15266203500\x00\x00\x00\x00', ], @@ -1912,7 +1960,7 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B58040\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [ + (Ecu.abs, 0x7b0, None): [ b'F152658341\x00\x00\x00\x00\x00\x00' ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -1984,6 +2032,9 @@ TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TS NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} +# the DSU uses the AEB message for longitudinal on these cars +UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC} + # these cars have a radar which sends ACC messages instead of the camera RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022} diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 5ace68649..cf1c25e85 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -1,20 +1,12 @@ #!/usr/bin/env python3 import re -import struct -import traceback import cereal.messaging as messaging -import panda.python.uds as uds -from panda.python.uds import FUNCTIONAL_ADDRS +from panda.python.uds import get_rx_addr_for_tx_addr, FUNCTIONAL_ADDRS from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery +from selfdrive.car.fw_query_definitions import StdQueries from system.swaglog import cloudlog -OBD_VIN_REQUEST = b'\x09\x02' -OBD_VIN_RESPONSE = b'\x49\x02\x01' - -UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) -UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) - VIN_UNKNOWN = "0" * 17 VIN_RE = "[A-HJ-NPR-Z0-9]{17}" @@ -24,28 +16,48 @@ def is_valid_vin(vin: str): def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): + addrs = list(range(0x7e0, 0x7e8)) + list(range(0x18DA00F1, 0x18DB00F1, 0x100)) # addrs to process/wait for + valid_vin_addrs = [0x7e0, 0x7e2, 0x18da10f1, 0x18da0ef1] # engine, VMCU, 29-bit engine, PGM-FI for i in range(retry): - for request, response in ((UDS_VIN_REQUEST, UDS_VIN_RESPONSE), (OBD_VIN_REQUEST, OBD_VIN_RESPONSE)): + for request, response in ((StdQueries.UDS_VIN_REQUEST, StdQueries.UDS_VIN_RESPONSE), (StdQueries.OBD_VIN_REQUEST, StdQueries.OBD_VIN_RESPONSE)): try: - query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [request, ], [response, ], functional_addr=True, debug=debug) - for (addr, rx_addr), vin in query.get_data(timeout).items(): + query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, [request, ], [response, ], functional_addrs=FUNCTIONAL_ADDRS, debug=debug) + results = query.get_data(timeout) - # Honda Bosch response starts with a length, trim to correct length - if vin.startswith(b'\x11'): - vin = vin[1:18] + for addr in valid_vin_addrs: + vin = results.get((addr, None)) + if vin is not None: + # Ford pads with null bytes + if len(vin) == 24: + vin = re.sub(b'\x00*$', b'', vin) - return addr[0], rx_addr, vin.decode() - print(f"vin query retry ({i+1}) ...") + # Honda Bosch response starts with a length, trim to correct length + if vin.startswith(b'\x11'): + vin = vin[1:18] + + return get_rx_addr_for_tx_addr(addr), vin.decode() + + cloudlog.error(f"vin query retry ({i+1}) ...") except Exception: - cloudlog.warning(f"VIN query exception: {traceback.format_exc()}") + cloudlog.exception("VIN query exception") - return 0, 0, VIN_UNKNOWN + return 0, VIN_UNKNOWN if __name__ == "__main__": + import argparse import time + + parser = argparse.ArgumentParser(description='Get VIN of the car') + parser.add_argument('--debug', action='store_true') + parser.add_argument('--bus', type=int, default=1) + parser.add_argument('--timeout', type=float, default=0.1) + parser.add_argument('--retry', type=int, default=5) + args = parser.parse_args() + sendcan = messaging.pub_sock('sendcan') logcan = messaging.sub_sock('can') time.sleep(1) - addr, vin_rx_addr, vin = get_vin(logcan, sendcan, 1, debug=False) - print(f'TX: {hex(addr)}, RX: {hex(vin_rx_addr)}, VIN: {vin}') + + vin_rx_addr, vin = get_vin(logcan, sendcan, args.bus, args.timeout, args.retry, debug=args.debug) + print(f'RX: {hex(vin_rx_addr)}, VIN: {vin}') diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index f14c11989..fff554867 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -7,6 +7,7 @@ from selfdrive.car.volkswagen import mqbcan, pqcan from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState class CarController: @@ -17,6 +18,7 @@ class CarController: self.packer_pt = CANPacker(dbc_name) self.apply_steer_last = 0 + self.gra_acc_counter_last = None self.frame = 0 self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 @@ -24,7 +26,6 @@ class CarController: def update(self, CC, CS, ext_bus): actuators = CC.actuators hud_control = CC.hudControl - can_sends = [] # **** Steering Controls ************************************************ # @@ -70,9 +71,12 @@ class CarController: # **** Acceleration Controls ******************************************** # if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: - tsk_status = self.CCS.tsk_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) + acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 - can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, tsk_status, accel)) + stopping = actuators.longControlState == LongCtrlState.stopping + starting = actuators.longControlState == LongCtrlState.starting + can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, CC.longActive, accel, + acc_control, stopping, starting, CS.esp_hold_confirmation)) # **** HUD Controls ***************************************************** # @@ -91,15 +95,15 @@ class CarController: # **** Stock ACC Button Controls **************************************** # - if self.CP.pcmCruise and self.frame % self.CCP.GRA_ACC_STEP == 0: - idx = (CS.gra_stock_values["COUNTER"] + 1) % 16 - if CC.cruiseControl.cancel: - can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, cancel=True)) - elif CC.cruiseControl.resume: - can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, resume=True)) + gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last + if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume): + counter = (CS.gra_stock_values["COUNTER"] + 1) % 16 + can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, counter, + cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume)) new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX + self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index facc740a1..def14ab01 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -12,6 +12,7 @@ class CarState(CarStateBase): super().__init__(CP) self.CCP = CarControllerParams(CP) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} + self.esp_hold_confirmation = False def create_button_events(self, pt_cp, buttons): button_events = [] @@ -105,25 +106,27 @@ class CarState(CarStateBase): ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"]) # Update ACC radar status. + self.acc_type = ext_cp.vl["ACC_06"]["ACC_Typ"] if pt_cp.vl["TSK_06"]["TSK_Status"] == 2: # ACC okay and enabled, but not currently engaged ret.cruiseState.available = True ret.cruiseState.enabled = False elif pt_cp.vl["TSK_06"]["TSK_Status"] in (3, 4, 5): - # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5) + # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or brake only (5) ret.cruiseState.available = True ret.cruiseState.enabled = True else: # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7) ret.cruiseState.available = False ret.cruiseState.enabled = False - ret.cruiseState.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + self.esp_hold_confirmation = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation ret.accFaulted = pt_cp.vl["TSK_06"]["TSK_Status"] in (6, 7) # Update ACC setpoint. When the setpoint is zero or there's an error, the # radar sends a set-speed of ~90.69 m/s / 203mph. if self.CP.pcmCruise: - ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS + ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS if ret.cruiseState.speed > 90: ret.cruiseState.speed = 0 @@ -170,7 +173,7 @@ class CarState(CarStateBase): ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 ret.gasPressed = ret.gas > 0 ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects - ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremstestschalter"]) + ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremslichtschalter"]) ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"]) # Update gear and/or clutch position data. @@ -215,6 +218,7 @@ class CarState(CarStateBase): ret.stockAeb = False # Update ACC radar status. + self.acc_type = 0 # TODO: this is ACC "basic" with nonzero min speed, support FtS (1) later ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"]) ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"]) if self.CP.pcmCruise: @@ -477,12 +481,14 @@ class CarState(CarStateBase): class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ - ("ACC_Wunschgeschw", "ACC_02"), # ACC set speed + ("ACC_Wunschgeschw_02", "ACC_02"), # ACC set speed + ("ACC_Typ", "ACC_06"), # Basic vs F2S vs SNG ("AWV2_Freigabe", "ACC_10"), # FCW brake jerk release ("ANB_Teilbremsung_Freigabe", "ACC_10"), # AEB partial braking release ("ANB_Zielbremsung_Freigabe", "ACC_10"), # AEB target braking release ] fwd_radar_checks = [ + ("ACC_06", 50), # From J428 ACC radar control module ("ACC_10", 50), # From J428 ACC radar control module ("ACC_02", 17), # From J428 ACC radar control module ] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 7e686b970..816e7fcf3 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -6,6 +6,7 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter +ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -21,17 +22,19 @@ class CarInterface(CarInterfaceBase): self.cp_ext = self.cp_cam @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "volkswagen" ret.radarOffCan = 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]: # Getriebe_1 + if 0x440 in fingerprint[0] or use_off_car_defaults: # Getriebe_1 ret.transmissionType = TransmissionType.automatic else: ret.transmissionType = TransmissionType.manual @@ -49,18 +52,12 @@ class CarInterface(CarInterfaceBase): # Panda ALLOW_DEBUG firmware required. ret.dashcamOnly = True - if disable_radar and ret.networkLocation == NetworkLocation.gateway: - # Proof-of-concept, prep for E2E only. No radar points available. Follow-to-stop not yet supported, but should - # be simple to add when a suitable test car becomes available. Panda ALLOW_DEBUG firmware required. - ret.openpilotLongitudinalControl = True - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL - else: # Set global MQB parameters ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)] ret.enableBsm = 0x30F in fingerprint[0] # SWA_01 - if 0xAD in fingerprint[0]: # Getriebe_11 + if 0xAD in fingerprint[0] or use_off_car_defaults: # Getriebe_11 ret.transmissionType = TransmissionType.automatic elif 0x187 in fingerprint[0]: # EV_Gearshift ret.transmissionType = TransmissionType.direct @@ -86,8 +83,20 @@ class CarInterface(CarInterfaceBase): # Global longitudinal tuning defaults, can be overridden per-vehicle + ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or use_off_car_defaults + if experimental_long: + # Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required. + ret.openpilotLongitudinalControl = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL + if ret.transmissionType == TransmissionType.manual: + ret.minEnableSpeed = 4.5 + ret.pcmCruise = not ret.openpilotLongitudinalControl - ret.longitudinalActuatorDelayUpperBound = 0.5 # s + ret.stoppingControl = True + ret.startingState = True + ret.startAccel = 1.0 + ret.vEgoStarting = 1.0 + ret.vEgoStopping = 1.0 ret.longitudinalTuning.kpV = [0.1] ret.longitudinalTuning.kiV = [0.0] @@ -125,6 +134,13 @@ class CarInterface(CarInterfaceBase): ret.mass = 1230 + STD_CARGO_KG ret.wheelbase = 2.55 + elif candidate == CAR.SHARAN_MK2: + ret.mass = 1639 + STD_CARGO_KG + ret.wheelbase = 2.92 + ret.minEnableSpeed = 30 * CV.KPH_TO_MS + ret.minSteerSpeed = 50 * CV.KPH_TO_MS + ret.steerActuatorDelay = 0.2 + elif candidate == CAR.TAOS_MK1: ret.mass = 1498 + STD_CARGO_KG ret.wheelbase = 2.69 @@ -197,6 +213,7 @@ class CarInterface(CarInterfaceBase): else: raise ValueError(f"unsupported car {candidate}") + ret.autoResumeSng = ret.minEnableSpeed == -1 ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.centerToFront = ret.wheelbase * 0.45 ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, @@ -208,7 +225,8 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], - pcm_enable=not self.CS.CP.openpilotLongitudinalControl) + pcm_enable=not self.CS.CP.openpilotLongitudinalControl, + enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.): diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 1a4ca339a..25a710dbb 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -26,13 +26,83 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres return packer.make_can_msg("LDW_02", bus, values) -def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, resume=False): +def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=False, resume=False): values = gra_stock_values.copy() values.update({ - "COUNTER": idx, + "COUNTER": counter, "GRA_Abbrechen": cancel, "GRA_Tip_Wiederaufnahme": resume, }) return packer.make_can_msg("GRA_ACC_01", bus, values) + + +def acc_control_value(main_switch_on, acc_faulted, long_active): + if acc_faulted: + acc_control = 6 + elif long_active: + acc_control = 3 + elif main_switch_on: + acc_control = 2 + else: + acc_control = 0 + + return acc_control + + +def acc_hud_status_value(main_switch_on, acc_faulted, long_active): + # TODO: happens to resemble the ACC control value for now, but extend this for init/gas override later + 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): + 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_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_Anfahren": starting, + "ACC_Anhalten": stopping, + } + commands.append(packer.make_can_msg("ACC_06", bus, acc_06_values)) + + if starting: + acc_hold_type = 4 # hold release / startup + elif esp_hold: + acc_hold_type = 3 # hold standby + elif stopping: + acc_hold_type = 1 # hold request + else: + acc_hold_type = 0 + + 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_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_Anforderung_HMS": acc_hold_type, + "ACC_Anfahren": starting, + "ACC_Anhalten": stopping, + } + commands.append(packer.make_can_msg("ACC_07", bus, acc_07_values)) + + return commands + + +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): + values = { + "ACC_Status_Anzeige": acc_hud_status, + "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, + "ACC_Gesetzte_Zeitluecke": 3, + "ACC_Display_Prio": 3, + # TODO: ACC_Abstandsindex for lead car distance, must determine analog vs digital cluster for scaling + } + + return packer.make_can_msg("ACC_02", bus, values) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 1e7ed2134..0bcbf6abb 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -23,11 +23,11 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres return packer.make_can_msg("LDW_Status", bus, values) -def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, resume=False): +def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=False, resume=False): values = gra_stock_values.copy() values.update({ - "COUNTER": idx, + "COUNTER": counter, "GRA_Abbrechen": cancel, "GRA_Recall": resume, }) @@ -35,15 +35,15 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, return packer.make_can_msg("GRA_Neu", bus, values) -def tsk_status_value(main_switch_on, acc_faulted, long_active): +def acc_control_value(main_switch_on, acc_faulted, long_active): if long_active: - tsk_status = 1 + acc_control = 1 elif main_switch_on: - tsk_status = 2 + acc_control = 2 else: - tsk_status = 0 + acc_control = 0 - return tsk_status + return acc_control def acc_hud_status_value(main_switch_on, acc_faulted, long_active): @@ -59,26 +59,32 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active): return hud_status -def create_acc_accel_control(packer, bus, adr_status, accel): +def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold): + commands = [] + values = { - "ACS_Sta_ADR": adr_status, - "ACS_StSt_Info": adr_status != 1, - "ACS_Typ_ACC": 0, # TODO: this is ACC "basic", find a way to detect FtS support (1) - "ACS_Sollbeschl": accel if adr_status == 1 else 3.01, - "ACS_zul_Regelabw": 0.2 if adr_status == 1 else 1.27, - "ACS_max_AendGrad": 3.0 if adr_status == 1 else 5.08, + "ACS_Sta_ADR": acc_control, + "ACS_StSt_Info": acc_control != 1, + "ACS_Typ_ACC": acc_type, + "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, } - return packer.make_can_msg("ACC_System", bus, values) + commands.append(packer.make_can_msg("ACC_System", bus, values)) + + return commands -def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): values = { - "ACA_StaACC": acc_status, + "ACA_StaACC": acc_hud_status, "ACA_Zeitluecke": 2, "ACA_V_Wunsch": set_speed, "ACA_gemZeitl": 8 if lead_visible else 0, + # 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 } - # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke return packer.make_can_msg("ACC_GRA_Anziege", bus, values) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 58f071901..cb2343e08 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,12 +1,14 @@ from collections import defaultdict, namedtuple -from dataclasses import dataclass +from dataclasses import dataclass, field from enum import Enum from typing import Dict, List, Union 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.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu NetworkLocation = car.CarParams.NetworkLocation @@ -17,9 +19,7 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) class CarControllerParams: HCA_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz - GRA_ACC_STEP = 3 # GRA_ACC_01/GRA_Neu message frequency 33Hz ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz - ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz ACCEL_MAX = 2.0 # 2.0 m/s max acceleration ACCEL_MIN = -3.5 # 3.5 m/s max deceleration @@ -36,6 +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.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)) @@ -64,6 +65,7 @@ class CarControllerParams: else: self.LDW_STEP = 10 # LDW_02 message frequency 10Hz + self.ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50)) self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) @@ -114,6 +116,7 @@ class CAR: PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants PASSAT_NMS = "VOLKSWAGEN PASSAT NMS" # Chassis A3, North America/China/Mideast NMS Passat, incl. facelift POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN" # Chassis AW, Mk6 VW Polo + SHARAN_MK2 = "VOLKSWAGEN SHARAN 2ND GEN" # Chassis 7N, Mk2 Volkswagen Sharan and SEAT Alhambra TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN" # Chassis B2, Mk1 VW Taos and Tharu TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN" # Chassis C1, Mk1 VW T-Cross SWB and LWB variants TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants @@ -133,7 +136,7 @@ class CAR: SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants -PQ_CARS = {CAR.PASSAT_NMS} +PQ_CARS = {CAR.PASSAT_NMS, CAR.SHARAN_MK2} DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None)) @@ -148,92 +151,96 @@ class Footnote(Enum): PASSAT = CarFootnote( "Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.", Column.MODEL) - VW_HARNESS = CarFootnote( - "Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma " + - "store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black " + - "(older design) or light brown (newer design). In the interim, if your car has a J533 connector CAN gateway " + - "inside the dashboard, choose \"VW J533 Development\" from the vehicle drop-down for a suitable harness. " + - "(Some newer models are also observed to not have a J533 connector.)", - Column.MODEL) - VW_VARIANT = CarFootnote( - "Includes versions with extra rear cargo space (may be called Variant, Estate, SportWagen, Shooting Brake, etc.)", - Column.MODEL) + VW_EXP_LONG = CarFootnote ( + "Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness " + + "are limited to using stock ACC.", + Column.LONGITUDINAL) + VW_MQB_A0 = CarFootnote( + "Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot " + + "in software, but doesn't yet have a harness available from the comma store.", + Column.HARNESS) @dataclass class VWCarInfo(CarInfo): - package: str = "Driver Assistance" - harness: Enum = Harness.vw + package: str = "Adaptive Cruise Control (ACC) & Lane Assist" + harness: Enum = Harness.j533 + footnotes: List[Enum] = field(default_factory=lambda: [Footnote.VW_EXP_LONG]) CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { CAR.ARTEON_MK1: [ - VWCarInfo("Volkswagen Arteon 2018-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen Arteon R 2020-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen Arteon eHybrid 2020-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen CC 2018-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), + VWCarInfo("Volkswagen Arteon 2018-22", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarInfo("Volkswagen Arteon R 2020-22", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarInfo("Volkswagen Arteon eHybrid 2020-22", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarInfo("Volkswagen CC 2018-22", video_link="https://youtu.be/FAomFKPFlDA"), ], CAR.ATLAS_MK1: [ - VWCarInfo("Volkswagen Atlas 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen Atlas Cross Sport 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen Teramont 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen Teramont Cross Sport 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen Teramont X 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Atlas 2018-23"), + VWCarInfo("Volkswagen Atlas Cross Sport 2021-22"), + VWCarInfo("Volkswagen Teramont 2018-22"), + VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"), + VWCarInfo("Volkswagen Teramont X 2021-22"), ], CAR.GOLF_MK7: [ VWCarInfo("Volkswagen e-Golf 2014-20"), - VWCarInfo("Volkswagen Golf 2015-20", footnotes=[Footnote.VW_VARIANT]), + VWCarInfo("Volkswagen Golf 2015-20"), VWCarInfo("Volkswagen Golf Alltrack 2015-19"), VWCarInfo("Volkswagen Golf GTD 2015-20"), VWCarInfo("Volkswagen Golf GTE 2015-20"), VWCarInfo("Volkswagen Golf GTI 2015-21"), - VWCarInfo("Volkswagen Golf R 2015-19", footnotes=[Footnote.VW_VARIANT]), + VWCarInfo("Volkswagen Golf R 2015-19"), VWCarInfo("Volkswagen Golf SportsVan 2015-20"), ], CAR.JETTA_MK7: [ - VWCarInfo("Volkswagen Jetta 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen Jetta GLI 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Jetta 2018-22"), + VWCarInfo("Volkswagen Jetta GLI 2021-22"), ], CAR.PASSAT_MK8: [ - VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.PASSAT, Footnote.VW_VARIANT], harness=Harness.j533), - VWCarInfo("Volkswagen Passat Alltrack 2015-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen Passat GTE 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533), + VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.VW_EXP_LONG, Footnote.PASSAT]), + VWCarInfo("Volkswagen Passat Alltrack 2015-22"), + VWCarInfo("Volkswagen Passat GTE 2015-22"), ], - CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22", harness=Harness.j533), + CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22"), CAR.POLO_MK6: [ - VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), + VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), ], - CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + CAR.SHARAN_MK2: [ + VWCarInfo("Volkswagen Sharan 2018-22"), + VWCarInfo("SEAT Alhambra 2018-20"), + ], + CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022"), + CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), + CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22"), CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"), CAR.TRANSPORTER_T61: [ - VWCarInfo("Volkswagen Caravelle 2020", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), - VWCarInfo("Volkswagen California 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Caravelle 2020"), + VWCarInfo("Volkswagen California 2021"), ], - CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), CAR.AUDI_A3_MK3: [ - VWCarInfo("Audi A3 2014-19", "ACC + Lane Assist"), - VWCarInfo("Audi A3 Sportback e-tron 2017-18", "ACC + Lane Assist"), - VWCarInfo("Audi RS3 2018", "ACC + Lane Assist"), - VWCarInfo("Audi S3 2015-17", "ACC + Lane Assist"), + VWCarInfo("Audi A3 2014-19"), + VWCarInfo("Audi A3 Sportback e-tron 2017-18"), + VWCarInfo("Audi RS3 2018"), + VWCarInfo("Audi S3 2015-17"), ], - CAR.AUDI_Q2_MK1: VWCarInfo("Audi Q2 2018", "ACC + Lane Assist"), - CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2020-21", "ACC + Lane Assist"), + CAR.AUDI_Q2_MK1: VWCarInfo("Audi Q2 2018"), + 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_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.KAMIQ]), - CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21", footnotes=[Footnote.VW_HARNESS]), + CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.VW_EXP_LONG, 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_SCALA_MK1: VWCarInfo("Škoda Scala 2020"), - CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-18"), + CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), + CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"), CAR.SKODA_OCTAVIA_MK3: [ VWCarInfo("Škoda Octavia 2015, 2018-19"), VWCarInfo("Škoda Octavia RS 2016"), ], } + # All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars # with a manual trans won't return transmission firmware, but all other cars will. # @@ -243,6 +250,30 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { # ECU SW part numbers are invalid for vehicle ID and compatibility checks. Try to have # them repaired by the tuner before including them in openpilot. +VOLKSWAGEN_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.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) +VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + +VOLKSWAGEN_RX_OFFSET = 0x6a + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [VOLKSWAGEN_VERSION_REQUEST_MULTI], + [VOLKSWAGEN_VERSION_RESPONSE], + whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar], + rx_offset=VOLKSWAGEN_RX_OFFSET, + ), + Request( + [VOLKSWAGEN_VERSION_REQUEST_MULTI], + [VOLKSWAGEN_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine, Ecu.transmission], + ), + ], +) + FW_VERSIONS = { CAR.ARTEON_MK1: { (Ecu.engine, 0x7e0, None): [ @@ -274,7 +305,9 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703H906026AA\xf1\x899970', b'\xf1\x8703H906026AJ\xf1\x890638', + b'\xf1\x8703H906026AJ\xf1\x891017', b'\xf1\x8703H906026AT\xf1\x891922', + b'\xf1\x8703H906026BC\xf1\x892664', b'\xf1\x8703H906026F \xf1\x896696', b'\xf1\x8703H906026F \xf1\x899970', b'\xf1\x8703H906026J \xf1\x896026', @@ -287,12 +320,14 @@ FW_VERSIONS = { b'\xf1\x8709G927158DR\xf1\x893536', b'\xf1\x8709G927158DR\xf1\x893742', b'\xf1\x8709G927158FT\xf1\x893835', + b'\xf1\x8709G927158GL\xf1\x893939', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655BC\xf1\x890503\xf1\x82\0161914151912001103111122031200', b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900', b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\0162214152212001105141122052900', b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00', + b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1115151112001105171122052J00', ], (Ecu.eps, 0x712, None): [ b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1', @@ -300,6 +335,7 @@ FW_VERSIONS = { b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572R \xf1\x890372', b'\xf1\x872Q0907572T \xf1\x890383', b'\xf1\x875Q0907572H \xf1\x890620', @@ -369,6 +405,8 @@ FW_VERSIONS = { b'\xf1\x870D9300012 \xf1\x894937', b'\xf1\x870D9300012 \xf1\x895045', b'\xf1\x870D9300014M \xf1\x895004', + b'\xf1\x870D9300014Q \xf1\x895006', + b'\xf1\x870D9300020Q \xf1\x895201', b'\xf1\x870D9300020S \xf1\x895201', b'\xf1\x870D9300040A \xf1\x893613', b'\xf1\x870D9300040S \xf1\x894311', @@ -420,6 +458,7 @@ FW_VERSIONS = { b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1', b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A01A18A1', b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A0JA16A1', + b'\xf1\x873QM909144 \xf1\x895072\xf1\x82\x0571A01714A1', b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820519A9040203', b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00441A1', b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00608A1', @@ -515,6 +554,7 @@ FW_VERSIONS = { b'\xf1\x8703N906026E \xf1\x892114', b'\xf1\x8704E906023AH\xf1\x893379', b'\xf1\x8704L906026ET\xf1\x891990', + b'\xf1\x8704L906026FP\xf1\x892012', b'\xf1\x8704L906026GA\xf1\x892013', b'\xf1\x8704L906026KD\xf1\x894798', b'\xf1\x873G0906264 \xf1\x890004', @@ -522,6 +562,7 @@ FW_VERSIONS = { (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300043H \xf1\x891601', b'\xf1\x870CW300048R \xf1\x890610', + b'\xf1\x870D9300013A \xf1\x894905', b'\xf1\x870D9300014L \xf1\x895002', b'\xf1\x870D9300041A \xf1\x894801', b'\xf1\x870DD300045T \xf1\x891601', @@ -539,6 +580,7 @@ FW_VERSIONS = { ], (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00611A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00711A1', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0060803', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1', @@ -591,6 +633,18 @@ FW_VERSIONS = { b'\xf1\x872Q0907572R \xf1\x890372', ], }, + CAR.SHARAN_MK2: { + # TODO: Sharan Mk2 EPS and DQ250 auto trans both require KWP2000 support for fingerprinting + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906016HE\xf1\x894635', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x877N0959655D \xf1\x890016\xf1\x82\x0801100705----10--', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0153', + ], + }, CAR.TAOS_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906027NJ\xf1\x891445', @@ -737,9 +791,11 @@ FW_VERSIONS = { b'\xf1\x8704E906023BL\xf1\x895190', b'\xf1\x8704E906027CJ\xf1\x897798', b'\xf1\x8704L997022N \xf1\x899459', + b'\xf1\x875G0906259A \xf1\x890004', b'\xf1\x875G0906259L \xf1\x890002', b'\xf1\x875G0906259Q \xf1\x890002', b'\xf1\x878V0906259F \xf1\x890002', + b'\xf1\x878V0906259J \xf1\x890002', b'\xf1\x878V0906259K \xf1\x890001', b'\xf1\x878V0906264B \xf1\x890003', b'\xf1\x878V0907115B \xf1\x890007', @@ -749,6 +805,7 @@ FW_VERSIONS = { b'\xf1\x870CW300044T \xf1\x895245', b'\xf1\x870CW300048 \xf1\x895201', b'\xf1\x870D9300012 \xf1\x894912', + b'\xf1\x870D9300012K \xf1\x894513', b'\xf1\x870D9300013B \xf1\x894931', b'\xf1\x870D9300041N \xf1\x894512', b'\xf1\x870D9300043T \xf1\x899699', @@ -757,6 +814,7 @@ FW_VERSIONS = { b'\xf1\x870DD300046F \xf1\x891602', b'\xf1\x870DD300046G \xf1\x891601', b'\xf1\x870DL300012E \xf1\x892012', + b'\xf1\x870GC300011 \xf1\x890403', b'\xf1\x870GC300013M \xf1\x892402', b'\xf1\x870GC300042J \xf1\x891402', ], @@ -764,7 +822,9 @@ FW_VERSIONS = { b'\xf1\x875Q0959655AB\xf1\x890388\xf1\x82\0211111001111111206110412111321139114', b'\xf1\x875Q0959655AM\xf1\x890315\xf1\x82\x1311111111111111311411011231129321212100', b'\xf1\x875Q0959655AM\xf1\x890318\xf1\x82\x1311111111111112311411011531159321212100', + b'\xf1\x875Q0959655AR\xf1\x890315\xf1\x82\x1311110011131115311211012331239321212100', b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x1311110011131100311111011731179321342100', + b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\x13111112111111--241115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\023111112111111--171115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13121111111111--341117141212231291163221', @@ -773,11 +833,13 @@ FW_VERSIONS = { ], (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1', + b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G01A16A1', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G0HA16A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571G0JA14A1', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521G0G809A1', b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00303A0', 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\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1', ], @@ -809,20 +871,24 @@ FW_VERSIONS = { CAR.AUDI_Q3_MK2: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8705E906018N \xf1\x899970', + b'\xf1\x8705L906022M \xf1\x890901', b'\xf1\x8783A906259 \xf1\x890001', b'\xf1\x8783A906259 \xf1\x890005', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158CN\xf1\x893608', + b'\xf1\x870GC300045D \xf1\x892802', b'\xf1\x870GC300046F \xf1\x892701', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655BF\xf1\x890403\xf1\x82\x1321211111211200311121232152219321422111', + b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111224118A119321532111', b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000800', + b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\x0571G60533A1', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572R \xf1\x890372', @@ -998,6 +1064,7 @@ FW_VERSIONS = { }, CAR.SKODA_SUPERB_MK3: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906026ET\xf1\x891343', b'\xf1\x8704L906026FP\xf1\x891196', b'\xf1\x8704L906026KB\xf1\x894071', b'\xf1\x8704L906026KD\xf1\x894798', @@ -1008,9 +1075,11 @@ FW_VERSIONS = { b'\xf1\x870CW300042H \xf1\x891601', b'\xf1\x870D9300011T \xf1\x894801', b'\xf1\x870D9300012 \xf1\x894940', + b'\xf1\x870D9300041H \xf1\x894905', b'\xf1\x870GC300043 \xf1\x892301', ], (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', diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1f394d487..a18bec83a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -12,12 +12,11 @@ import cereal.messaging as messaging from common.conversions import Conversions as CV from panda import ALTERNATIVE_EXPERIENCE from system.swaglog import cloudlog -from system.version import get_short_branch +from system.version import is_tested_branch, get_short_branch from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can -from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET -from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise -from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature +from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET +from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -49,7 +48,6 @@ Desire = log.LateralPlan.Desire LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection EventName = car.CarEvent.EventName -ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel @@ -82,45 +80,45 @@ class Controls: self.log_sock = messaging.sub_sock('androidLog') + self.params = Params() + self.sm = sm + if self.sm is None: + ignore = ['testJoystick'] + if SIMULATION: + ignore += ['driverCameraState', 'managerState'] + if self.params.get_bool('WideCameraOnly'): + ignore += ['roadCameraState'] + self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', + 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', + 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets, + ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan', 'testJoystick']) + if CI is None: # wait for one pandaState and one CAN packet print("Waiting for CAN messages...") get_one_can(self.can_sock) - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) + num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], num_pandas) else: self.CI, self.CP = CI, CI.CP - params = Params() - self.joystick_mode = params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None) - joystick_packet = ['testJoystick'] if self.joystick_mode else [] - - self.sm = sm - if self.sm is None: - ignore = [] - if SIMULATION: - ignore += ['driverCameraState', 'managerState'] - if params.get_bool('WideCameraOnly'): - ignore += ['roadCameraState'] - self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', - 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, - ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) + self.joystick_mode = self.params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None) # set alternative experiences from parameters - self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator") + self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.CP.alternativeExperience = 0 if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - if self.CP.dashcamOnly and params.get_bool("DashcamOverride"): + if self.CP.dashcamOnly and self.params.get_bool("DashcamOverride"): self.CP.dashcamOnly = False # read params - self.is_metric = params.get_bool("IsMetric") - self.is_ldw_enabled = params.get_bool("IsLdwEnabled") - openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") - passive = params.get_bool("Passive") or not openpilot_enabled_toggle + self.is_metric = self.params.get_bool("IsMetric") + self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") + passive = self.params.get_bool("Passive") or not openpilot_enabled_toggle # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() @@ -134,10 +132,20 @@ class Controls: safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] + if is_tested_branch(): + self.CP.experimentalLongitudinalAvailable = False + # Write CarParams for radard cp_bytes = self.CP.to_bytes() - params.put("CarParams", cp_bytes) + self.params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) + put_nonblocking("CarParamsPersistent", cp_bytes) + + # cleanup old params + if not self.CP.experimentalLongitudinalAvailable: + self.params.remove("ExperimentalLongitudinalEnabled") + if not self.CP.openpilotLongitudinalControl: + self.params.remove("ExperimentalMode") self.CC = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() @@ -161,25 +169,22 @@ class Controls: self.state = State.disabled self.enabled = False self.active = False - self.can_rcv_error = False + self.can_rcv_timeout = False self.soft_disable_timer = 0 - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL - self.v_cruise_kph_last = 0 self.mismatch_counter = 0 self.cruise_mismatch_counter = 0 - self.can_rcv_error_counter = 0 + self.can_rcv_timeout_counter = 0 self.last_blinker_frame = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = None - self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} self.last_actuators = car.CarControl.Actuators.new_message() self.steer_limited = False self.desired_curvature = 0.0 self.desired_curvature_rate = 0.0 + self.v_cruise_helper = VCruiseHelper(self.CP) # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True @@ -209,9 +214,9 @@ class Controls: controls_state = Params().get("ReplayControlsState") if controls_state is not None: controls_state = log.ControlsState.from_bytes(controls_state) - self.v_cruise_kph = controls_state.vCruise + self.v_cruise_helper.v_cruise_kph = controls_state.vCruise - if self.sm['pandaStates'][0].controlsAllowed: + if any(ps.controlsAllowed for ps in self.sm['pandaStates']): self.state = State.enabled def update_events(self, CS): @@ -229,19 +234,23 @@ class Controls: self.events.add(EventName.controlsInitializing) return + # no more events while in dashcam mode + if self.read_only: + return + # Block resume if cruise never previously enabled resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) - if not self.CP.pcmCruise and self.v_cruise_kph == V_CRUISE_INITIAL and resume_pressed: + if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: self.events.add(EventName.resumeBlocked) # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ - (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)): + (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ + (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): self.events.add(EventName.pedalPressed) if CS.gasPressed: - self.events.add(EventName.pedalPressedPreEnable if self.disengage_on_accelerator else - EventName.gasPressedOverride) + self.events.add(EventName.gasPressedOverride) if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) @@ -305,7 +314,7 @@ class Controls: else: safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES - if safety_mismatch or self.mismatch_counter >= 200: + if safety_mismatch or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) if log.PandaState.FaultType.relayMalfunction in pandaState.faults: @@ -339,19 +348,19 @@ class Controls: # generic catch-all. ideally, a more specific event should be added above instead has_disable_events = self.events.any(ET.NO_ENTRY) and (self.events.any(ET.SOFT_DISABLE) or self.events.any(ET.IMMEDIATE_DISABLE)) no_system_errors = (not has_disable_events) or (len(self.events) == num_events) - if (not self.sm.all_checks() or self.can_rcv_error) and no_system_errors: + if (not self.sm.all_checks() or self.can_rcv_timeout) and no_system_errors: if not self.sm.all_alive(): self.events.add(EventName.commIssue) elif not self.sm.all_freq_ok(): self.events.add(EventName.commIssueAvgFreq) - else: # invalid or can_rcv_error. + else: # invalid or can_rcv_timeout. self.events.add(EventName.commIssue) logs = { 'invalid': [s for s, valid in self.sm.valid.items() if not valid], 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], - 'can_error': self.can_rcv_error, + 'can_rcv_timeout': self.can_rcv_timeout, } if logs != self.logged_comm_issue: cloudlog.event("commIssue", error=True, **logs) @@ -440,10 +449,10 @@ class Controls: # Check for CAN timeout if not can_strs: - self.can_rcv_error_counter += 1 - self.can_rcv_error = True + self.can_rcv_timeout_counter += 1 + self.can_rcv_timeout = True else: - self.can_rcv_error = False + self.can_rcv_timeout = False # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through @@ -464,20 +473,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_kph_last = self.v_cruise_kph - - if CS.cruiseState.available: - # if stock cruise is completely disabled, then we can use our own set speed logic - if not self.CP.pcmCruise: - self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents, - self.button_timers, self.enabled, self.is_metric) - self.v_cruise_cluster_kph = self.v_cruise_kph - else: - self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH - self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH - else: - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -504,9 +500,9 @@ class Controls: self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) self.current_alert_types.append(ET.SOFT_DISABLE) - elif self.events.any(ET.OVERRIDE): + elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL): self.state = State.overriding - self.current_alert_types.append(ET.OVERRIDE) + self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] # SOFT DISABLING elif self.state == State.softDisabling: @@ -536,10 +532,10 @@ class Controls: self.state = State.softDisabling self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) self.current_alert_types.append(ET.SOFT_DISABLE) - elif not self.events.any(ET.OVERRIDE): + elif not (self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL)): self.state = State.enabled else: - self.current_alert_types.append(ET.OVERRIDE) + self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] # DISABLED elif self.state == State.disabled: @@ -550,14 +546,12 @@ class Controls: else: if self.events.any(ET.PRE_ENABLE): self.state = State.preEnabled - elif self.events.any(ET.OVERRIDE): + elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL): self.state = State.overriding else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - if not self.CP.pcmCruise: - self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) - self.v_cruise_cluster_kph = self.v_cruise_kph + self.v_cruise_helper.initialize_v_cruise(CS) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -569,11 +563,17 @@ class Controls: """Given the state, this function returns a CarControl packet""" # Update VehicleModel - params = self.sm['liveParameters'] - x = max(params.stiffnessFactor, 0.1) - sr = max(params.steerRatio, 0.1) + lp = self.sm['liveParameters'] + x = max(lp.stiffnessFactor, 0.1) + sr = max(lp.steerRatio, 0.1) self.VM.update_params(x, sr) + # Update Torque Params + if self.CP.lateralTuning.which() == 'torque': + torque_params = self.sm['liveTorqueParameters'] + if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams: + self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered) + lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] @@ -581,8 +581,8 @@ class Controls: CC.enabled = self.enabled # Check which actuators can be enabled CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ - CS.vEgo > self.CP.minSteerSpeed and not CS.standstill - CC.longActive = self.active and not self.events.any(ET.OVERRIDE) and self.CP.openpilotLongitudinalControl + CS.vEgo > self.CP.minSteerSpeed and not CS.standstill + CC.longActive = self.active and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state @@ -599,7 +599,7 @@ class Controls: if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) @@ -608,7 +608,7 @@ class Controls: lat_plan.psis, lat_plan.curvatures, lat_plan.curvatureRates) - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params, + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.last_actuators, self.steer_limited, self.desired_curvature, self.desired_curvature_rate, self.sm['liveLocationKalman']) else: @@ -663,16 +663,6 @@ class Controls: return CC, lac_log - def update_button_timers(self, buttonEvents): - # increment timer for buttons still pressed - for k in self.button_timers: - if self.button_timers[k] > 0: - self.button_timers[k] += 1 - - for b in buttonEvents: - if b.type.raw in self.button_timers: - self.button_timers[b.type.raw] = 1 if b.pressed else 0 - def publish_logs(self, CS, start_time, CC, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" @@ -685,6 +675,7 @@ class Controls: if len(angular_rate_value) > 2: CC.angularVelocity = angular_rate_value + CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: CC.cruiseControl.cancel = True @@ -694,7 +685,7 @@ class Controls: CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 hudControl = CC.hudControl - hudControl.setSpeed = float(self.v_cruise_cluster_kph * CV.KPH_TO_MS) + hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) hudControl.speedVisible = self.enabled hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead @@ -704,13 +695,13 @@ 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 == Calibration.CALIBRATED model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction if len(desire_prediction) and ldw_allowed: - right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 - left_lane_visible = self.sm['lateralPlan'].lProb > 0.5 + 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] @@ -747,10 +738,10 @@ class Controls: (self.state == State.softDisabling) # Curvature & Steering angle - params = self.sm['liveParameters'] + lp = self.sm['liveParameters'] - steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg) - curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll) + steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) + curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) # controlsState dat = messaging.new_message('controlsState') @@ -777,15 +768,16 @@ class Controls: controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) - controlsState.vCruise = float(self.v_cruise_kph) - controlsState.vCruiseCluster = float(self.v_cruise_cluster_kph) + controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph) + controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) - controlsState.canErrorCounter = self.can_rcv_error_counter + controlsState.canErrorCounter = self.can_rcv_timeout_counter + controlsState.experimentalMode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: @@ -835,6 +827,8 @@ class Controls: start_time = sec_since_boot() self.prof.checkpoint("Ratekeeper", ignore=True) + self.is_metric = self.params.get_bool("IsMetric") + # Sample data from sockets and get a carState CS = self.data_sample() cloudlog.timestamp("Data sampled") @@ -857,7 +851,6 @@ class Controls: self.publish_logs(CS, start_time, CC, lac_log) self.prof.checkpoint("Sent") - self.update_button_timers(CS.buttonEvents) self.CS_prev = CS def controlsd_thread(self): @@ -866,6 +859,7 @@ class Controls: self.rk.monitor_time() self.prof.display() + def main(sm=None, pm=None, logcan=None): controls = Controls(sm, pm, logcan) controls.controlsd_thread() diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 2dad05e21..cb878483d 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -22,7 +22,7 @@ def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] = a['text'] += extra_text Params().put(alert, json.dumps(a)) else: - Params().delete(alert) + Params().remove(alert) @dataclass diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 19ab2da21..4790b8f0e 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -5,7 +5,7 @@ from common.realtime import DT_MDL LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection -LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS +LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. DESIRES = { diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ffa837383..bdbdb7023 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -13,6 +13,7 @@ V_CRUISE_MIN = 8 # kph V_CRUISE_ENABLE_MIN = 40 # kph V_CRUISE_INITIAL = 255 # kph +MIN_SPEED = 1.0 LAT_MPC_N = 16 LON_MPC_N = 32 CONTROL_N = 17 @@ -21,6 +22,7 @@ CAR_ROTATION_RADIUS = 0.0 # EU guidelines MAX_LATERAL_JERK = 5.0 +ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type CRUISE_LONG_PRESS = 50 CRUISE_NEAREST_FUNC = { @@ -33,10 +35,108 @@ CRUISE_INTERVAL_SIGN = { } -class MPC_COST_LAT: - PATH = 1.0 - HEADING = 1.0 - STEER_RATE = 1.0 +class VCruiseHelper: + def __init__(self, CP): + self.CP = CP + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_kph_last = 0 + self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} + self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} + + @property + def v_cruise_initialized(self): + return self.v_cruise_kph != V_CRUISE_INITIAL + + def update_v_cruise(self, CS, enabled, is_metric): + self.v_cruise_kph_last = self.v_cruise_kph + + if CS.cruiseState.available: + if not self.CP.pcmCruise: + # if stock cruise is completely disabled, then we can use our own set speed logic + self._update_v_cruise_non_pcm(CS, enabled, is_metric) + self.v_cruise_cluster_kph = self.v_cruise_kph + self.update_button_timers(CS, enabled) + else: + self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + else: + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL + + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): + # handle button presses. TODO: this should be in state_control, but a decelCruise press + # would have the effect of both enabling and changing speed is checked after the state transition + if not enabled: + return + + long_press = False + button_type = None + + # should be CV.MPH_TO_KPH, but this causes rounding errors + v_cruise_delta = 1. if is_metric else 1.6 + + for b in CS.buttonEvents: + if b.type.raw in self.button_timers and not b.pressed: + if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS: + return # end long press + button_type = b.type.raw + break + else: + for k in self.button_timers.keys(): + if self.button_timers[k] and self.button_timers[k] % CRUISE_LONG_PRESS == 0: + button_type = k + long_press = True + break + + if button_type is None: + return + + # Don't adjust speed when pressing resume to exit standstill + cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill + if button_type == ButtonType.accelCruise and cruise_standstill: + return + + # Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge) + if not self.button_change_states[button_type]["enabled"]: + return + + v_cruise_delta = v_cruise_delta * (5 if long_press else 1) + if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval + self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta + else: + self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] + + # If set is pressed while overriding, clip cruise speed to minimum of vEgo + if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): + self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) + + self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) + + def update_button_timers(self, CS, enabled): + # increment timer for buttons still pressed + for k in self.button_timers: + if self.button_timers[k] > 0: + self.button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in self.button_timers: + # Start/end timer and store current state on change of button pressed + self.button_timers[b.type.raw] = 1 if b.pressed else 0 + self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} + + def initialize_v_cruise(self, CS): + # initializing is handled by the PCM + if self.CP.pcmCruise: + return + + # 250kph or above probably means we never had a set speed + if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: + self.v_cruise_kph = self.v_cruise_kph_last + else: + self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) + + self.v_cruise_cluster_kph = self.v_cruise_kph def apply_deadzone(error, deadzone): @@ -53,62 +153,12 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def update_v_cruise(v_cruise_kph, v_ego, gas_pressed, buttonEvents, button_timers, enabled, metric): - # handle button presses. TODO: this should be in state_control, but a decelCruise press - # would have the effect of both enabling and changing speed is checked after the state transition - if not enabled: - return v_cruise_kph - - long_press = False - button_type = None - - # should be CV.MPH_TO_KPH, but this causes rounding errors - v_cruise_delta = 1. if metric else 1.6 - - for b in buttonEvents: - if b.type.raw in button_timers and not b.pressed: - if button_timers[b.type.raw] > CRUISE_LONG_PRESS: - return v_cruise_kph # end long press - button_type = b.type.raw - break - else: - for k in button_timers.keys(): - if button_timers[k] and button_timers[k] % CRUISE_LONG_PRESS == 0: - button_type = k - long_press = True - break - - if button_type: - v_cruise_delta = v_cruise_delta * (5 if long_press else 1) - if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval - v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](v_cruise_kph / v_cruise_delta) * v_cruise_delta - else: - v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] - - # If set is pressed while overriding, clip cruise speed to minimum of vEgo - if gas_pressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): - v_cruise_kph = max(v_cruise_kph, v_ego * CV.MS_TO_KPH) - - v_cruise_kph = clip(round(v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) - - return v_cruise_kph - - -def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): - for b in buttonEvents: - # 250kph or above probably means we never had a set speed - if b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and v_cruise_last < 250: - return v_cruise_last - - return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) - - def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): if len(psis) != CONTROL_N: psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N - v_ego = max(v_ego, 0.1) + v_ego = max(MIN_SPEED, v_ego) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 @@ -125,10 +175,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): desired_curvature_rate = curvature_rates[0] max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 safe_desired_curvature_rate = clip(desired_curvature_rate, - -max_curvature_rate, - max_curvature_rate) + -max_curvature_rate, + max_curvature_rate) safe_desired_curvature = clip(desired_curvature, - current_curvature_desired - max_curvature_rate * DT_MDL, - current_curvature_desired + max_curvature_rate * DT_MDL) + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) return safe_desired_curvature, safe_desired_curvature_rate diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 5139ead84..a761cceec 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -31,7 +31,8 @@ class Priority(IntEnum): class ET: ENABLE = 'enable' PRE_ENABLE = 'preEnable' - OVERRIDE = 'override' + OVERRIDE_LATERAL = 'overrideLateral' + OVERRIDE_LONGITUDINAL = 'overrideLongitudinal' NO_ENTRY = 'noEntry' WARNING = 'warning' USER_DISABLE = 'userDisable' @@ -596,6 +597,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.buttonCancel: { ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"), }, EventName.brakeHold: { @@ -623,7 +625,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.gasPressedOverride: { - ET.OVERRIDE: Alert( + ET.OVERRIDE_LONGITUDINAL: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.steerOverride: { + ET.OVERRIDE_LATERAL: Alert( "", "", AlertStatus.normal, AlertSize.none, @@ -802,6 +812,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Cruise Faulted"), }, + EventName.accFaultedTemp: { + ET.NO_ENTRY: NoEntryAlert("Cruise Temporarily Faulted"), + }, + EventName.controlsMismatch: { ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"), ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"), diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py deleted file mode 100644 index 1facb66d6..000000000 --- a/selfdrive/controls/lib/lane_planner.py +++ /dev/null @@ -1,97 +0,0 @@ -import numpy as np -from cereal import log -from common.filter_simple import FirstOrderFilter -from common.numpy_fast import interp -from common.realtime import DT_MDL -from system.swaglog import cloudlog - - -TRAJECTORY_SIZE = 33 -# camera offset is meters from center car to camera -# model path is in the frame of the camera -PATH_OFFSET = 0.00 -CAMERA_OFFSET = 0.04 - - -class LanePlanner: - def __init__(self, wide_camera=False): - self.ll_t = np.zeros((TRAJECTORY_SIZE,)) - self.ll_x = np.zeros((TRAJECTORY_SIZE,)) - self.lll_y = np.zeros((TRAJECTORY_SIZE,)) - self.rll_y = np.zeros((TRAJECTORY_SIZE,)) - self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL) - self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL) - self.lane_width = 3.7 - - self.lll_prob = 0. - self.rll_prob = 0. - self.d_prob = 0. - - self.lll_std = 0. - self.rll_std = 0. - - self.l_lane_change_prob = 0. - self.r_lane_change_prob = 0. - - self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET - self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET - - def parse_model(self, md): - lane_lines = md.laneLines - if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE: - self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2 - # left and right ll x is the same - self.ll_x = lane_lines[1].x - self.lll_y = np.array(lane_lines[1].y) + self.camera_offset - self.rll_y = np.array(lane_lines[2].y) + self.camera_offset - self.lll_prob = md.laneLineProbs[1] - self.rll_prob = md.laneLineProbs[2] - self.lll_std = md.laneLineStds[1] - self.rll_std = md.laneLineStds[2] - - desire_state = md.meta.desireState - if len(desire_state): - self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft] - self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight] - - def get_d_path(self, v_ego, path_t, path_xyz): - # Reduce reliance on lanelines that are too far apart or - # will be in a few seconds - path_xyz[:, 1] += self.path_offset - l_prob, r_prob = self.lll_prob, self.rll_prob - width_pts = self.rll_y - self.lll_y - prob_mods = [] - for t_check in (0.0, 1.5, 3.0): - width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts) - prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) - mod = min(prob_mods) - l_prob *= mod - r_prob *= mod - - # Reduce reliance on uncertain lanelines - l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0]) - r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0]) - l_prob *= l_std_mod - r_prob *= r_std_mod - - # Find current lanewidth - self.lane_width_certainty.update(l_prob * r_prob) - current_lane_width = abs(self.rll_y[0] - self.lll_y[0]) - self.lane_width_estimate.update(current_lane_width) - speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5]) - self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \ - (1 - self.lane_width_certainty.x) * speed_lane_width - - clipped_lane_width = min(4.0, self.lane_width) - path_from_left_lane = self.lll_y + clipped_lane_width / 2.0 - path_from_right_lane = self.rll_y - clipped_lane_width / 2.0 - - self.d_prob = l_prob + r_prob - l_prob * r_prob - lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) - safe_idxs = np.isfinite(self.ll_t) - if safe_idxs[0]: - lane_path_y_interp = np.interp(path_t, self.ll_t[safe_idxs], lane_path_y[safe_idxs]) - path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1] - else: - cloudlog.warning("Lateral mpc - NaNs in laneline times, ignoring") - return path_xyz diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c4604d90e..d10d39d94 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,7 +4,6 @@ from cereal import log from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from selfdrive.controls.lib.pid import PIDController -from selfdrive.controls.lib.drive_helpers import apply_deadzone from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # At higher speeds (25+mph) we can assume: @@ -18,20 +17,24 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # friction in the steering wheel that needs to be overcome to # move it at all, this is compensated for too. - -FRICTION_THRESHOLD = 0.2 +LOW_SPEED_X = [0, 10, 20, 30] +LOW_SPEED_Y = [15, 13, 10, 5] class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) - self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, - k_f=CP.lateralTuning.torque.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) - self.get_steer_feedforward = CI.get_steer_feedforward_function() - self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle - self.friction = CP.lateralTuning.torque.friction - self.kf = CP.lateralTuning.torque.kf - self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg + self.torque_params = CP.lateralTuning.torque + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, + k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.torque_from_lateral_accel = CI.torque_from_lateral_accel() + self.use_steering_angle = self.torque_params.useSteeringAngle + self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg + + def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): + self.torque_params.latAccelFactor = latAccelFactor + self.torque_params.latAccelOffset = latAccelOffset + self.torque_params.friction = friction def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() @@ -51,23 +54,23 @@ class LatControlTorque(LatControl): desired_lateral_accel = desired_curvature * CS.vEgo ** 2 # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature - #desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - - low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200]) + 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 - pid_log.error = error + 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, + lateral_accel_deadzone, friction_compensation=False) + 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) - ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY - # convert friction into lateral accel units for feedforward - friction_compensation = interp(apply_deadzone(error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction]) - ff += friction_compensation / self.kf freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 - output_torque = self.pid.update(error, + output_torque = self.pid.update(pid_log.error, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator) @@ -78,9 +81,9 @@ class LatControlTorque(LatControl): pid_log.d = self.pid.d pid_log.f = self.pid.f pid_log.output = -output_torque - pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited) pid_log.actualLateralAccel = actual_lateral_accel pid_log.desiredLateralAccel = desired_lateral_accel + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited) # TODO left is positive in this convention return -output_torque, 0.0, pid_log diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index c0e735816..9607532ac 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -5,7 +5,6 @@ import numpy as np from casadi import SX, vertcat, sin, cos from common.realtime import sec_since_boot -from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N from selfdrive.modeld.constants import T_IDXS if __name__ == '__main__': # generating code @@ -18,6 +17,10 @@ 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' @@ -29,8 +32,8 @@ def gen_lat_model(): x_ego = SX.sym('x_ego') y_ego = SX.sym('y_ego') psi_ego = SX.sym('psi_ego') - curv_ego = SX.sym('curv_ego') - model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego) + psi_rate_ego = SX.sym('psi_rate_ego') + model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego) # parameters v_ego = SX.sym('v_ego') @@ -38,22 +41,22 @@ def gen_lat_model(): model.p = vertcat(v_ego, rotation_radius) # controls - curv_rate = SX.sym('curv_rate') - model.u = vertcat(curv_rate) + psi_accel_ego = SX.sym('psi_accel_ego') + model.u = vertcat(psi_accel_ego) # xdot x_ego_dot = SX.sym('x_ego_dot') y_ego_dot = SX.sym('y_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot') - curv_ego_dot = SX.sym('curv_ego_dot') + psi_rate_ego_dot = SX.sym('psi_rate_ego_dot') - model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot) + model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot) # dynamics model - f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), - v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), - v_ego * curv_ego, - curv_rate) + f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego, + v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego, + psi_rate_ego, + psi_accel_ego) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model @@ -72,26 +75,35 @@ def gen_lat_ocp(): ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' - Q = np.diag([0.0, 0.0]) - QR = np.diag([0.0, 0.0, 0.0]) + Q = np.diag(np.zeros(COST_E_DIM)) + QR = np.diag(np.zeros(COST_DIM)) ocp.cost.W = QR ocp.cost.W_e = Q - y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2] - curv_rate = ocp.model.u[0] + y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3] + psi_rate_ego_dot = ocp.model.u[0] v_ego = ocp.model.p[0] ocp.parameter_values = np.zeros((P_DIM, )) - ocp.cost.yref = np.zeros((3, )) - ocp.cost.yref_e = np.zeros((2, )) - # TODO hacky weights to keep behavior the same + ocp.cost.yref = np.zeros((COST_DIM, )) + ocp.cost.yref_e = np.zeros((COST_E_DIM, )) + # Add offset to smooth out low speed control + # TODO unclear if this right solution long term + v_ego_offset = v_ego + SPEED_OFFSET + # TODO there are two costs on psi_rate_ego_dot, one + # is correlated to jerk the other to steering wheel movement + # the steering wheel movement cost is added to prevent excessive + # wheel movements ocp.model.cost_y_expr = vertcat(y_ego, - ((v_ego +5.0) * psi_ego), - ((v_ego + 5.0) * 4.0 * curv_rate)) + v_ego_offset * psi_ego, + v_ego_offset * psi_rate_ego, + v_ego_offset * psi_rate_ego_dot, + psi_rate_ego_dot / (v_ego + 0.1)) ocp.model.cost_y_expr_e = vertcat(y_ego, - ((v_ego +5.0) * psi_ego)) + v_ego_offset * psi_ego, + v_ego_offset * psi_rate_ego) # set constraints ocp.constraints.constr_type = 'BGH' @@ -124,10 +136,10 @@ class LateralMpc(): def reset(self, x0=np.zeros(X_DIM)): self.x_sol = np.zeros((N+1, X_DIM)) self.u_sol = np.zeros((N, 1)) - self.yref = np.zeros((N+1, 3)) + self.yref = np.zeros((N+1, COST_DIM)) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.cost_set(N, "yref", self.yref[N][:2]) + self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) # Somehow needed for stable init for i in range(N+1): @@ -140,14 +152,17 @@ class LateralMpc(): self.solve_time = 0.0 self.cost = 0 - def set_weights(self, path_weight, heading_weight, steer_rate_weight): - W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight])) + def set_weights(self, path_weight, heading_weight, + lat_accel_weight, lat_jerk_weight, + steering_rate_weight): + W = np.asfortranarray(np.diag([path_weight, heading_weight, + lat_accel_weight, lat_jerk_weight, + steering_rate_weight])) for i in range(N): self.solver.cost_set(i, 'W', W) - #TODO hacky weights to keep behavior the same - self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2]) + self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM]) - def run(self, x0, p, y_pts, heading_pts, curv_rate_pts): + def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts): x0_cp = np.copy(x0) p_cp = np.copy(p) self.solver.constraints_set(0, "lbx", x0_cp) @@ -155,13 +170,13 @@ class LateralMpc(): self.yref[:,0] = y_pts v_ego = p_cp[0] # rotation_radius = p_cp[1] - self.yref[:,1] = heading_pts*(v_ego+5.0) - self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0 + 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.cost_set(N, "yref", self.yref[N][:2]) + self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) t = sec_since_boot() self.solution_status = self.solver.solve() diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 29dfc7711..932ad4953 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -3,17 +3,29 @@ from common.realtime import sec_since_boot, DT_MDL 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.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N -from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE +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.desire_helper import DesireHelper import cereal.messaging as messaging from cereal import log +TRAJECTORY_SIZE = 33 +CAMERA_OFFSET = 0.04 + + +PATH_COST = 1.0 +LATERAL_MOTION_COST = 0.11 +LATERAL_ACCEL_COST = 0.0 +LATERAL_JERK_COST = 0.05 +# 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 + class LateralPlanner: - def __init__(self, CP, use_lanelines=True, wide_camera=False): - self.use_lanelines = use_lanelines - self.LP = LanePlanner(wide_camera) + def __init__(self, CP): self.DH = DesireHelper() # Vehicle model parameters used to calculate lateral movement of car @@ -23,9 +35,8 @@ class LateralPlanner: self.solution_invalid_cnt = 0 self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) - self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) - self.plan_curv_rate = 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) @@ -37,57 +48,50 @@ class LateralPlanner: self.lat_mpc.reset(x0=self.x0) def update(self, sm): - v_ego = sm['carState'].vEgo + # clip speed , lateral planning is not possible at 0 speed + self.v_ego = max(MIN_SPEED, sm['carState'].vEgo) measured_curvature = sm['controlsState'].curvature # Parse model predictions md = sm['modelV2'] - self.LP.parse_model(md) if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE: self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) self.plan_yaw = np.array(md.orientation.z) - if len(md.position.xStd) == TRAJECTORY_SIZE: - self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) + self.plan_yaw_rate = np.array(md.orientationRate.z) # Lane change logic - lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + desire_state = md.meta.desireState + if len(desire_state): + self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft] + self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight] + lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) - # Turn off lanes during lane change - if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft: - self.LP.lll_prob *= self.DH.lane_change_ll_prob - self.LP.rll_prob *= self.DH.lane_change_ll_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) - # Calculate final driving path and set MPC costs - if self.use_lanelines: - d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE) - else: - d_path_xyz = self.path_xyz - # Heading cost is useful at low speed, otherwise end of plan can be off-heading - heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) - - y_pts = np.interp(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(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) - curv_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_curv_rate) + 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) self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 - assert len(curv_rate_pts) == LAT_MPC_N + 1 - lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2)) - p = np.array([v_ego, lateral_factor]) + 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]) self.lat_mpc.run(self.x0, p, y_pts, heading_pts, - curv_rate_pts) - # init state for next - # mpc.u_sol is the desired curvature rate given x0 curv state. - # with x0[3] = measured_curvature, this would be the actual desired rate. - # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. + yaw_rate_pts) + # init state for next iteration + # mpc.u_sol is the desired second derivative of psi given x0 curv state. + # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate. + # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution @@ -95,7 +99,7 @@ class LateralPlanner: t = sec_since_boot() if mpc_nans or self.lat_mpc.solution_status != 0: self.reset_mpc() - self.x0[3] = measured_curvature + self.x0[3] = measured_curvature * self.v_ego if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") @@ -112,20 +116,17 @@ class LateralPlanner: lateralPlan = plan_send.lateralPlan lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] - lateralPlan.laneWidth = float(self.LP.lane_width) lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() - lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist() - lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] - lateralPlan.lProb = float(self.LP.lll_prob) - lateralPlan.rProb = float(self.LP.rll_prob) - lateralPlan.dProb = float(self.LP.d_prob) + + lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist() + lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.solverExecutionTime = self.lat_mpc.solve_time lateralPlan.desire = self.DH.desire - lateralPlan.useLaneLines = self.use_lanelines + lateralPlan.useLaneLines = False lateralPlan.laneChangeState = self.DH.lane_change_state lateralPlan.laneChangeDirection = self.DH.lane_change_direction diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e9458607d..92a4f1f99 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -7,20 +7,22 @@ from selfdrive.modeld.constants import T_IDXS LongCtrlState = car.CarControl.Actuators.LongControlState -# As per ISO 15622:2018 for all speeds -ACCEL_MIN_ISO = -3.5 # m/s^2 -ACCEL_MAX_ISO = 2.0 # m/s^2 - def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, - v_target_future, brake_pressed, cruise_standstill): - """Update longitudinal control state machine""" - accelerating = v_target_future > v_target - stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ - (v_ego < CP.vEgoStopping and - ((v_target_future < CP.vEgoStopping and not accelerating) or brake_pressed)) + v_target_1sec, brake_pressed, cruise_standstill): + accelerating = v_target_1sec > v_target + planned_stop = (v_target < CP.vEgoStopping and + v_target_1sec < CP.vEgoStopping and + not accelerating) + stay_stopped = (v_ego < CP.vEgoStopping and + (brake_pressed or cruise_standstill)) + stopping_condition = planned_stop or stay_stopped - starting_condition = v_target_future > CP.vEgoStarting and accelerating and not cruise_standstill + starting_condition = (v_target_1sec > CP.vEgoStarting and + accelerating and + not cruise_standstill and + not brake_pressed) + started_condition = v_ego > CP.vEgoStarting if not active: long_control_state = LongCtrlState.off @@ -34,7 +36,15 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, long_control_state = LongCtrlState.stopping elif long_control_state == LongCtrlState.stopping: - if starting_condition: + if starting_condition and CP.startingState: + long_control_state = LongCtrlState.starting + elif starting_condition: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.starting: + if stopping_condition: + long_control_state = LongCtrlState.stopping + elif started_condition: long_control_state = LongCtrlState.pid return long_control_state @@ -60,64 +70,63 @@ class LongControl: # Interp control trajectory speeds = long_plan.speeds if len(speeds) == CONTROL_N: - v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) + v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_lower = 2 * (v_target_lower - v_target) / self.CP.longitudinalActuatorDelayLowerBound - a_target + a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_upper = 2 * (v_target_upper - v_target) / self.CP.longitudinalActuatorDelayUpperBound - a_target + a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now + + v_target = min(v_target_lower, v_target_upper) a_target = min(a_target_lower, a_target_upper) - v_target_future = speeds[-1] + v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds) else: v_target = 0.0 - v_target_future = 0.0 + v_target_now = 0.0 + v_target_1sec = 0.0 a_target = 0.0 - # TODO: This check is not complete and needs to be enforced by MPC - a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO) - self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] - # Update state machine output_accel = self.last_output_accel self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, - v_target, v_target_future, CS.brakePressed, + v_target, v_target_1sec, CS.brakePressed, CS.cruiseState.standstill) if self.long_control_state == LongCtrlState.off: self.reset(CS.vEgo) output_accel = 0. - # tracking objects and driving + elif self.long_control_state == LongCtrlState.stopping: + if output_accel > self.CP.stopAccel: + output_accel = min(output_accel, 0.0) + output_accel -= self.CP.stoppingDecelRate * DT_CTRL + self.reset(CS.vEgo) + + elif self.long_control_state == LongCtrlState.starting: + output_accel = self.CP.startAccel + self.reset(CS.vEgo) + elif self.long_control_state == LongCtrlState.pid: - self.v_pid = v_target + self.v_pid = v_target_now # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration - prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid + # TODO too complex, needs to be simplified and tested on toyotas + prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) freeze_integrator = prevent_overshoot error = self.v_pid - CS.vEgo error_deadzone = apply_deadzone(error, deadzone) - output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=a_target, freeze_integrator=freeze_integrator) + output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, + feedforward=a_target, + freeze_integrator=freeze_integrator) - if prevent_overshoot: - output_accel = min(output_accel, 0.0) + self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) - # Intention is to stop, switch to a different brake control until we stop - elif self.long_control_state == LongCtrlState.stopping: - # Keep applying brakes until the car is stopped - if not CS.standstill or output_accel > self.CP.stopAccel: - output_accel -= self.CP.stoppingDecelRate * DT_CTRL - output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) - self.reset(CS.vEgo) - - self.last_output_accel = output_accel - final_accel = clip(output_accel, accel_limits[0], accel_limits[1]) - - return final_accel + return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 94efd7a87..6b7981311 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -3,7 +3,7 @@ import os import numpy as np from common.realtime import sec_since_boot -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from system.swaglog import cloudlog from selfdrive.modeld.constants import index_function from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU @@ -20,11 +20,11 @@ LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") -SOURCES = ['lead0', 'lead1', 'cruise'] +SOURCES = ['lead0', 'lead1', 'cruise', 'e2e'] X_DIM = 3 U_DIM = 1 -PARAM_DIM = 4 +PARAM_DIM = 6 COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 @@ -36,7 +36,8 @@ A_EGO_COST = 0. J_EGO_COST = 5.0 A_CHANGE_COST = 200. DANGER_ZONE_COST = 100. -CRASH_DISTANCE = .5 +CRASH_DISTANCE = .25 +LEAD_DANGER_FACTOR = 0.75 LIMIT_COST = 1e6 ACADOS_SOLVER_TYPE = 'SQP_RTI' @@ -48,8 +49,10 @@ MAX_T = 10.0 T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)] T_IDXS = np.array(T_IDXS_LST) +FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) MIN_ACCEL = -3.5 +MAX_ACCEL = 2.0 T_FOLLOW = 1.45 COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 @@ -57,8 +60,8 @@ STOP_DISTANCE = 6.0 def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) -def get_safe_obstacle_distance(v_ego): - return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE +def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW): + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE def desired_follow_distance(v_ego, v_lead): return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) @@ -89,7 +92,9 @@ def gen_long_model(): a_max = SX.sym('a_max') x_obstacle = SX.sym('x_obstacle') prev_a = SX.sym('prev_a') - model.p = vertcat(a_min, a_max, x_obstacle, prev_a) + lead_t_follow = SX.sym('lead_t_follow') + lead_danger_factor = SX.sym('lead_danger_factor') + model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -123,11 +128,13 @@ def gen_long_ocp(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] prev_a = ocp.model.p[3] + lead_t_follow = ocp.model.p[4] + lead_danger_factor = ocp.model.p[5] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - desired_dist_comfort = get_safe_obstacle_distance(v_ego) + desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car @@ -148,12 +155,12 @@ def gen_long_ocp(): constraints = vertcat(v_ego, (a_ego - a_min), (a_max - a_ego), - ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) + ((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.)) ocp.model.con_h_expr = constraints x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW, LEAD_DANGER_FACTOR]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -190,8 +197,8 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, e2e=False): - self.e2e = e2e + def __init__(self, mode='acc'): + self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] @@ -225,49 +232,40 @@ class LongitudinalMpc: self.x0 = np.zeros(X_DIM) self.set_weights() - def set_weights(self, prev_accel_constraint=True): - if self.e2e: - self.set_weights_for_xva_policy() - self.params[:,0] = -10. - self.params[:,1] = 10. - self.params[:,2] = 1e5 - else: - self.set_weights_for_lead_policy(prev_accel_constraint) - - def set_weights_for_lead_policy(self, prev_accel_constraint=True): - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST])) + def set_cost_weights(self, cost_weights, constraint_cost_weights): + W = np.asfortranarray(np.diag(cost_weights)) for i in range(N): + # TODO don't hardcode A_CHANGE_COST idx # reduce the cost on (a-a_prev) later in the horizon. - W[4,4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) + W[4,4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) # Set L2 slack cost on lower bound constraints - Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]) + Zl = np.array(constraint_cost_weights) for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights_for_xva_policy(self): - W = np.asfortranarray(np.diag([0., 0.2, 0.25, 1., 0.0, .1])) - for i in range(N): - self.solver.cost_set(i, 'W', W) - # Setting the slice without the copy make the array not contiguous, - # causing issues with the C interface. - self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) - - # Set L2 slack cost on lower bound constraints - Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]) - for i in range(N): - self.solver.cost_set(i, 'Zl', Zl) + def set_weights(self, prev_accel_constraint=True): + if self.mode == 'acc': + a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] + elif self.mode == 'blended': + a_change_cost = 40.0 if prev_accel_constraint else 0 + cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] + else: + raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') + self.set_cost_weights(cost_weights, constraint_cost_weights) def set_cur_state(self, v, a): v_prev = self.x0[1] self.x0[1] = v self.x0[2] = a - if abs(v_prev - v) > 2.: # probably only helps if v < v_prev + if abs(v_prev - v) > 2.: # probably only helps if v < v_prev for i in range(0, N+1): self.solver.set(i, 'x', self.x0) @@ -303,61 +301,89 @@ class LongitudinalMpc: return lead_xv def set_accel_limits(self, min_a, max_a): + # TODO this sets a max accel limit, but the minimum limit is only for cruise decel + # needs refactor self.cruise_min_a = min_a - self.cruise_max_a = max_a + self.max_a = max_a - def update(self, carstate, radarstate, v_cruise): + def update(self, carstate, radarstate, v_cruise, x, v, a, j): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) - # set accel limits in params - self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL]) - self.params[:,1] = self.cruise_max_a - # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) - # Fake an obstacle for cruise, this ensures smooth acceleration to set speed - # when the leads are no factor. - v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) - v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), - v_lower, - v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped) + self.params[:,0] = MIN_ACCEL + self.params[:,1] = self.max_a + + # Update in ACC mode or ACC/e2e blend + if self.mode == 'acc': + self.params[:,5] = LEAD_DANGER_FACTOR + + # Fake an obstacle for cruise, this ensures smooth acceleration to set speed + # when the leads are no factor. + v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) + v_upper = v_ego + (T_IDXS * self.max_a * 1.05) + v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), + v_lower, + v_upper) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped) + x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) + self.source = SOURCES[np.argmin(x_obstacles[0])] + + # These are not used in ACC mode + x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0 + + elif self.mode == 'blended': + self.params[:,5] = 1.0 + + x_obstacles = np.column_stack([lead_0_obstacle, + lead_1_obstacle]) + cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] + xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) + x = np.cumsum(np.insert(xforward, 0, x[0])) + + x_and_cruise = np.column_stack([x, cruise_target]) + x = np.min(x_and_cruise, axis=1) + + self.source = 'e2e' if x_and_cruise[0,0] < x_and_cruise[0,1] else 'cruise' + + else: + raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update') + + self.yref[:,1] = x + self.yref[:,2] = v + self.yref[:,3] = a + self.yref[:,5] = j + for i in range(N): + self.solver.set(i, "yref", self.yref[i]) + self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) - self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.prev_a) + self.params[:,4] = T_FOLLOW self.run() - if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and + if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and radarstate.leadOne.modelProb > 0.9): self.crash_cnt += 1 else: self.crash_cnt = 0 - def update_with_xva(self, x, v, a): - # v, and a are in local frame, but x is wrt the x[0] position - # In >90degree turns, x goes to 0 (and may even be -ve) - # So, we use integral(v) + x[0] to obtain the forward-distance - xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) - x = np.cumsum(np.insert(xforward, 0, x[0])) - self.yref[:,1] = x - self.yref[:,2] = v - self.yref[:,3] = a - for i in range(N): - self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - self.params[:,3] = np.copy(self.prev_a) - self.run() + # Check if it got within lead comfort range + # TODO This should be done cleaner + if self.mode == 'blended': + if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], T_FOLLOW))- self.x_sol[:,0] < 0.0): + self.source = 'lead0' + if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], T_FOLLOW))- self.x_sol[:,0] < 0.0) and \ + (lead_1_obstacle[0] - lead_0_obstacle[0]): + self.source = 'lead1' def run(self): # t0 = sec_since_boot() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cf5113677..def0a1208 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,15 +1,16 @@ #!/usr/bin/env python3 import math import numpy as np -from common.numpy_fast import interp +from common.numpy_fast import clip, interp import cereal.messaging as messaging from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter +from common.params import Params from common.realtime import DT_MDL 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 +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 system.swaglog import cloudlog @@ -17,8 +18,8 @@ from system.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted A_CRUISE_MIN = -1.2 -A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6] -A_CRUISE_MAX_BP = [0., 15., 25., 40.] +A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] +A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] @@ -44,24 +45,52 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -class Planner: +class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP + self.params = Params() + self.param_read_counter = 0 + self.mpc = LongitudinalMpc() + self.read_param() self.fcw = False self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) + self.v_model_error = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - def update(self, sm): - v_ego = sm['carState'].vEgo + def read_param(self): + e2e = self.params.get_bool('ExperimentalMode') and self.CP.openpilotLongitudinalControl + self.mpc.mode = 'blended' if e2e else 'acc' + @staticmethod + def parse_model(model_msg, model_error): + if (len(model_msg.position.x) == 33 and + len(model_msg.velocity.x) == 33 and + len(model_msg.acceleration.x) == 33): + x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC + v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error + a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x) + j = np.zeros(len(T_IDXS_MPC)) + else: + x = np.zeros(len(T_IDXS_MPC)) + v = np.zeros(len(T_IDXS_MPC)) + a = np.zeros(len(T_IDXS_MPC)) + j = np.zeros(len(T_IDXS_MPC)) + return x, v, a, j + + def update(self, sm, read=True): + if self.param_read_counter % 50 == 0 and read: + self.read_param() + self.param_read_counter += 1 + + v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS @@ -75,15 +104,24 @@ class Planner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) + if self.mpc.mode == 'acc': + accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) + else: + accel_limits = [MIN_ACCEL, MAX_ACCEL] + accel_limits_turns = [MIN_ACCEL, MAX_ACCEL] + if reset_state: self.v_desired_filter.x = v_ego - self.a_desired = 0.0 + # Clip aEgo to cruise limits to prevent large accelerations when becoming active + self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) # 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 - accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] - accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) @@ -95,14 +133,15 @@ class Planner: self.mpc.set_weights(prev_accel_constraint) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['carState'], sm['radarState'], v_cruise) + x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) + self.mpc.update(sm['carState'], 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.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 - self.fcw = self.mpc.crash_cnt > 5 + self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill if self.fcw: cloudlog.info("FCW triggered") diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 85699866b..4bb017926 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -2,8 +2,7 @@ from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D -# the longer lead decels, the more likely it will keep decelerating -# TODO is this a good default? +# Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 1.5 # radar tracks @@ -152,7 +151,8 @@ class Cluster(): def potential_low_speed_lead(self, v_ego): # stop for stuff in front of you and low speed, even without model confirmation - return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25 + # Radar points closer than 0.75, are almost always glitches on toyota radars + return abs(self.yRel) < 1.0 and (v_ego < v_ego_stationary) and (0.75 < self.dRel < 25) def is_potential_fcw(self, model_prob): return model_prob > .9 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ca7523f2e..93d0c80da 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -3,7 +3,7 @@ from cereal import car from common.params import Params from common.realtime import Priority, config_realtime_process from system.swaglog import cloudlog -from selfdrive.controls.lib.longitudinal_planner import Planner +from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging @@ -16,13 +16,8 @@ def plannerd_thread(sm=None, pm=None): CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) - use_lanelines = False - wide_camera = params.get_bool('WideCameraOnly') - - cloudlog.event("e2e mode", on=use_lanelines) - - longitudinal_planner = Planner(CP) - lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) + longitudinal_planner = LongitudinalPlanner(CP) + lateral_planner = LateralPlanner(CP) if sm is None: sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py index b6f3c91bd..6436abb4f 100755 --- a/selfdrive/debug/check_freq.py +++ b/selfdrive/debug/check_freq.py @@ -2,7 +2,7 @@ import argparse import numpy as np from collections import defaultdict, deque -from typing import DefaultDict, Deque +from typing import DefaultDict, Deque, MutableSequence from common.realtime import sec_since_boot import cereal.messaging as messaging @@ -19,7 +19,7 @@ if __name__ == "__main__": socket_names = args.socket sockets = {} - rcv_times: DefaultDict[str, Deque[float]] = defaultdict(lambda: deque(maxlen=100)) + rcv_times: DefaultDict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100)) valids: DefaultDict[str, Deque[bool]] = defaultdict(lambda: deque(maxlen=100)) t = sec_since_boot() diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index f182026aa..ac7e7102d 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -6,7 +6,7 @@ firmware versions. If you want to try on a new radar make sure to note the defau in case it's different from the other radars and you need to revert the changes. After changing the config the car should not show any faults when openpilot is not running. -These config changes are persistent accross car reboots. You need to run this script again +These config changes are persistent across car reboots. You need to run this script again to go back to the default values. USE AT YOUR OWN RISK! Safety features, like AEB and FCW, might be affected by these changes.""" diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index c55a05815..8c4dbc55e 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -67,20 +67,30 @@ if __name__ == "__main__": print("Timeout fetching data from EPS") quit() - coding_variant, current_coding_array = None, None - # EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS) - # APA racks (MQB_PP_APA) have a different coding layout, which should - # be easy to support once we identify the specific config bit - if odx_file == "EV_SteerAssisMQB\x00": + 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" - current_coding_array = struct.unpack("!4B", current_coding) - hca_enabled = (current_coding_array[0] & (1 << 4) != 0) - hca_text = ("DISABLED", "ENABLED")[hca_enabled] - print(f" Lane Assist: {hca_text}") + 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": + coding_variant = "APA" + coding_byte = 3 + coding_bit = 0 + else: print("Configuration changes not yet supported on this EPS!") quit() + current_coding_array = struct.unpack(f"!{coding_length}B", current_coding) + hca_enabled = (current_coding_array[coding_byte] & (1 << coding_bit) != 0) + hca_text = ("DISABLED", "ENABLED")[hca_enabled] + print(f" Lane Assist: {hca_text}") + try: params = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION).decode("utf-8") param_version_system_params = params[1:3] @@ -101,14 +111,14 @@ if __name__ == "__main__": if args.action in ["enable", "disable"]: print("\nAttempting configuration update") - assert(coding_variant == "ZF") # revisit when we have the APA rack coding bit + assert(coding_variant in ("ZF", "APA")) # ZF 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_0 = current_coding_array[0] | (1 << 4) + new_byte = current_coding_array[coding_byte] | (1 << coding_bit) else: - new_byte_0 = current_coding_array[0] & ~(1 << 4) - new_coding = new_byte_0.to_bytes(1, "little") + current_coding[1:] + new_byte = current_coding_array[coding_byte] & ~(1 << coding_bit) + new_coding = current_coding[0:coding_byte] + new_byte.to_bytes(1, "little") + current_coding[coding_byte+1:] try: seed = uds_client.security_access(ACCESS_TYPE_LEVEL_1.REQUEST_SEED) # type: ignore diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 9e6536f9b..1c68eb67b 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -31,6 +31,7 @@ INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_WANTED = 50 # We want a little bit more than we need for stability MAX_ALLOWED_SPREAD = np.radians(2) RPY_INIT = np.array([0.0,0.0,0.0]) +WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0]) # These values are needed to accommodate biggest modelframe PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657]) @@ -65,8 +66,8 @@ class Calibrator: # Read saved calibration params = Params() calibration_params = params.get("CalibrationParams") - self.wide_camera = params.get_bool('WideCameraOnly') rpy_init = RPY_INIT + wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT valid_blocks = 0 if param_put and calibration_params: @@ -74,24 +75,34 @@ class Calibrator: msg = log.Event.from_bytes(calibration_params) rpy_init = np.array(msg.liveCalibration.rpyCalib) valid_blocks = msg.liveCalibration.validBlocks + wide_from_device_euler = np.array(msg.liveCalibration.wideFromDeviceEuler) except Exception: cloudlog.exception("Error reading cached CalibrationParams") - self.reset(rpy_init, valid_blocks) + self.reset(rpy_init, valid_blocks, wide_from_device_euler) self.update_status() - def reset(self, rpy_init: np.ndarray = RPY_INIT, valid_blocks: int = 0, smooth_from: Optional[np.ndarray] = None) -> None: + def reset(self, rpy_init: np.ndarray = RPY_INIT, + valid_blocks: int = 0, + wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT, + smooth_from: Optional[np.ndarray] = None) -> None: if not np.isfinite(rpy_init).all(): self.rpy = RPY_INIT.copy() else: self.rpy = rpy_init.copy() + if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3: + self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy() + else: + self.wide_from_device_euler = wide_from_device_euler_init.copy() + if not np.isfinite(valid_blocks) or valid_blocks < 0: self.valid_blocks = 0 else: self.valid_blocks = valid_blocks self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1)) + self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1)) self.idx = 0 self.block_idx = 0 @@ -113,6 +124,7 @@ class Calibrator: def update_status(self) -> None: valid_idxs = self.get_valid_idxs() if valid_idxs: + self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0) rpys = self.rpys[valid_idxs] self.rpy = np.mean(rpys, axis=0) max_rpy_calib = np.array(np.max(rpys, axis=0)) @@ -146,14 +158,14 @@ class Calibrator: else: return self.rpy - def handle_cam_odom(self, trans: List[float], rot: List[float], trans_std: List[float]) -> Optional[np.ndarray]: + def handle_cam_odom(self, trans: List[float], + 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) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) - if self.wide_camera: - angle_std_threshold = 4*MAX_VEL_ANGLE_STD - else: - angle_std_threshold = MAX_VEL_ANGLE_STD + angle_std_threshold = MAX_VEL_ANGLE_STD certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or (self.valid_blocks < INPUTS_NEEDED)) if not (straight_and_fast and certain_if_calib): @@ -165,7 +177,14 @@ class Calibrator: new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy))) new_rpy = sanity_clip(new_rpy) - self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE) + if len(wide_from_device_euler) == 3: + new_wide_from_device_euler = np.array(wide_from_device_euler) + else: + new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT + self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE) + self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] + + (BLOCK_SIZE - self.idx) * new_wide_from_device_euler) / float(BLOCK_SIZE) self.idx = (self.idx + 1) % BLOCK_SIZE if self.idx == 0: self.block_idx += 1 @@ -187,6 +206,7 @@ class Calibrator: liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) liveCalibration.rpyCalib = smooth_rpy.tolist() liveCalibration.rpyCalibSpread = self.calib_spread.tolist() + liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist() if self.not_car: liveCalibration.validBlocks = INPUTS_NEEDED @@ -223,6 +243,7 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m calibrator.handle_v_ego(sm['carState'].vEgo) new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, sm['cameraOdometry'].rot, + sm['cameraOdometry'].wideFromDeviceEuler, sm['cameraOdometry'].transStd) if DEBUG and new_rpy is not None: diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 830eb3320..6936d88ac 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -16,7 +16,7 @@ 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_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -28,7 +28,7 @@ from system.swaglog import cloudlog MAX_TIME_GAP = 10 EPHEMERIS_CACHE = 'LaikadEphemeris' -DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache" +DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/" CACHE_VERSION = 0.1 POS_FIX_RESIDUAL_THRESHOLD = 100.0 @@ -61,6 +61,7 @@ class Laikad: self.last_pos_fix = [] self.last_pos_residual = [] self.last_pos_fix_t = None + self.gps_week = None self.use_qcom = use_qcom def load_cache(self): @@ -107,11 +108,11 @@ class Laikad: return self.last_pos_fix def is_good_report(self, gnss_msg): - if gnss_msg.which == 'drMeasurementReport' and self.use_qcom: + 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] - elif gnss_msg.which == 'measurementReport' and not self.use_qcom: + elif gnss_msg.which() == 'measurementReport' and not self.use_qcom: return True else: return False @@ -129,9 +130,28 @@ class Laikad: new_meas = read_raw_ublox(report) return week, tow, new_meas + def is_ephemeris(self, gnss_msg): + if self.use_qcom: + return gnss_msg.which() == 'drSvPoly' + else: + return gnss_msg.which() == 'ephemeris' + + def read_ephemeris(self, gnss_msg): + # TODO this only works on GLONASS + if self.use_qcom: + # 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) + else: + ephem = convert_ublox_ephem(gnss_msg.ephemeris) + self.astro_dog.add_navs({ephem.prn: [ephem]}) + self.cache_ephemeris(t=ephem.epoch) + def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): if self.is_good_report(gnss_msg): week, tow, new_meas = self.read_report(gnss_msg) + self.gps_week = week t = gnss_mono_time * 1e-9 if week > 0: @@ -167,17 +187,16 @@ class Laikad: "gpsTimeOfWeek": tow, "positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid), + # TODO std is incorrectly the dimension of the measurements and not 3D "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), "ubloxMonoTime": gnss_mono_time, "correctedMeasurements": meas_msgs } return dat - # TODO this only works on GLONASS, qcom needs live ephemeris parsing too - elif gnss_msg.which == 'ephemeris': - ephem = convert_ublox_ephem(gnss_msg.ephemeris) - self.astro_dog.add_navs({ephem.prn: [ephem]}) - self.cache_ephemeris(t=ephem.epoch) - #elif gnss_msg.which == 'ionoData': + elif self.is_ephemeris(gnss_msg): + self.read_ephemeris(gnss_msg) + + #elif gnss_msg.which() == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): @@ -265,9 +284,11 @@ def create_measurement_msg(meas: GNSSMeasurement): 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 - week, time_of_week = -1, -1 + elif ephem.eph_type == EphemerisType.QCOM_POLY: + source_type = EphemerisSourceType.qcom else: assert ephem.file_epoch is not None week = ephem.file_epoch.week @@ -325,10 +346,11 @@ class EphemerisSourceType(IntEnum): nav = 0 nasaUltraRapid = 1 glonassIacUltraRapid = 2 + qcom = 3 def main(sm=None, pm=None): - use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps") + use_qcom = not Params().get_bool("UbloxAvailable", block=True) if use_qcom: raw_gnss_socket = "qcomGnss" else: @@ -348,6 +370,17 @@ def main(sm=None, pm=None): if sm.updated[raw_gnss_socket]: gnss_msg = sm[raw_gnss_socket] + + # 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']: + continue + + 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) + continue + msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) if msg is not None: pm.send('gnssMeasurements', msg) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 7714931b9..4325900c0 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -19,6 +19,14 @@ const double VALID_TIME_SINCE_RESET = 1.0; // s const double VALID_POS_STD = 50.0; // m const double MAX_RESET_TRACKER = 5.0; const double SANE_GPS_UNCERTAINTY = 1500.0; // m +const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker +const double DECAY = 0.99995; // same as reset tracker +const double MAX_FILTER_REWIND_TIME = 0.8; // s + +// TODO: GPS sensor time offsets are empirically calculated +// They should be replaced with synced time from a real clock +const double GPS_LOCATION_SENSOR_TIME_OFFSET = 0.630; // s +const double GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET = 0.095; // s static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { VectorXd res(floatlist.size()); @@ -195,51 +203,69 @@ VectorXd Localizer::get_stdev() { return this->kf->get_P().diagonal().array().sqrt(); } -void Localizer::handle_sensors(double current_time, const capnp::List::Reader& log) { +bool Localizer::are_inputs_ok() { + return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid; +} + +void Localizer::observation_timings_invalid_reset(){ + this->observation_timings_invalid = false; +} + +void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { // TODO does not yet account for double sensor readings in the log - for (int i = 0; i < log.size(); i++) { - const cereal::SensorEventData::Reader& sensor_reading = log[i]; - // Ignore empty readings (e.g. in case the magnetometer had no data ready) - if (sensor_reading.getTimestamp() == 0) { - continue; + // Ignore empty readings (e.g. in case the magnetometer had no data ready) + if (log.getTimestamp() == 0) { + return; + } + + double sensor_time = 1e-9 * log.getTimestamp(); + + // sensor time and log time should be close + if (std::abs(current_time - sensor_time) > 0.1) { + LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); + this->observation_timings_invalid = true; + return; + } + else if (!this->is_timestamp_valid(sensor_time)) { + this->observation_timings_invalid = true; + return; + } + + // TODO: handle messages from two IMUs at the same time + if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) { + return; + } + + // Gyro Uncalibrated + if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { + auto v = log.getGyroUncalibrated().getV(); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ROTATION_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); + this->observation_values_invalid["gyroscope"] *= DECAY; } - - double sensor_time = 1e-9 * sensor_reading.getTimestamp(); - - // sensor time and log time should be close - if (std::abs(current_time - sensor_time) > 0.1) { - LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); - return; + else{ + this->observation_values_invalid["gyroscope"] += 1.0; } + } - // TODO: handle messages from two IMUs at the same time - if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::BMX055) { - continue; + // Accelerometer + if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) { + auto v = log.getAcceleration().getV(); + + // TODO: reduce false positives and re-enable this check + // check if device fell, estimate 10 for g + // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving + // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; + + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ACCEL_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); + this->observation_values_invalid["accelerometer"] *= DECAY; } - - // Gyro Uncalibrated - if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { - auto v = sensor_reading.getGyroUncalibrated().getV(); - auto meas = Vector3d(-v[2], -v[1], -v[0]); - if (meas.norm() < ROTATION_SANITY_CHECK) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); - } - } - - // Accelerometer - if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) { - auto v = sensor_reading.getAcceleration().getV(); - - // TODO: reduce false positives and re-enable this check - // check if device fell, estimate 10 for g - // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving - //this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - - auto meas = Vector3d(-v[2], -v[1], -v[0]); - if (meas.norm() < ACCEL_SANITY_CHECK) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); - } + else{ + this->observation_values_invalid["accelerometer"] += 1.0; } } } @@ -260,7 +286,7 @@ void Localizer::input_fake_gps_observations(double current_time) { this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } -void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) { +void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { // ignore the message if the fix is invalid bool gps_invalid_flag = (log.getFlags() % 2 == 0); bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); @@ -268,13 +294,22 @@ 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); - if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){ + // 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) { + this->gps_valid = false; this->determine_gps_mode(current_time); return; } + double sensor_time = current_time - sensor_time_offset; + // Process message - this->last_gps_fix = current_time; + this->gps_valid = true; this->gps_mode = true; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; this->converter = std::make_unique(geodetic); @@ -303,14 +338,14 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); } else if (gps_est_error > 100.0) { LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); } - this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { @@ -324,8 +359,14 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; return; } @@ -333,10 +374,12 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; return; } if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; return; } @@ -352,12 +395,19 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry { rot_device }, { rot_device_cov }); this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, { trans_device }, { trans_device_cov }); + this->observation_values_invalid["cameraOdometry"] *= DECAY; } void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + if (log.getRpyCalib().size() > 0) { auto live_calib = floatlist2vector(log.getRpyCalib()); if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { + this->observation_values_invalid["liveCalibration"] += 1.0; return; } @@ -365,6 +415,7 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra this->device_from_calib = euler2rot(this->calib); this->calib_from_device = this->device_from_calib.transpose(); this->calibrated = log.getCalStatus() == 1; + this->observation_values_invalid["liveCalibration"] *= DECAY; } } @@ -396,8 +447,8 @@ void Localizer::time_check(double current_time) { void Localizer::update_reset_tracker() { // reset tracker is tuned to trigger when over 1reset/10s over 2min period - if (this->isGpsOK()) { - this->reset_tracker *= .99995; + if (this->is_gps_ok()) { + this->reset_tracker *= DECAY; } else { this->reset_tracker = 0.0; } @@ -441,12 +492,14 @@ void Localizer::handle_msg_bytes(const char *data, const size_t size) { void Localizer::handle_msg(const cereal::Event::Reader& log) { double t = log.getLogMonoTime() * 1e-9; this->time_check(t); - if (log.isSensorEvents()) { - this->handle_sensors(t, log.getSensorEvents()); + if (log.isAccelerometer()) { + this->handle_sensor(t, log.getAccelerometer()); + } else if (log.isGyroscope()) { + this->handle_sensor(t, log.getGyroscope()); } else if (log.isGpsLocation()) { - this->handle_gps(t, log.getGpsLocation()); + this->handle_gps(t, log.getGpsLocation(), GPS_LOCATION_SENSOR_TIME_OFFSET); } else if (log.isGpsLocationExternal()) { - this->handle_gps(t, log.getGpsLocationExternal()); + this->handle_gps(t, log.getGpsLocationExternal(), GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET); } else if (log.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -470,15 +523,32 @@ kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_build return msg_builder.toBytes(); } +bool Localizer::is_gps_ok() { + return this->gps_valid; +} -bool Localizer::isGpsOK() { - return this->kf->get_filter_time() - this->last_gps_fix < 1.0; +bool Localizer::critical_services_valid(std::map critical_services) { + for (auto &kv : critical_services){ + if (kv.second >= INPUT_INVALID_THRESHOLD){ + return false; + } + } + return true; +} + +bool Localizer::is_timestamp_valid(double current_time) { + double filter_time = this->kf->get_filter_time(); + if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { + LOGE("Observation timestamp is older than the max rewind threshold of the filter"); + return false; + } + return true; } void Localizer::determine_gps_mode(double current_time) { - // 1. If the pos_std is greater than what's not acceptible and localizer is in gps-mode, reset to no-gps-mode - // 2. If the pos_std is greater than what's not acceptible and localizer is in no-gps-mode, fake obs - // 3. If the pos_std is smaller than what's not acceptible, let gps-mode be whatever it is + // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode + // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs + // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt(); if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ if (this->gps_mode){ @@ -492,13 +562,16 @@ void Localizer::determine_gps_mode(double current_time) { } int Localizer::locationd_thread() { + ublox_available = Params().getBool("UbloxAvailable", true); + const char* gps_location_socket; - if (util::file_exists("/persist/comma/use-quectel-rawgps")) { - gps_location_socket = "gpsLocation"; - } else { + if (ublox_available) { gps_location_socket = "gpsLocationExternal"; + } else { + gps_location_socket = "gpsLocation"; } - const std::initializer_list service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"}; + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + "carState", "carParams", "accelerometer", "gyroscope"}; PubMaster pm({"liveLocationKalman"}); // TODO: remove carParams once we're always sending at 100Hz @@ -506,10 +579,15 @@ int Localizer::locationd_thread() { uint64_t cnt = 0; bool filterInitialized = false; + const std::vector critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; + for (std::string service : critical_input_services) { + this->observation_values_invalid.insert({service, 0.0}); + } while (!do_exit) { sm.update(); if (filterInitialized){ + this->observation_timings_invalid_reset(); for (const char* service : service_list) { if (sm.updated(service) && sm.valid(service)){ const cereal::Event::Reader log = sm[service]; @@ -521,11 +599,11 @@ int Localizer::locationd_thread() { } // 100Hz publish for notcars, 20Hz for cars - const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "sensorEvents" : "cameraOdometry"; + const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "accelerometer" : "cameraOdometry"; if (sm.updated(trigger_msg)) { - bool inputsOK = sm.allAliveAndValid(); - bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents"); - bool gpsOK = this->isGpsOK(); + bool inputsOK = sm.allAliveAndValid() && this->are_inputs_ok(); + bool gpsOK = this->is_gps_ok(); + bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); MessageBuilder msg_builder; kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 7c0cb6b7f..f0872d9f5 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include "cereal/messaging/messaging.h" @@ -32,8 +33,12 @@ public: void finite_check(double current_time = NAN); void time_check(double current_time = NAN); void update_reset_tracker(); - bool isGpsOK(); + bool is_gps_ok(); + bool critical_services_valid(std::map critical_services); + bool is_timestamp_valid(double current_time); void determine_gps_mode(double current_time); + bool are_inputs_ok(); + void observation_timings_invalid_reset(); kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); @@ -45,8 +50,8 @@ public: void handle_msg_bytes(const char *data, const size_t size); void handle_msg(const cereal::Event::Reader& log); - void handle_sensors(double current_time, const capnp::List::Reader& log); - void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log); + void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); void handle_car_state(double current_time, const cereal::CarState::Reader& log); void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); @@ -68,8 +73,11 @@ private: std::unique_ptr converter; int64_t unix_timestamp_millis = 0; - double last_gps_fix = 0; double reset_tracker = 0.0; bool device_fell = false; bool gps_mode = false; + bool gps_valid = false; + bool ublox_available = true; + bool observation_timings_invalid = false; + std::map observation_values_invalid; }; diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc index 5ff0f2699..f8c03365e 100755 --- a/selfdrive/locationd/models/live_kf.cc +++ b/selfdrive/locationd/models/live_kf.cc @@ -44,7 +44,7 @@ LiveKalman::LiveKalman() { // init filter this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), - std::vector{3}, std::vector(), 0.2); + std::vector{3}, std::vector(), 0.8); } void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) { diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 86672b046..9bd4ed083 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -92,7 +92,7 @@ class ParamsLearner: self.speed = msg.vEgo in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed - self.active = self.speed > 5 and in_linear_region + self.active = self.speed > 1 and in_linear_region if self.active: self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]])) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py new file mode 100755 index 000000000..588bca157 --- /dev/null +++ b/selfdrive/locationd/torqued.py @@ -0,0 +1,301 @@ +#!/usr/bin/env python3 +import os +import sys +import signal +import numpy as np +from collections import deque, defaultdict + +import cereal.messaging as messaging +from cereal import car, log +from common.params import Params +from common.realtime import config_realtime_process, DT_MDL +from common.filter_simple import FirstOrderFilter +from system.swaglog import cloudlog +from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY + +HISTORY = 5 # secs +POINTS_PER_BUCKET = 1500 +MIN_POINTS_TOTAL = 4000 +MIN_POINTS_TOTAL_QLOG = 600 +FIT_POINTS_TOTAL = 2000 +FIT_POINTS_TOTAL_QLOG = 600 +MIN_VEL = 15 # m/s +FRICTION_FACTOR = 1.5 # ~85% of data coverage +FACTOR_SANITY = 0.3 +FRICTION_SANITY = 0.5 +STEER_MIN_THRESHOLD = 0.02 +MIN_FILTER_DECAY = 50 +MAX_FILTER_DECAY = 250 +LAT_ACC_THRESHOLD = 1 +STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)] +MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) +MAX_RESETS = 5.0 +MAX_INVALID_THRESHOLD = 10 +MIN_ENGAGE_BUFFER = 2 # secs + +VERSION = 1 # bump this to invalidate old parameter caches +ALLOWED_CARS = ['toyota', 'hyundai'] + + +def slope2rot(slope): + sin = np.sqrt(slope**2 / (slope**2 + 1)) + cos = np.sqrt(1 / (slope**2 + 1)) + return np.array([[cos, -sin], [sin, cos]]) + + +class NPQueue: + def __init__(self, maxlen, rowsize): + self.maxlen = maxlen + self.arr = np.empty((0, rowsize)) + + def __len__(self): + return len(self.arr) + + def append(self, pt): + if len(self.arr) < self.maxlen: + self.arr = np.append(self.arr, [pt], axis=0) + else: + self.arr[:-1] = self.arr[1:] + self.arr[-1] = pt + + +class PointBuckets: + def __init__(self, x_bounds, min_points, min_points_total): + self.x_bounds = x_bounds + self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds} + self.buckets_min_points = {bounds: min_point for bounds, min_point in zip(x_bounds, min_points)} + self.min_points_total = min_points_total + + def bucket_lengths(self): + return [len(v) for v in self.buckets.values()] + + def __len__(self): + return sum(self.bucket_lengths()) + + def is_valid(self): + return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= self.min_points_total) + + def add_point(self, x, y): + for bound_min, bound_max in self.x_bounds: + if (x >= bound_min) and (x < bound_max): + self.buckets[(bound_min, bound_max)].append([x, 1.0, y]) + break + + def get_points(self, num_points=None): + points = np.vstack([x.arr for x in self.buckets.values()]) + if num_points is None: + return points + return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] + + def load_points(self, points): + for x, y in points: + self.add_point(x, y) + + +class TorqueEstimator: + def __init__(self, CP, decimated=False): + self.hist_len = int(HISTORY / DT_MDL) + self.lag = CP.steerActuatorDelay + .2 # from controlsd + if decimated: + self.min_bucket_points = MIN_BUCKET_POINTS / 10 + self.min_points_total = MIN_POINTS_TOTAL_QLOG + self.fit_points = FIT_POINTS_TOTAL_QLOG + else: + self.min_bucket_points = MIN_BUCKET_POINTS + self.min_points_total = MIN_POINTS_TOTAL + self.fit_points = FIT_POINTS_TOTAL + + self.offline_friction = 0.0 + self.offline_latAccelFactor = 0.0 + self.resets = 0.0 + self.use_params = CP.carName in ALLOWED_CARS + + if CP.lateralTuning.which() == 'torque': + self.offline_friction = CP.lateralTuning.torque.friction + self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor + + self.reset() + + initial_params = { + 'latAccelFactor': self.offline_latAccelFactor, + 'latAccelOffset': 0.0, + 'frictionCoefficient': self.offline_friction, + 'points': [] + } + self.decay = MIN_FILTER_DECAY + self.min_lataccel_factor = (1.0 - FACTOR_SANITY) * self.offline_latAccelFactor + self.max_lataccel_factor = (1.0 + FACTOR_SANITY) * self.offline_latAccelFactor + self.min_friction = (1.0 - FRICTION_SANITY) * self.offline_friction + self.max_friction = (1.0 + FRICTION_SANITY) * self.offline_friction + + # try to restore cached params + params = Params() + params_cache = params.get("LiveTorqueCarParams") + torque_cache = params.get("LiveTorqueParameters") + if params_cache is not None and torque_cache is not None: + try: + cache_ltp = log.Event.from_bytes(torque_cache).liveTorqueParameters + cache_CP = car.CarParams.from_bytes(params_cache) + if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION): + if cache_ltp.liveValid: + initial_params = { + 'latAccelFactor': cache_ltp.latAccelFactorFiltered, + 'latAccelOffset': cache_ltp.latAccelOffsetFiltered, + 'frictionCoefficient': cache_ltp.frictionCoefficientFiltered + } + initial_params['points'] = cache_ltp.points + self.decay = cache_ltp.decay + self.filtered_points.load_points(initial_params['points']) + cloudlog.info("restored torque params from cache") + except Exception: + cloudlog.exception("failed to restore cached torque params") + params.remove("LiveTorqueCarParams") + params.remove("LiveTorqueParameters") + + self.filtered_params = {} + for param in initial_params: + self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL) + + def get_restore_key(self, CP, version): + a, b = None, None + if CP.lateralTuning.which() == 'torque': + a = CP.lateralTuning.torque.friction + b = CP.lateralTuning.torque.latAccelFactor + return (CP.carFingerprint, CP.lateralTuning.which(), a, b, version) + + def reset(self): + self.resets += 1.0 + self.invalid_values_tracker = 0.0 + self.decay = MIN_FILTER_DECAY + self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len)) + self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total) + + def estimate_params(self): + points = self.filtered_points.get_points(self.fit_points) + # total least square solution as both x and y are noisy observations + # this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals + try: + _, _, v = np.linalg.svd(points, full_matrices=False) + slope, offset = -v.T[0:2, 2] / v.T[2, 2] + _, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T + friction_coeff = np.std(spread) * FRICTION_FACTOR + except np.linalg.LinAlgError as e: + cloudlog.exception(f"Error computing live torque params: {e}") + slope = offset = friction_coeff = np.nan + return slope, offset, friction_coeff + + def update_params(self, params): + self.decay = min(self.decay + DT_MDL, MAX_FILTER_DECAY) + for param, value in params.items(): + self.filtered_params[param].update(value) + self.filtered_params[param].update_alpha(self.decay) + + def is_sane(self, latAccelFactor, latAccelOffset, friction): + if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, friction]]): + return False + return (self.max_friction >= friction >= self.min_friction) and\ + (self.max_lataccel_factor >= latAccelFactor >= self.min_lataccel_factor) + + def handle_log(self, t, which, msg): + if which == "carControl": + self.raw_points["carControl_t"].append(t + self.lag) + self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) + self.raw_points["active"].append(msg.latActive) + elif which == "carState": + self.raw_points["carState_t"].append(t + self.lag) + self.raw_points["vego"].append(msg.vEgo) + self.raw_points["steer_override"].append(msg.steeringPressed) + elif which == "liveLocationKalman": + if len(self.raw_points['steer_torque']) == self.hist_len: + yaw_rate = msg.angularVelocityCalibrated.value[2] + roll = msg.orientationNED.value[0] + active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['active']).astype(bool) + steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool) + vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego']) + steer = np.interp(t, self.raw_points['carControl_t'], self.raw_points['steer_torque']) + lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY) + if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD): + self.filtered_points.add_point(float(steer), float(lateral_acc)) + + def get_msg(self, valid=True, with_points=False): + msg = messaging.new_message('liveTorqueParameters') + msg.valid = valid + liveTorqueParameters = msg.liveTorqueParameters + liveTorqueParameters.version = VERSION + liveTorqueParameters.useParams = self.use_params + + if self.filtered_points.is_valid(): + latAccelFactor, latAccelOffset, friction_coeff = self.estimate_params() + liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor) + liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset) + liveTorqueParameters.frictionCoefficientRaw = float(friction_coeff) + + if self.is_sane(latAccelFactor, latAccelOffset, friction_coeff): + liveTorqueParameters.liveValid = True + self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': friction_coeff}) + self.invalid_values_tracker = max(0.0, self.invalid_values_tracker - 0.5) + else: + cloudlog.exception("Live torque parameters are outside acceptable bounds.") + liveTorqueParameters.liveValid = False + self.invalid_values_tracker += 1.0 + # Reset when ~10 invalid over 5 secs + if self.invalid_values_tracker > MAX_INVALID_THRESHOLD: + # Do not reset the filter as it may cause a drastic jump, just reset points + self.reset() + else: + liveTorqueParameters.liveValid = False + + if with_points: + liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist() + + liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x) + liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x) + liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x) + liveTorqueParameters.totalBucketPoints = len(self.filtered_points) + liveTorqueParameters.decay = self.decay + liveTorqueParameters.maxResets = self.resets + return msg + + +def main(sm=None, pm=None): + config_realtime_process([0, 1, 2, 3], 5) + + if sm is None: + sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman']) + + if pm is None: + pm = messaging.PubMaster(['liveTorqueParameters']) + + params = Params() + CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) + estimator = TorqueEstimator(CP) + + def cache_params(sig, frame): + signal.signal(sig, signal.SIG_DFL) + cloudlog.warning("caching torque params") + + params = Params() + params.put("LiveTorqueCarParams", CP.as_builder().to_bytes()) + + msg = estimator.get_msg(with_points=True) + params.put("LiveTorqueParameters", msg.to_bytes()) + + sys.exit(0) + if "REPLAY" not in os.environ: + signal.signal(signal.SIGINT, cache_params) + + while True: + sm.update() + if sm.all_checks(): + for which in sm.updated.keys(): + if sm.updated[which]: + t = sm.logMonoTime[which] * 1e-9 + estimator.handle_log(t, which, sm[which]) + + # 4Hz driven by liveLocationKalman + if sm.frame % 5 == 0: + pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index c9f732e9a..b45dffae3 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -103,23 +103,17 @@ std::pair> UbloxMsgParser::gen_msg() { switch (ubx_message.msg_type()) { case 0x0107: return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; - break; case 0x0213: return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; - break; case 0x0215: return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; - break; case 0x0a09: return {"ubloxGnss", gen_mon_hw(static_cast(body))}; - break; case 0x0a0b: return {"ubloxGnss", gen_mon_hw2(static_cast(body))}; - break; default: LOGE("Unknown message type %x", ubx_message.msg_type()); return {"ubloxGnss", kj::Array()}; - break; } } diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index a15aac380..92706c53e 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -5,10 +5,8 @@ libs = [common, cereal, messaging, visionipc, 'avformat', 'avcodec', 'swscale', 'avutil', 'yuv', 'OpenCL', 'pthread'] -src = ['logger.cc', 'video_writer.cc', 'encoder/encoder.cc'] -if arch == "larch64": - src += ['encoder/v4l_encoder.cc'] -else: +src = ['logger.cc', 'video_writer.cc', 'encoder/encoder.cc', 'encoder/v4l_encoder.cc'] +if arch != "larch64": src += ['encoder/ffmpeg_encoder.cc'] if arch == "Darwin": diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc index 90ba487ff..e882e4cf8 100644 --- a/selfdrive/loggerd/bootlog.cc +++ b/selfdrive/loggerd/bootlog.cc @@ -26,10 +26,13 @@ static kj::Array build_boot_log() { // Gather output of commands std::vector bootlog_commands = { - "[ -e /dev/nvme0 ] && sudo nvme smart-log --output-format=json /dev/nvme0", "[ -x \"$(command -v journalctl)\" ] && journalctl", }; + if (Hardware::TICI()) { + bootlog_commands.push_back("[ -e /dev/nvme0 ] && sudo nvme smart-log --output-format=json /dev/nvme0"); + } + auto commands = boot.initCommands().initEntries(bootlog_commands.size()); for (int j = 0; j < bootlog_commands.size(); j++) { auto lentry = commands[j]; @@ -52,13 +55,11 @@ int main(int argc, char** argv) { bool r = util::create_directories(LOG_ROOT + "/boot/", 0775); assert(r); - RawFile bz_file(path.c_str()); - + RawFile file(path.c_str()); // Write initdata - bz_file.write(logger_build_init_data().asBytes()); - + file.write(logger_build_init_data().asBytes()); // Write bootlog - bz_file.write(build_boot_log().asBytes()); + file.write(build_boot_log().asBytes()); return 0; } diff --git a/selfdrive/loggerd/encoder/v4l_encoder.cc b/selfdrive/loggerd/encoder/v4l_encoder.cc index 016e6c570..88aeb2125 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.cc +++ b/selfdrive/loggerd/encoder/v4l_encoder.cc @@ -37,11 +37,11 @@ static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=N assert(v4l_buf.m.planes[0].data_offset == 0); } -static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, VisionBuf *buf, struct timeval timestamp={0}, unsigned int bytesused=0) { +static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, VisionBuf *buf, struct timeval timestamp={}) { v4l2_plane plane = { .length = (unsigned int)buf->len, .m = { .userptr = (unsigned long)buf->addr, }, - .bytesused = bytesused, + .bytesused = (uint32_t)buf->len, .reserved = {(unsigned int)buf->fd} }; @@ -51,7 +51,6 @@ static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, Vis .memory = V4L2_MEMORY_USERPTR, .m = { .planes = &plane, }, .length = 1, - .bytesused = 0, .flags = V4L2_BUF_FLAG_TIMESTAMP_COPY, .timestamp = timestamp }; diff --git a/selfdrive/loggerd/encoder/v4l_encoder.h b/selfdrive/loggerd/encoder/v4l_encoder.h index b7c378be8..c2a53dd6e 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.h +++ b/selfdrive/loggerd/encoder/v4l_encoder.h @@ -28,7 +28,6 @@ private: static void dequeue_handler(V4LEncoder *e); std::thread dequeue_handler_thread; - VisionBuf buf_in[BUF_IN_COUNT]; VisionBuf buf_out[BUF_OUT_COUNT]; SafeQueue free_buf_in; }; diff --git a/selfdrive/loggerd/encoderd.cc b/selfdrive/loggerd/encoderd.cc index 9bd8e2f1d..db5f4b61a 100644 --- a/selfdrive/loggerd/encoderd.cc +++ b/selfdrive/loggerd/encoderd.cc @@ -52,17 +52,23 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { VisionBuf buf_info = vipc_client.buffers[0]; LOGW("encoder %s init %dx%d", cam_info.filename, buf_info.width, buf_info.height); - // main encoder - encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height, - cam_info.fps, cam_info.bitrate, - cam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, - buf_info.width, buf_info.height, false)); - // qcamera encoder - if (cam_info.has_qcamera) { - encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, buf_info.width, buf_info.height, - qcam_info.fps, qcam_info.bitrate, - qcam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, - qcam_info.frame_width, qcam_info.frame_height, false)); + if (buf_info.width > 0 && buf_info.height > 0) { + // main encoder + encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height, + cam_info.fps, cam_info.bitrate, + cam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, + buf_info.width, buf_info.height, false)); + // qcamera encoder + if (cam_info.has_qcamera) { + encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, buf_info.width, buf_info.height, + qcam_info.fps, qcam_info.bitrate, + qcam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, + qcam_info.frame_width, qcam_info.frame_height, false)); + } + } else { + LOGE("not initting empty encoder"); + s->max_waiting--; + break; } } diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc index da3710cc7..aaf267e52 100644 --- a/selfdrive/loggerd/logger.cc +++ b/selfdrive/loggerd/logger.cc @@ -182,7 +182,7 @@ int logger_next(LoggerState *s, const char* root_path, pthread_mutex_unlock(&s->lock); - // write beggining of log metadata + // write beginning of log metadata log_init_data(s); lh_log_sentinel(s->cur_handle, is_start_of_route ? SentinelType::START_OF_ROUTE : SentinelType::START_OF_SEGMENT); return 0; diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index e0892e68b..9beb3c3bf 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -55,7 +55,7 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct int bytes_count = 0; // extract the message - capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr((capnp::word *)msg->getData(), msg->getSize())); + capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr((capnp::word *)msg->getData(), msg->getSize() / sizeof(capnp::word))); auto event = cmsg.getRoot(); auto edata = (name == "driverEncodeData") ? event.getDriverEncodeData() : ((name == "wideRoadEncodeData") ? event.getWideRoadEncodeData() : diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h index 6eafbe08d..1fa634982 100644 --- a/selfdrive/loggerd/loggerd.h +++ b/selfdrive/loggerd/loggerd.h @@ -10,6 +10,7 @@ #include #include #include +#include #include "cereal/messaging/messaging.h" #include "cereal/services.h" diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index 12f894061..c8a7d4153 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -15,7 +15,7 @@ from system.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") -TOTAL_SCONS_NODES = 2035 +TOTAL_SCONS_NODES = 2395 MAX_BUILD_PROGRESS = 100 PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) diff --git a/selfdrive/manager/helpers.py b/selfdrive/manager/helpers.py index b07362dd4..983c7cc0b 100644 --- a/selfdrive/manager/helpers.py +++ b/selfdrive/manager/helpers.py @@ -33,6 +33,6 @@ def unblock_stdout() -> None: pass # os.wait() returns a tuple with the pid and a 16 bit value - # whose low byte is the signal number and whose high byte is the exit satus + # 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) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 9c370cb3d..928507f65 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -39,7 +39,9 @@ def manager_init() -> None: default_params: List[Tuple[str, Union[str, bytes]]] = [ ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "1"), + ("GsmMetered", "1"), ("HasAcceptedTerms", "0"), + ("LanguageSetting", "main_en"), ("OpenpilotEnabledToggle", "1"), ] if not PC: @@ -48,9 +50,6 @@ def manager_init() -> None: if params.get_bool("RecordFrontLock"): params.put_bool("RecordFront", True) - if not params.get_bool("DisableRadar_Allow"): - params.delete("DisableRadar") - # set unset params for k, v in default_params: if params.get(k) is None: diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 927de563c..dbccb8d4a 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -31,6 +31,7 @@ procs = [ NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), + # NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"]), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), @@ -38,13 +39,16 @@ procs = [ NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"), + PythonProcess("torqued", "selfdrive.locationd.torqued"), PythonProcess("controlsd", "selfdrive.controls.controlsd"), PythonProcess("deleter", "selfdrive.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("navd", "selfdrive.navd.navd"), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), + PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI), PythonProcess("plannerd", "selfdrive.controls.plannerd"), PythonProcess("radard", "selfdrive.controls.radard"), PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True), @@ -53,11 +57,9 @@ procs = [ PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), PythonProcess("statsd", "selfdrive.statsd", offroad=True), + # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar), PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar), - - # Experimental - PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=(TICI and os.path.isfile("/persist/comma/use-quectel-rawgps"))), ] managed_processes = {p.name: p for p in procs} diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index f2e5319e8..6d4df0423 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -4,6 +4,7 @@ import signal import time import unittest +from common.params import Params import selfdrive.manager.manager as manager from selfdrive.manager.process import DaemonProcess from selfdrive.manager.process_config import managed_processes @@ -12,7 +13,7 @@ from system.hardware import HARDWARE os.environ['FAKEUPLOAD'] = "1" MAX_STARTUP_TIME = 3 -ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['updated', 'pandad'])] +ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['pandad', ])] class TestManager(unittest.TestCase): @@ -20,6 +21,10 @@ class TestManager(unittest.TestCase): os.environ['PASSIVE'] = '0' HARDWARE.set_power_save(False) + # ensure clean CarParams + params = Params() + params.clear_all() + def tearDown(self): manager.manager_cleanup() @@ -40,6 +45,7 @@ class TestManager(unittest.TestCase): Ensure all processes exit cleanly when stopped. """ HARDWARE.set_power_save(False) + manager.manager_init() manager.manager_prepare() for p in ALL_PROCESSES: managed_processes[p].start() diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index a108e2c9d..f1a8d7188 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -23,9 +23,9 @@ common_src = [ ] thneed_src = [ - "thneed/thneed.cc", + "thneed/thneed_common.cc", + "thneed/thneed_qcom2.cc", "thneed/serialize.cc", - "thneed/optimizer.cc", "runners/thneedmodel.cc", ] @@ -61,25 +61,54 @@ else: common_model = lenv.Object(common_src) -# build thneed model -if use_thneed and arch == "larch64": - fn = File("models/supercombo").abspath - compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs) - cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} --in {fn}.dlc --out {fn}.thneed --binary --optimize" - - lib_paths = ':'.join(Dir(p).abspath for p in lenv["LIBPATH"]) - kernel_path = os.path.join(Dir('.').abspath, "thneed", "kernels") - cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}", 'KERNEL_PATH': kernel_path}) - - kernels = [os.path.join(kernel_path, x) for x in os.listdir(kernel_path) if x.endswith(".cl")] - cenv.Command(fn + ".thneed", [fn + ".dlc", kernels, compiler], cmd) - lenv.Program('_dmonitoringmodeld', [ "dmonitoringmodeld.cc", "models/dmonitoring.cc", ]+common_model, LIBS=libs) -lenv.Program('_modeld', [ +# build thneed model +if use_thneed and arch == "larch64" or GetOption('pc_thneed'): + fn = File("models/supercombo").abspath + + if GetOption('pc_thneed'): + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && GPU=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" + else: + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 MATMUL=1 PYOPENCL_NO_CACHE=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" + + # is there a better way then listing all of tinygrad? + lenv.Command(fn + ".thneed", [fn + ".onnx", + "#tinygrad_repo/openpilot/compile.py", + "#tinygrad_repo/accel/opencl/conv.cl", + "#tinygrad_repo/accel/opencl/matmul.cl", + "#tinygrad_repo/accel/opencl/ops_opencl.py", + "#tinygrad_repo/accel/opencl/preprocessing.py", + "#tinygrad_repo/extra/onnx.py", + "#tinygrad_repo/extra/thneed.py", + "#tinygrad_repo/extra/utils.py", + "#tinygrad_repo/tinygrad/llops/ops_gpu.py", + "#tinygrad_repo/tinygrad/llops/ops_opencl.py", + "#tinygrad_repo/tinygrad/helpers.py", + "#tinygrad_repo/tinygrad/mlops.py", + "#tinygrad_repo/tinygrad/ops.py", + "#tinygrad_repo/tinygrad/shapetracker.py", + "#tinygrad_repo/tinygrad/tensor.py", + "#tinygrad_repo/tinygrad/nn/__init__.py" + ], cmd) + +llenv = lenv.Clone() +if GetOption('pc_thneed'): + pc_thneed_src = [ + "thneed/thneed_common.cc", + "thneed/thneed_pc.cc", + "thneed/serialize.cc", + "runners/thneedmodel.cc", + ] + llenv['CFLAGS'].append("-DUSE_THNEED") + llenv['CXXFLAGS'].append("-DUSE_THNEED") + common_model += llenv.Object(pc_thneed_src) + libs += ['dl'] + +llenv.Program('_modeld', [ "modeld.cc", "models/driving.cc", ]+common_model, LIBS=libs + transformations) diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 653661a3a..cfc71a0e2 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -35,7 +35,7 @@ mat3 update_calibration(Eigen::Vector3d device_from_calib_euler, bool wide_camer 0.00000000e+00, 7.31372216e-19, 1.00000000e+00, 2.19780220e-03, 4.11497335e-19, -5.62637363e-01, -6.66298828e-20, 2.19780220e-03, -3.33626374e-01).finished(); - + static const auto view_from_device = (Eigen::Matrix() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, diff --git a/selfdrive/modeld/models/dmonitoring_model_q.dlc b/selfdrive/modeld/models/dmonitoring_model_q.dlc index 5253e359f..485009a99 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_q.dlc and b/selfdrive/modeld/models/dmonitoring_model_q.dlc differ diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9bf7e6218..4015731c4 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -38,14 +38,14 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { #else s->m = std::make_unique("models/supercombo.dlc", #endif - &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true); + &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true, false, context); #ifdef TEMPORAL - s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); + s->m->addRecurrent(&s->feature_buffer[0], TEMPORAL_SIZE); #endif #ifdef DESIRE - s->m->addDesire(s->pulse_desire, DESIRE_LEN); + s->m->addDesire(s->pulse_desire, DESIRE_LEN*(HISTORY_BUFFER_LEN+1)); #endif #ifdef TRAFFIC_CONVENTION @@ -56,18 +56,20 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) { #ifdef DESIRE + std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN); if (desire_in != NULL) { for (int i = 1; i < DESIRE_LEN; i++) { // Model decides when action is completed // so desire input is just a pulse triggered on rising edge if (desire_in[i] - s->prev_desire[i] > .99) { - s->pulse_desire[i] = desire_in[i]; + s->pulse_desire[DESIRE_LEN*HISTORY_BUFFER_LEN+i] = desire_in[i]; } else { - s->pulse_desire[i] = 0.0; + s->pulse_desire[DESIRE_LEN*HISTORY_BUFFER_LEN+i] = 0.0; } s->prev_desire[i] = desire_in[i]; } } +LOGT("Desire enqueued"); #endif int rhd_idx = is_rhd; @@ -92,11 +94,18 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, s->m->execute(); LOGT("Execution finished"); + #ifdef TEMPORAL + std::memmove(&s->feature_buffer[0], &s->feature_buffer[FEATURE_LEN], sizeof(float) * FEATURE_LEN*(HISTORY_BUFFER_LEN-1)); + std::memcpy(&s->feature_buffer[FEATURE_LEN*(HISTORY_BUFFER_LEN-1)], &s->output[OUTPUT_SIZE], sizeof(float) * FEATURE_LEN); + LOGT("Features enqueued"); + #endif + return (ModelOutput*)&s->output; } void model_free(ModelState* s) { delete s->frame; + delete s->wide_frame; } void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) { @@ -202,6 +211,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic std::array pos_x_std, pos_y_std, pos_z_std; std::array vel_x, vel_y, vel_z; std::array rot_x, rot_y, rot_z; + std::array acc_x, acc_y, acc_z; std::array rot_rate_x, rot_rate_y, rot_rate_z; for(int i=0; i prediction; - float prob; - - constexpr const ModelOutputStopLinePrediction &get_best_prediction(int t_idx) const { - int max_idx = 0; - for (int i = 1; i < prediction.size(); i++) { - if (prediction[i].prob > prediction[max_idx].prob) { - max_idx = i; - } - } - return prediction[max_idx]; - } -}; -static_assert(sizeof(ModelOutputStopLines) == (sizeof(ModelOutputStopLinePrediction)*STOP_LINE_MHP_N) + sizeof(float)); struct ModelOutputPose { ModelOutputXYZ velocity_mean; @@ -188,6 +161,20 @@ struct ModelOutputPose { }; static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4); +struct ModelOutputWideFromDeviceEuler { + ModelOutputXYZ mean; + ModelOutputXYZ std; +}; +static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2); + +struct ModelOutputTemporalPose { + ModelOutputXYZ velocity_mean; + ModelOutputXYZ rotation_mean; + ModelOutputXYZ velocity_std; + ModelOutputXYZ rotation_std; +}; +static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4); + struct ModelOutputDisengageProb { float gas_disengage; float brake_disengage; @@ -233,33 +220,41 @@ struct ModelOutputMeta { }; static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN)); +struct ModelOutputFeatures { + std::array feature; +}; +static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN)); + struct ModelOutput { const ModelOutputPlans plans; const ModelOutputLaneLines lane_lines; const ModelOutputRoadEdges road_edges; const ModelOutputLeads leads; - const ModelOutputStopLines stop_lines; const ModelOutputMeta meta; const ModelOutputPose pose; + const ModelOutputWideFromDeviceEuler wide_from_device_euler; + const ModelOutputTemporalPose temporal_pose; }; constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); + #ifdef TEMPORAL - constexpr int TEMPORAL_SIZE = 512; + constexpr int TEMPORAL_SIZE = HISTORY_BUFFER_LEN * FEATURE_LEN; #else constexpr int TEMPORAL_SIZE = 0; #endif -constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE; +constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE; // TODO: convert remaining arrays to std::array and update model runners struct ModelState { - ModelFrame *frame; - ModelFrame *wide_frame; + ModelFrame *frame = nullptr; + ModelFrame *wide_frame = nullptr; + std::array feature_buffer = {}; std::array output = {}; std::unique_ptr m; #ifdef DESIRE float prev_desire[DESIRE_LEN] = {}; - float pulse_desire[DESIRE_LEN] = {}; + float pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN+1)] = {}; #endif #ifdef TRAFFIC_CONVENTION float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; diff --git a/selfdrive/modeld/models/supercombo.dlc b/selfdrive/modeld/models/supercombo.dlc deleted file mode 100644 index 603a8b87b..000000000 Binary files a/selfdrive/modeld/models/supercombo.dlc and /dev/null differ diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx new file mode 100644 index 000000000..e16a69999 Binary files /dev/null and b/selfdrive/modeld/models/supercombo.onnx differ diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py index ac7cc6881..d4a11a7c0 100755 --- a/selfdrive/modeld/runners/onnx_runner.py +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -9,6 +9,8 @@ os.environ["OMP_WAIT_POLICY"] = "PASSIVE" import onnxruntime as ort # pylint: disable=import-error +ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8} + def read(sz, tf8=False): dd = [] gt = 0 @@ -18,7 +20,7 @@ def read(sz, tf8=False): assert(len(st) > 0) dd.append(st) gt += len(st) - r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32) + r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32) if tf8: r = r / 255. return r @@ -29,22 +31,23 @@ def write(d): def run_loop(m, tf8_input=False): ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()] keys = [x.name for x in m.get_inputs()] + itypes = [ORT_TYPES_TO_NP_TYPES[x.type] for x in m.get_inputs()] # run once to initialize CUDA provider if "CUDAExecutionProvider" in m.get_providers(): - m.run(None, dict(zip(keys, [np.zeros(shp, dtype=np.float32) for shp in ishapes]))) + m.run(None, dict(zip(keys, [np.zeros(shp, dtype=itp) for shp, itp in zip(ishapes, itypes)]))) print("ready to run onnx model", keys, ishapes, file=sys.stderr) while 1: inputs = [] - for k, shp in zip(keys, ishapes): + for k, shp, itp in zip(keys, ishapes, itypes): ts = np.product(shp) #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr) - inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp)) + inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp).astype(itp)) ret = m.run(None, dict(zip(keys, inputs))) #print(ret, file=sys.stderr) for r in ret: - write(r) + write(r.astype(np.float32)) if __name__ == "__main__": diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc index 1f9f551ab..447d90fd7 100644 --- a/selfdrive/modeld/runners/onnxmodel.cc +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -14,7 +14,7 @@ #include "common/swaglog.h" #include "common/util.h" -ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra, bool _use_tf8) { +ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra, bool _use_tf8, cl_context context) { LOGD("loading model %s", path); output = _output; diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h index 4ac599e2a..d5b7bfecf 100644 --- a/selfdrive/modeld/runners/onnxmodel.h +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -6,7 +6,7 @@ class ONNXModel : public RunModel { public: - ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false, bool _use_tf8 = false); + ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false, bool _use_tf8 = false, cl_context context = NULL); ~ONNXModel(); void addRecurrent(float *state, int state_size); void addDesire(float *state, int state_size); diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index 02b8c4b20..c60781140 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -1,4 +1,5 @@ #pragma once +#include "common/clutil.h" class RunModel { public: virtual ~RunModel() {} diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 4d6917e89..ff4adcd8d 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -14,7 +14,7 @@ void PrintErrorStringAndExit() { std::exit(EXIT_FAILURE); } -SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8) { +SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8, cl_context context) { output = loutput; output_size = loutput_size; use_extra = luse_extra; @@ -186,75 +186,14 @@ std::unique_ptr SNPEModel::addExtra(float *state, in } void SNPEModel::execute() { -#ifdef USE_THNEED - if (Runtime == zdl::DlSystem::Runtime_t::GPU) { - if (!thneed_recorded) { - bool ret = inputBuffer->setBufferAddress(input); - assert(ret == true); - if (use_extra) { - assert(extra != NULL); - bool extra_ret = extraBuffer->setBufferAddress(extra); - assert(extra_ret == true); - } - if (!snpe->execute(inputMap, outputMap)) { - PrintErrorStringAndExit(); - } - memset(recurrent, 0, recurrent_size*sizeof(float)); - thneed->record = true; - if (!snpe->execute(inputMap, outputMap)) { - PrintErrorStringAndExit(); - } - thneed->stop(); - printf("thneed cached\n"); - - // doing self test - float *outputs_golden = (float *)malloc(output_size*sizeof(float)); - memcpy(outputs_golden, output, output_size*sizeof(float)); - memset(output, 0, output_size*sizeof(float)); - memset(recurrent, 0, recurrent_size*sizeof(float)); - uint64_t start_time = nanos_since_boot(); - if (extra != NULL) { - float *inputs[5] = {recurrent, trafficConvention, desire, extra, input}; - thneed->execute(inputs, output); - } else { - float *inputs[4] = {recurrent, trafficConvention, desire, input}; - thneed->execute(inputs, output); - } - uint64_t elapsed_time = nanos_since_boot() - start_time; - printf("ran model in %.2f ms\n", float(elapsed_time)/1e6); - - if (memcmp(output, outputs_golden, output_size*sizeof(float)) == 0) { - printf("thneed selftest passed\n"); - } else { - for (int i = 0; i < output_size; i++) { - printf("mismatch %3d: %f %f\n", i, output[i], outputs_golden[i]); - } - assert(false); - } - free(outputs_golden); - thneed_recorded = true; - } else { - if (use_extra) { - float *inputs[5] = {recurrent, trafficConvention, desire, extra, input}; - thneed->execute(inputs, output); - } else { - float *inputs[4] = {recurrent, trafficConvention, desire, input}; - thneed->execute(inputs, output); - } - } - } else { -#endif - bool ret = inputBuffer->setBufferAddress(input); - assert(ret == true); - if (use_extra) { - bool extra_ret = extraBuffer->setBufferAddress(extra); - assert(extra_ret == true); - } - if (!snpe->execute(inputMap, outputMap)) { - PrintErrorStringAndExit(); - } -#ifdef USE_THNEED + bool ret = inputBuffer->setBufferAddress(input); + assert(ret == true); + if (use_extra) { + bool extra_ret = extraBuffer->setBufferAddress(extra); + assert(extra_ret == true); + } + if (!snpe->execute(inputMap, outputMap)) { + PrintErrorStringAndExit(); } -#endif } diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index ed9d58d1e..08ae16c2b 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -23,7 +23,7 @@ class SNPEModel : public RunModel { public: - SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false); + SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false, cl_context context = NULL); void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc index dbe80a946..67db01bb9 100644 --- a/selfdrive/modeld/runners/thneedmodel.cc +++ b/selfdrive/modeld/runners/thneedmodel.cc @@ -2,11 +2,10 @@ #include -ThneedModel::ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra) { - thneed = new Thneed(true); +ThneedModel::ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8, cl_context context) { + thneed = new Thneed(true, context); thneed->load(path); thneed->clexec(); - thneed->find_inputs_outputs(); recorded = false; output = loutput; diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h index fe0f9ae44..f3f34dc7f 100644 --- a/selfdrive/modeld/runners/thneedmodel.h +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -5,7 +5,7 @@ class ThneedModel : public RunModel { public: - ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false); + ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false, cl_context context = NULL); void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc deleted file mode 100644 index f76c63b2b..000000000 --- a/selfdrive/modeld/thneed/compile.cc +++ /dev/null @@ -1,81 +0,0 @@ -#include -#include - -#include "selfdrive/modeld/runners/snpemodel.h" -#include "selfdrive/modeld/thneed/thneed.h" -#include "system/hardware/hw.h" - -#define TEMPORAL_SIZE 512 -#define DESIRE_LEN 8 -#define TRAFFIC_CONVENTION_LEN 2 - -// TODO: This should probably use SNPE directly. -int main(int argc, char* argv[]) { - bool run_optimizer = false, save_binaries = false; - const char *input_file = NULL, *output_file = NULL; - static struct option long_options[] = { - {"in", required_argument, 0, 'i' }, - {"out", required_argument, 0, 'o' }, - {"binary", no_argument, 0, 'b' }, - {"optimize", no_argument, 0, 'f' }, - {0, 0, 0, 0 } - }; - int long_index = 0, opt = 0; - while ((opt = getopt_long_only(argc, argv,"", long_options, &long_index)) != -1) { - switch (opt) { - case 'i': input_file = optarg; break; - case 'o': output_file = optarg; break; - case 'b': save_binaries = true; break; - case 'f': run_optimizer = true; break; - } - } - - // no input? - if (!input_file) { - printf("usage: -i -o --binary --optimize\n"); - return -1; - } - - #define OUTPUT_SIZE 0x10000 - - float *output = (float*)calloc(OUTPUT_SIZE, sizeof(float)); - SNPEModel mdl(input_file, output, 0, USE_GPU_RUNTIME, true); - mdl.thneed->run_optimizer = run_optimizer; - - float state[TEMPORAL_SIZE] = {0}; - float desire[DESIRE_LEN] = {0}; - float traffic_convention[TRAFFIC_CONVENTION_LEN] = {0}; - float *input = (float*)calloc(0x1000000, sizeof(float)); - float *extra = (float*)calloc(0x1000000, sizeof(float)); - - mdl.addRecurrent(state, TEMPORAL_SIZE); - mdl.addDesire(desire, DESIRE_LEN); - mdl.addTrafficConvention(traffic_convention, TRAFFIC_CONVENTION_LEN); - mdl.addImage(input, 0); - mdl.addExtra(extra, 0); - - // first run - printf("************** execute 1 **************\n"); - memset(output, 0, OUTPUT_SIZE * sizeof(float)); - mdl.execute(); - - // don't save? - if (!output_file) { - printf("no output file, exiting\n"); - return 0; - } - - // save model - printf("saving %s with binary %d\n", output_file, save_binaries); - mdl.thneed->save(output_file, save_binaries); - - // test model - auto thneed = new Thneed(true); - thneed->record = false; - thneed->load(output_file); - thneed->clexec(); - thneed->find_inputs_outputs(); - - return 0; -} - diff --git a/selfdrive/modeld/thneed/kernels/convolution_.cl b/selfdrive/modeld/thneed/kernels/convolution_.cl deleted file mode 100644 index 1b9d74b83..000000000 --- a/selfdrive/modeld/thneed/kernels/convolution_.cl +++ /dev/null @@ -1,272 +0,0 @@ - read_only image2d_t input, -#ifndef DEPTHWISE - short startPackedInputChannel, - short numPackedInputChannelsForGroup, short totalNumPackedInputChannels, - // typo required for API compatibility - short packedOuputChannelOffset, short totalNumPackedOutputChannels, -#else - short totalNumPackedChannels, -#endif - read_only image2d_t weights, __constant float *biases, - short filterSizeX, short filterSizeY, - write_only image2d_t output, - short paddingX, short paddingY, short strideX, short strideY, -#ifdef SUPPORT_DILATION - short dilationX, short dilationY, -#endif - short neuron, float a, float b, float min_clamp, float max_clamp, -#ifndef DEPTHWISE - // note: these are not supported - __constant float *parameters, __constant float *batchNormBiases, -#endif - short numOutputColumns -#ifdef SUPPORT_ACCUMULATION - , short doAccumulate, read_only image2d_t accumulator -#endif - ) { - -#ifndef NUM_OUTPUTS - #define NUM_OUTPUTS 4 -#endif - - // init - const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; - short packedOutputChannel = get_global_id(0); - short startOutputColumn = mul24((short)get_global_id(1), NUM_OUTPUTS); - short outputRow = get_global_id(2); - -#ifdef DEPTHWISE - short totalNumPackedInputChannels = totalNumPackedChannels; - short totalNumPackedOutputChannels = totalNumPackedChannels; - short startPackedInputChannel = packedOutputChannel; -#endif - - short startX = mad24(mad24(startOutputColumn, strideX, -paddingX), totalNumPackedInputChannels, startPackedInputChannel); - short strideWithChannels = mul24(strideX, totalNumPackedInputChannels); - - float4 outputValues[NUM_OUTPUTS]; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] = (float4)(0, 0, 0, 0); - } - - int2 inputLocation; - inputLocation.y = mad24(outputRow, strideY, -paddingY); - - int2 weightLocation; - weightLocation.x = 0; - weightLocation.y = packedOutputChannel; - -#ifdef DEPTHWISE - -#ifdef SUPPORT_DILATION - - // depthwise convolution - for (short rfRow = 0; rfRow < filterSizeY; ++rfRow) { - for (short rfColumn = 0; rfColumn < filterSizeX; ++rfColumn) { - short dilatedStepX = mul24(totalNumPackedChannels, dilationX); - inputLocation.x = mad24(rfColumn, dilatedStepX, startX); - float4 inputValues[4]; - for (short i = 0; i < 4; ++i) { - inputValues[i] = read_imagef(input, smp, inputLocation); - inputLocation.x += strideWithChannels; - } - float4 weightValues = read_imagef(weights, smp, weightLocation); - ++weightLocation.x; - outputValues[0] += inputValues[0] * weightValues; - outputValues[1] += inputValues[1] * weightValues; - outputValues[2] += inputValues[2] * weightValues; - outputValues[3] += inputValues[3] * weightValues; - } - inputLocation.y += dilationY; - } - -#else - - // depthwise unstrided convolution - 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, inputLocation); - 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, inputLocation); - inputLocation.x += totalNumPackedChannels; - float4 weightValues = read_imagef(weights, smp, weightLocation); - ++weightLocation.x; - outputValues[0] += inputValues[0] * weightValues; - outputValues[1] += inputValues[1] * weightValues; - outputValues[2] += inputValues[2] * weightValues; - outputValues[3] += inputValues[3] * weightValues; - } - ++inputLocation.y; - } - -#endif - -#elif defined(ONLY_1X1_CONV) - - // 1x1 convolution - short endPackedInputChannel = startPackedInputChannel + numPackedInputChannelsForGroup; - for (short packedInputChannel = startPackedInputChannel; packedInputChannel < endPackedInputChannel; ++packedInputChannel) { - float4 weightValues[4]; - for (short outChIdx = 0; outChIdx < 4; ++outChIdx) { - weightValues[outChIdx] = read_imagef(weights, smp, weightLocation); - ++weightLocation.x; - } - - inputLocation.x = startX + packedInputChannel; - float4 inputValues[NUM_OUTPUTS]; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - inputValues[i] = read_imagef(input, smp, inputLocation); - inputLocation.x += strideWithChannels; - } - - for (short i = 0; i < NUM_OUTPUTS; ++i) { - 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; - } - } - packedOutputChannel += packedOuputChannelOffset; - -#else - - // normal convolution - for (short rfRow = 0; rfRow < filterSizeY; ++rfRow) { - for (short packedInputChannel = 0; packedInputChannel < numPackedInputChannelsForGroup; ++packedInputChannel) { - short startXForChannel = startX + packedInputChannel; - for (short rfColumn = 0; rfColumn < filterSizeX; ++rfColumn) { - - float4 weightValues[4]; - for (short outChIdx = 0; outChIdx < 4; ++outChIdx) { - weightValues[outChIdx] = read_imagef(weights, smp, weightLocation); - ++weightLocation.x; - } - -#ifdef SUPPORT_DILATION - short dilatedStepX = mul24(totalNumPackedInputChannels, dilationX); - inputLocation.x = mad24(rfColumn, dilatedStepX, startXForChannel); -#else - inputLocation.x = mad24(rfColumn, totalNumPackedInputChannels, startXForChannel); -#endif - float4 inputValues[NUM_OUTPUTS]; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - inputValues[i] = read_imagef(input, smp, inputLocation); - inputLocation.x += strideWithChannels; - } - for (short i = 0; i < NUM_OUTPUTS; ++i) { - 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; - } - } - } -#ifdef SUPPORT_DILATION - inputLocation.y += dilationY; -#else - ++inputLocation.y; -#endif - } - packedOutputChannel += packedOuputChannelOffset; -#endif - - // bias - short outputChannel = mul24(packedOutputChannel, 4); - float4 biasValues = vload4(0, biases + outputChannel); - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] += biasValues; - } - -#ifdef SUPPORT_ACCUMULATION - // accumulate - if (doAccumulate) { - int2 outputLocation; - short outputColumn = startOutputColumn; - outputLocation.y = outputRow; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputLocation.x = mad24(outputColumn, totalNumPackedOutputChannels, packedOutputChannel); - if (outputColumn < numOutputColumns) { - outputValues[i] += read_imagef(accumulator, smp, outputLocation); - } - ++outputColumn; - } - } -#endif - - // activation - switch (neuron) { - case 1: - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] = max(outputValues[i], 0.0f); - } - break; - case 2: - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] = a * tanh(b * outputValues[i]); - } - break; - case 3: - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] = native_recip(1.0f + native_exp(-a * outputValues[i] + b)); - } - break; - case 4: - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] = max(outputValues[i], min_clamp); - outputValues[i] = min(outputValues[i], max_clamp); - } - break; - case 5: - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputValues[i] = max(outputValues[i], 0.0f) + a * (native_exp(min(outputValues[i], 0.0f)) - 1.0f); - } - break; - } - - // output - int2 outputLocation; - short outputColumn = startOutputColumn; - outputLocation.y = outputRow; - for (short i = 0; i < NUM_OUTPUTS; ++i) { - outputLocation.x = mad24(outputColumn, totalNumPackedOutputChannels, packedOutputChannel); - if (outputColumn < numOutputColumns) { - write_imagef(output, outputLocation, outputValues[i]); - } - ++outputColumn; - } -} diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl deleted file mode 100644 index fcea88ce9..000000000 --- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl +++ /dev/null @@ -1,3 +0,0 @@ -#define SUPPORT_DILATION - -__kernel void convolution_horizontal_reduced_reads( diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl deleted file mode 100644 index 0d15d8058..000000000 --- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl +++ /dev/null @@ -1,4 +0,0 @@ -#define ONLY_1X1_CONV -#define SUPPORT_ACCUMULATION - -__kernel void convolution_horizontal_reduced_reads_1x1( diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl deleted file mode 100644 index 69421fc2a..000000000 --- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl +++ /dev/null @@ -1,3 +0,0 @@ -#define NUM_OUTPUTS 5 - -__kernel void convolution_horizontal_reduced_reads_5_outputs( diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl deleted file mode 100644 index 50e39941d..000000000 --- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl +++ /dev/null @@ -1,4 +0,0 @@ -#define DEPTHWISE -#define SUPPORT_DILATION - -__kernel void convolution_horizontal_reduced_reads_depthwise( diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl deleted file mode 100644 index b347cb6c7..000000000 --- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl +++ /dev/null @@ -1,3 +0,0 @@ -#define DEPTHWISE - -__kernel void convolution_horizontal_reduced_reads_depthwise_stride_1( diff --git a/selfdrive/modeld/thneed/optimizer.cc b/selfdrive/modeld/thneed/optimizer.cc deleted file mode 100644 index 39737d3d7..000000000 --- a/selfdrive/modeld/thneed/optimizer.cc +++ /dev/null @@ -1,261 +0,0 @@ -#include -#include -#include -#include -#include "thneed.h" - -#include "common/util.h" -#include "common/clutil.h" - -extern map g_program_source; - -/*static int is_same_size_image(cl_mem a, cl_mem b) { - size_t a_width, a_height, a_depth, a_array_size, a_row_pitch, a_slice_pitch; - clGetImageInfo(a, CL_IMAGE_WIDTH, sizeof(a_width), &a_width, NULL); - clGetImageInfo(a, CL_IMAGE_HEIGHT, sizeof(a_height), &a_height, NULL); - clGetImageInfo(a, CL_IMAGE_DEPTH, sizeof(a_depth), &a_depth, NULL); - clGetImageInfo(a, CL_IMAGE_ARRAY_SIZE, sizeof(a_array_size), &a_array_size, NULL); - clGetImageInfo(a, CL_IMAGE_ROW_PITCH, sizeof(a_row_pitch), &a_row_pitch, NULL); - clGetImageInfo(a, CL_IMAGE_SLICE_PITCH, sizeof(a_slice_pitch), &a_slice_pitch, NULL); - - size_t b_width, b_height, b_depth, b_array_size, b_row_pitch, b_slice_pitch; - clGetImageInfo(b, CL_IMAGE_WIDTH, sizeof(b_width), &b_width, NULL); - clGetImageInfo(b, CL_IMAGE_HEIGHT, sizeof(b_height), &b_height, NULL); - clGetImageInfo(b, CL_IMAGE_DEPTH, sizeof(b_depth), &b_depth, NULL); - clGetImageInfo(b, CL_IMAGE_ARRAY_SIZE, sizeof(b_array_size), &b_array_size, NULL); - clGetImageInfo(b, CL_IMAGE_ROW_PITCH, sizeof(b_row_pitch), &b_row_pitch, NULL); - clGetImageInfo(b, CL_IMAGE_SLICE_PITCH, sizeof(b_slice_pitch), &b_slice_pitch, NULL); - - return (a_width == b_width) && (a_height == b_height) && - (a_depth == b_depth) && (a_array_size == b_array_size) && - (a_row_pitch == b_row_pitch) && (a_slice_pitch == b_slice_pitch); -}*/ - -static cl_mem make_image_like(cl_context context, cl_mem val) { - cl_image_format format; - size_t width, height, row_pitch; - clGetImageInfo(val, CL_IMAGE_FORMAT, sizeof(format), &format, NULL); - assert(format.image_channel_order == CL_RGBA); - assert(format.image_channel_data_type == CL_HALF_FLOAT); - clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL); - clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL); - clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL); - - cl_image_desc desc = {0}; - desc.image_type = CL_MEM_OBJECT_IMAGE2D; - desc.image_width = width; - desc.image_height = height; - desc.image_row_pitch = row_pitch; - - cl_mem buf = clCreateBuffer(context, CL_MEM_READ_WRITE, row_pitch*height, NULL, NULL); - assert(buf != NULL); - desc.buffer = buf; - - cl_int err; - cl_mem tmp = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &err); - //printf("got %d for image %zux%zu %zu\n", err, width, height, row_pitch); - assert(tmp != NULL); - - return tmp; -} - -// convolution_horizontal_reduced_reads_1x1 is 66% of the model runtime -// make that faster and the model gets faster - -// this cuts ~2 ms off the model runtime right now -int Thneed::optimize() { - const char *kernel_path = getenv("KERNEL_PATH"); - if (!kernel_path) { kernel_path = "/data/openpilot/selfdrive/modeld/thneed/kernels"; printf("no KERNEL_PATH set, defaulting to %s\n", kernel_path); } - - string convolution_; - { - char fn[0x100]; - snprintf(fn, sizeof(fn), "%s/%s.cl", kernel_path, "convolution_"); - convolution_ = util::read_file(fn); - } - - // load custom kernels - map g_programs; - for (auto &k : kq) { - // replace program? - if (g_programs.find(k->name) == g_programs.end()) { - char fn[0x100]; - snprintf(fn, sizeof(fn), "%s/%s.cl", kernel_path, k->name.c_str()); - if (util::file_exists(fn)) { - string kernel_src = util::read_file(fn); - if (k->name.rfind("convolution_", 0) == 0) { - kernel_src += convolution_; - } - printf("building kernel %s with len %lu\n", k->name.c_str(), kernel_src.length()); - k->program = cl_program_from_source(context, device_id, kernel_src); - - // save in cache - g_programs[k->name] = k->program; - g_program_source[k->program] = kernel_src; - } else { - g_programs[k->name] = NULL; - } - } else { - // cached replacement - if (g_programs[k->name] != NULL) { - k->program = g_programs[k->name]; - } - } - - // hack in accumulator to convolution_horizontal_reduced_reads_1x1 - if (k->name == "convolution_horizontal_reduced_reads_1x1") { - k->arg_names.push_back("doAccumulate"); - short doAccumulate = 0; - k->args.push_back(string((char *)&doAccumulate, sizeof(doAccumulate))); - k->args_size.push_back(2); - k->arg_names.push_back("accumulator"); - k->args.push_back(k->args[k->get_arg_num("output")]); - k->args_size.push_back(8); - k->num_args += 2; - } - - // assert that parameters + batchNormBiases are not used - // since they aren't supported in custom replacement kernels - if (k->name == "convolution_horizontal_reduced_reads_1x1" || - k->name == "convolution_horizontal_reduced_reads" || - k->name == "convolution_horizontal_reduced_reads_5_outputs") { - string p1 = k->args[k->get_arg_num("parameters")]; - string p2 = k->args[k->get_arg_num("batchNormBiases")]; - assert(p1.length() == 8 && *((uint64_t*)p1.data()) == 0); - assert(p2.length() == 8 && *((uint64_t*)p2.data()) == 0); - } - } - - // optimizer - size_t start_size; - do { - start_size = kq.size(); - - // get optimizations - map replacements; - for (int i = 0; i < kq.size(); i++) { - // fusing elementwise_sum + activate_image will save 3 enqueues - - // delete useless copy layers - // saves ~0.7 ms - /*if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") { - string in = kq[i]->args[kq[i]->get_arg_num("input")]; - string out = kq[i]->args[kq[i]->get_arg_num("output")]; - if (is_same_size_image(*(cl_mem*)in.data(), *(cl_mem*)out.data())) { - cl_mem tmp = make_image_like(context, *(cl_mem *)in.data()); - replacements[in] = string((char *)&tmp, sizeof(tmp)); - replacements[out] = string((char *)&tmp, sizeof(tmp)); - - kq.erase(kq.begin()+i); --i; - } - }*/ - - // NOTE: if activations/accumulation are done in the wrong order, this will be wrong - - // fuse activations into convs and fc_Wtx - // saves ~1.5 ms - // NOTE: this changes the outputs because of rounding, should be better now! - if (i != 0 && kq[i]->name == "activate_image") { - if (kq[i-1]->name == "convolution_horizontal_reduced_reads_1x1" || - kq[i-1]->name == "convolution_horizontal_reduced_reads_5_outputs" || - kq[i-1]->name == "convolution_horizontal_reduced_reads" || - kq[i-1]->name == "convolution_horizontal_reduced_reads_depthwise" || - kq[i-1]->name == "convolution_horizontal_reduced_reads_depthwise_stride_1" || - kq[i-1]->name == "fc_Wtx") { - string lastout = kq[i-1]->args[kq[i-1]->get_arg_num("output")]; - string in = kq[i]->args[kq[i]->get_arg_num("input")]; - string out = kq[i]->args[kq[i]->get_arg_num("output")]; - - if (lastout == in) { - short neuron = *(int*)kq[i]->args[kq[i]->get_arg_num("neuron")].data(); - assert(neuron <= 5); - - // ELU isn't supported in fc_Wtx - assert(!(kq[i-1]->name == "fc_Wtx" && neuron == 5)); - - kq[i-1]->args[kq[i-1]->get_arg_num("neuron")] = string((char *)&neuron, sizeof(neuron)); - - cl_mem tmp = make_image_like(context, *(cl_mem *)lastout.data()); - replacements[in] = string((char *)&tmp, sizeof(tmp)); - replacements[out] = string((char *)&tmp, sizeof(tmp)); - - kq.erase(kq.begin()+i); --i; - } - } - } - - // fuse accumulation into convs and fc_Wtx - if (i != 0 && kq[i]->name == "elementwise_sum") { - if (kq[i-1]->name == "convolution_horizontal_reduced_reads_1x1" || - kq[i-1]->name == "fc_Wtx") { - string lastout = kq[i-1]->args[kq[i-1]->get_arg_num("output")]; - string a = kq[i]->args[kq[i]->get_arg_num("a")]; - string b = kq[i]->args[kq[i]->get_arg_num("b")]; - string out = kq[i]->args[kq[i]->get_arg_num("output")]; - - if (lastout == a) { - kq[i-1]->args[kq[i-1]->get_arg_num("accumulator")] = b; - } else if (lastout == b) { - kq[i-1]->args[kq[i-1]->get_arg_num("accumulator")] = a; - } else { - continue; - } - - cl_mem tmp = make_image_like(context, *(cl_mem *)lastout.data()); - replacements[lastout] = string((char *)&tmp, sizeof(tmp)); - replacements[out] = string((char *)&tmp, sizeof(tmp)); - - short doAccumulate = 1; - kq[i-1]->args[kq[i-1]->get_arg_num("doAccumulate")] = string((char *)&doAccumulate, sizeof(doAccumulate)); - - kq.erase(kq.begin()+i); --i; - } - } - } - - // remap inputs and outputs, and clear the kernels - for (int i = 0; i < kq.size(); i++) { - kq[i]->kernel = NULL; - for (int j = 0; j < kq[i]->num_args; j++) { - if (replacements.find(kq[i]->args[j]) != replacements.end()) { - kq[i]->args[j] = replacements[kq[i]->args[j]]; - } - } - } - - printf("optimize %lu -> %lu\n", start_size, kq.size()); - } while (kq.size() != start_size); - - size_t work_group_size = 0; - clGetDeviceInfo(device_id, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(work_group_size), &work_group_size, NULL); - printf("max work group size %lu\n", work_group_size); - - // local work group optimizer - for (auto &k : kq) { - // only do it for convs, since others might share memory - if (k->name.rfind("convolution_", 0) == 0) { - int best = -1; - if (k->local_work_size[0] * k->local_work_size[1] * k->local_work_size[2] < work_group_size/2) { - uint64_t base_time = k->benchmark(); - uint64_t best_time = base_time; - for (int i = 0; i < 3; i++) { - k->local_work_size[i] *= 2; - uint64_t this_time = k->benchmark(); - if (this_time < best_time) { - best = i; - best_time = this_time; - } - k->local_work_size[i] /= 2; - } - if (best != -1) { - k->local_work_size[best] *= 2; - //printf("%s %.2f ms doubled %d to %.2f ms\n", k->name.c_str(), base_time/1e6, best, best_time/1e6); - } - } - - } - } - - return 0; -} - diff --git a/selfdrive/modeld/thneed/serialize.cc b/selfdrive/modeld/thneed/serialize.cc index 136576fe1..f789e5bf5 100644 --- a/selfdrive/modeld/thneed/serialize.cc +++ b/selfdrive/modeld/thneed/serialize.cc @@ -14,9 +14,9 @@ void Thneed::load(const char *filename) { string buf = util::read_file(filename); int jsz = *(int *)buf.data(); - string err; + string jsonerr; string jj(buf.data() + sizeof(int), jsz); - Json jdat = Json::parse(jj, err); + Json jdat = Json::parse(jj, jsonerr); map real_mem; real_mem[NULL] = NULL; @@ -33,11 +33,14 @@ void Thneed::load(const char *filename) { assert(mobj["needs_load"].bool_value() == false); } else { if (mobj["needs_load"].bool_value()) { - //printf("loading %p %d @ 0x%X\n", clbuf, sz, ptr); clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, &buf[ptr], NULL); + if (debug >= 1) printf("loading %p %d @ 0x%X\n", clbuf, sz, ptr); ptr += sz; } else { - clbuf = clCreateBuffer(context, CL_MEM_READ_WRITE, sz, NULL, NULL); + // TODO: is there a faster way to init zeroed out buffers? + void *host_zeros = calloc(sz, 1); + clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, host_zeros, NULL); + free(host_zeros); } } assert(clbuf != NULL); @@ -48,13 +51,33 @@ void Thneed::load(const char *filename) { desc.image_width = mobj["width"].int_value(); desc.image_height = mobj["height"].int_value(); desc.image_row_pitch = mobj["row_pitch"].int_value(); + assert(sz == desc.image_height*desc.image_row_pitch); +#ifdef QCOM2 desc.buffer = clbuf; - - cl_image_format format; +#else + // TODO: we are creating unused buffers on PC + clReleaseMemObject(clbuf); +#endif + cl_image_format format = {0}; format.image_channel_order = CL_RGBA; - format.image_channel_data_type = CL_HALF_FLOAT; + format.image_channel_data_type = mobj["float32"].bool_value() ? CL_FLOAT : CL_HALF_FLOAT; - clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, NULL); + cl_int errcode; + +#ifndef QCOM2 + if (mobj["needs_load"].bool_value()) { + clbuf = clCreateImage(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, &format, &desc, &buf[ptr-sz], &errcode); + } else { + clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode); + } +#else + clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode); +#endif + if (clbuf == NULL) { + printf("clError: %s create image %zux%zu rp %zu with buffer %p\n", cl_get_error_string(errcode), + desc.image_width, desc.image_height, desc.image_row_pitch, desc.buffer + ); + } assert(clbuf != NULL); } @@ -67,6 +90,30 @@ void Thneed::load(const char *filename) { g_programs[name] = cl_program_from_source(context, device_id, source.string_value()); } + for (auto &obj : jdat["inputs"].array_items()) { + auto mobj = obj.object_items(); + int sz = mobj["size"].int_value(); + cl_mem aa = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())]; + input_clmem.push_back(aa); + input_sizes.push_back(sz); + printf("Thneed::load: adding input %s with size %d\n", mobj["name"].string_value().data(), sz); + + cl_int cl_err; + void *ret = clEnqueueMapBuffer(command_queue, aa, CL_TRUE, CL_MAP_WRITE, 0, sz, 0, NULL, NULL, &cl_err); + if (cl_err != CL_SUCCESS) printf("clError: %s map %p %d\n", cl_get_error_string(cl_err), aa, sz); + assert(cl_err == CL_SUCCESS); + inputs.push_back(ret); + } + + for (auto &obj : jdat["outputs"].array_items()) { + auto mobj = obj.object_items(); + int sz = mobj["size"].int_value(); + printf("Thneed::save: adding output with size %d\n", sz); + // TODO: support multiple outputs + output = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())]; + assert(output != NULL); + } + for (auto &obj : jdat["binaries"].array_items()) { string name = obj["name"].string_value(); size_t length = obj["length"].int_value(); @@ -105,154 +152,3 @@ void Thneed::load(const char *filename) { clFinish(command_queue); } - -void Thneed::save(const char *filename, bool save_binaries) { - printf("Thneed::save: saving to %s\n", filename); - - // get kernels - std::vector kernels; - std::set saved_objects; - std::vector objects; - std::map programs; - std::map binaries; - - for (auto &k : kq) { - kernels.push_back(k->to_json()); - - // check args for objects - int i = 0; - for (auto &a : k->args) { - if (a.size() == 8) { - if (saved_objects.find(a) == saved_objects.end()) { - saved_objects.insert(a); - cl_mem val = *(cl_mem*)(a.data()); - if (val != NULL) { - bool needs_load = k->arg_names[i] == "weights" || k->arg_names[i] == "biases"; - - auto jj = Json::object({ - {"id", a}, - {"arg_type", k->arg_types[i]}, - }); - - if (k->arg_types[i] == "image2d_t" || k->arg_types[i] == "image1d_t") { - cl_mem buf; - clGetImageInfo(val, CL_IMAGE_BUFFER, sizeof(buf), &buf, NULL); - string aa = string((char *)&buf, sizeof(buf)); - jj["buffer_id"] = aa; - - size_t width, height, row_pitch; - clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL); - clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL); - clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL); - jj["width"] = (int)width; - jj["height"] = (int)height; - jj["row_pitch"] = (int)row_pitch; - jj["size"] = (int)(height * row_pitch); - jj["needs_load"] = false; - - if (saved_objects.find(aa) == saved_objects.end()) { - saved_objects.insert(aa); - size_t sz; - clGetMemObjectInfo(buf, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - // save the buffer - objects.push_back(Json::object({ - {"id", aa}, - {"arg_type", ""}, - {"needs_load", needs_load}, - {"size", (int)sz} - })); - if (needs_load) assert(sz == height * row_pitch); - } - } else { - size_t sz = 0; - clGetMemObjectInfo(val, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - jj["size"] = (int)sz; - jj["needs_load"] = needs_load; - } - - objects.push_back(jj); - } - } - } - i++; - } - - if (save_binaries) { - int err; - size_t binary_size = 0; - err = clGetProgramInfo(k->program, CL_PROGRAM_BINARY_SIZES, sizeof(binary_size), &binary_size, NULL); - assert(err == 0); - assert(binary_size > 0); - string sv(binary_size, '\x00'); - - uint8_t* bufs[1] = { (uint8_t*)sv.data(), }; - err = clGetProgramInfo(k->program, CL_PROGRAM_BINARIES, sizeof(bufs), &bufs, NULL); - assert(err == 0); - - binaries[k->name] = sv; - } else { - programs[k->name] = g_program_source[k->program]; - } - } - - vector saved_buffers; - for (auto &obj : objects) { - auto mobj = obj.object_items(); - cl_mem val = *(cl_mem*)(mobj["id"].string_value().data()); - int sz = mobj["size"].int_value(); - if (mobj["needs_load"].bool_value()) { - char *buf = (char *)malloc(sz); - if (mobj["arg_type"] == "image2d_t" || mobj["arg_type"] == "image1d_t") { - assert(false); - } else { - // buffers allocated with CL_MEM_HOST_WRITE_ONLY, hence this hack - //hexdump((uint32_t*)val, 0x100); - - // the worst hack in thneed, the flags are at 0x14 - ((uint32_t*)val)[0x14] &= ~CL_MEM_HOST_WRITE_ONLY; - cl_int ret = clEnqueueReadBuffer(command_queue, val, CL_TRUE, 0, sz, buf, 0, NULL, NULL); - assert(ret == CL_SUCCESS); - } - //printf("saving buffer: %d %p %s\n", sz, buf, mobj["arg_type"].string_value().c_str()); - saved_buffers.push_back(string(buf, sz)); - free(buf); - } - } - - std::vector jbinaries; - for (auto &obj : binaries) { - jbinaries.push_back(Json::object({{"name", obj.first}, {"length", (int)obj.second.size()}})); - saved_buffers.push_back(obj.second); - } - - Json jdat = Json::object({ - {"kernels", kernels}, - {"objects", objects}, - {"programs", programs}, - {"binaries", jbinaries}, - }); - - string str = jdat.dump(); - int jsz = str.length(); - - FILE *f = fopen(filename, "wb"); - fwrite(&jsz, 1, sizeof(jsz), f); - fwrite(str.data(), 1, jsz, f); - for (auto &s : saved_buffers) { - fwrite(s.data(), 1, s.length(), f); - } - fclose(f); -} - -Json CLQueuedKernel::to_json() const { - return Json::object { - { "name", name }, - { "work_dim", (int)work_dim }, - { "global_work_size", Json::array { (int)global_work_size[0], (int)global_work_size[1], (int)global_work_size[2] } }, - { "local_work_size", Json::array { (int)local_work_size[0], (int)local_work_size[1], (int)local_work_size[2] } }, - { "num_args", (int)num_args }, - { "args", args }, - { "args_size", args_size }, - }; -} - diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc deleted file mode 100644 index d673c4f56..000000000 --- a/selfdrive/modeld/thneed/thneed.cc +++ /dev/null @@ -1,604 +0,0 @@ -#include "selfdrive/modeld/thneed/thneed.h" - -#include -#include - -#include -#include -#include -#include -#include - -#include "common/clutil.h" -#include "common/timing.h" - -Thneed *g_thneed = NULL; -int g_fd = -1; -map, string> g_args; -map, int> g_args_size; -map g_program_source; - -void hexdump(uint8_t *d, int len) { - assert((len%4) == 0); - printf(" dumping %p len 0x%x\n", d, len); - for (int i = 0; i < len/4; i++) { - if (i != 0 && (i%0x10) == 0) printf("\n"); - printf("%8x ", d[i]); - } - printf("\n"); -} - -// *********** ioctl interceptor *********** - -extern "C" { - -int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL; -#undef ioctl -int ioctl(int filedes, unsigned long request, void *argp) { - request &= 0xFFFFFFFF; // needed on QCOM2 - if (my_ioctl == NULL) my_ioctl = reinterpret_cast(dlsym(RTLD_NEXT, "ioctl")); - Thneed *thneed = g_thneed; - - // save the fd - if (request == IOCTL_KGSL_GPUOBJ_ALLOC) g_fd = filedes; - - // note that this runs always, even without a thneed object - if (request == IOCTL_KGSL_DRAWCTXT_CREATE) { - struct kgsl_drawctxt_create *create = (struct kgsl_drawctxt_create *)argp; - create->flags &= ~KGSL_CONTEXT_PRIORITY_MASK; - create->flags |= 1 << KGSL_CONTEXT_PRIORITY_SHIFT; // priority from 1-15, 1 is max priority - printf("IOCTL_KGSL_DRAWCTXT_CREATE: creating context with flags 0x%x\n", create->flags); - } - - if (thneed != NULL) { - if (request == IOCTL_KGSL_GPU_COMMAND) { - struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp; - if (thneed->record) { - thneed->timestamp = cmd->timestamp; - thneed->context_id = cmd->context_id; - thneed->cmds.push_back(unique_ptr(new CachedCommand(thneed, cmd))); - } - if (thneed->debug >= 1) { - printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx context_id: %u timestamp: %u numcmds: %d numobjs: %d\n", - thneed->cmds.size(), - cmd->flags, - cmd->context_id, cmd->timestamp, cmd->numcmds, cmd->numobjs); - } - } else if (request == IOCTL_KGSL_GPUOBJ_SYNC) { - struct kgsl_gpuobj_sync *cmd = (struct kgsl_gpuobj_sync *)argp; - struct kgsl_gpuobj_sync_obj *objs = (struct kgsl_gpuobj_sync_obj *)(cmd->objs); - - if (thneed->debug >= 2) { - printf("IOCTL_KGSL_GPUOBJ_SYNC count:%d ", cmd->count); - for (int i = 0; i < cmd->count; i++) { - printf(" -- offset:0x%lx len:0x%lx id:%d op:%d ", objs[i].offset, objs[i].length, objs[i].id, objs[i].op); - } - printf("\n"); - } - - if (thneed->record) { - thneed->cmds.push_back(unique_ptr(new - CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count)))); - } - } else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) { - struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp; - if (thneed->debug >= 1) { - printf("IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID: context_id: %d timestamp: %d timeout: %d\n", - cmd->context_id, cmd->timestamp, cmd->timeout); - } - } else if (request == IOCTL_KGSL_SETPROPERTY) { - if (thneed->debug >= 1) { - struct kgsl_device_getproperty *prop = (struct kgsl_device_getproperty *)argp; - printf("IOCTL_KGSL_SETPROPERTY: 0x%x sizebytes:%zu\n", prop->type, prop->sizebytes); - if (thneed->debug >= 2) { - hexdump((uint8_t *)prop->value, prop->sizebytes); - if (prop->type == KGSL_PROP_PWR_CONSTRAINT) { - struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value; - hexdump((uint8_t *)constraint->data, constraint->size); - } - } - } - } else if (request == IOCTL_KGSL_DRAWCTXT_CREATE || request == IOCTL_KGSL_DRAWCTXT_DESTROY) { - // this happens - } else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) { - // this happens - } else { - if (thneed->debug >= 1) { - printf("other ioctl %lx\n", request); - } - } - } - - int ret = my_ioctl(filedes, request, argp); - if (ret != 0) printf("ioctl returned %d with errno %d\n", ret, errno); - return ret; -} - -} - -// *********** GPUMalloc *********** - -GPUMalloc::GPUMalloc(int size, int fd) { - struct kgsl_gpuobj_alloc alloc; - memset(&alloc, 0, sizeof(alloc)); - alloc.size = size; - alloc.flags = 0x10000a00; - ioctl(fd, IOCTL_KGSL_GPUOBJ_ALLOC, &alloc); - void *addr = mmap64(NULL, alloc.mmapsize, 0x3, 0x1, fd, alloc.id*0x1000); - assert(addr != MAP_FAILED); - - base = (uint64_t)addr; - remaining = size; -} - -GPUMalloc::~GPUMalloc() { - // TODO: free the GPU malloced area -} - -void *GPUMalloc::alloc(int size) { - void *ret = (void*)base; - size = (size+0xff) & (~0xFF); - assert(size <= remaining); - remaining -= size; - base += size; - return ret; -} - -// *********** CachedSync, at the ioctl layer *********** - -void CachedSync::exec() { - struct kgsl_gpuobj_sync cmd; - - cmd.objs = (uint64_t)data.data(); - cmd.obj_len = data.length(); - cmd.count = data.length() / sizeof(struct kgsl_gpuobj_sync_obj); - - int ret = ioctl(thneed->fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd); - assert(ret == 0); -} - -// *********** CachedCommand, at the ioctl layer *********** - -CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) { - thneed = lthneed; - assert(cmd->numsyncs == 0); - - memcpy(&cache, cmd, sizeof(cache)); - - if (cmd->numcmds > 0) { - cmds = make_unique(cmd->numcmds); - memcpy(cmds.get(), (void *)cmd->cmdlist, sizeof(struct kgsl_command_object)*cmd->numcmds); - cache.cmdlist = (uint64_t)cmds.get(); - for (int i = 0; i < cmd->numcmds; i++) { - void *nn = thneed->ram->alloc(cmds[i].size); - memcpy(nn, (void*)cmds[i].gpuaddr, cmds[i].size); - cmds[i].gpuaddr = (uint64_t)nn; - } - } - - if (cmd->numobjs > 0) { - objs = make_unique(cmd->numobjs); - memcpy(objs.get(), (void *)cmd->objlist, sizeof(struct kgsl_command_object)*cmd->numobjs); - cache.objlist = (uint64_t)objs.get(); - for (int i = 0; i < cmd->numobjs; i++) { - void *nn = thneed->ram->alloc(objs[i].size); - memset(nn, 0, objs[i].size); - objs[i].gpuaddr = (uint64_t)nn; - } - } - - kq = thneed->ckq; - thneed->ckq.clear(); -} - -void CachedCommand::exec() { - cache.timestamp = ++thneed->timestamp; - int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache); - - if (thneed->debug >= 1) printf("CachedCommand::exec got %d\n", ret); - - if (thneed->debug >= 2) { - for (auto &it : kq) { - it->debug_print(false); - } - } - - assert(ret == 0); -} - -// *********** Thneed *********** - -Thneed::Thneed(bool do_clinit) { - if (do_clinit) clinit(); - assert(g_fd != -1); - fd = g_fd; - ram = make_unique(0x80000, fd); - timestamp = -1; - g_thneed = this; - char *thneed_debug_env = getenv("THNEED_DEBUG"); - debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0; -} - -void Thneed::stop() { - find_inputs_outputs(); - printf("Thneed::stop: recorded %lu commands\n", cmds.size()); - record = false; -} - -void Thneed::find_inputs_outputs() { - cl_int err; - if (inputs.size() > 0) return; - - // save the global inputs/outputs - for (auto &k : kq) { - for (int i = 0; i < k->num_args; i++) { - if (k->name == "zero_pad_image_float" && k->arg_names[i] == "input") { - cl_mem aa = *(cl_mem*)(k->args[i].data()); - input_clmem.push_back(aa); - - size_t sz; - clGetMemObjectInfo(aa, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - input_sizes.push_back(sz); - - void *ret = clEnqueueMapBuffer(command_queue, aa, CL_TRUE, CL_MAP_WRITE, 0, sz, 0, NULL, NULL, &err); - assert(err == CL_SUCCESS); - inputs.push_back(ret); - } - - if (k->name == "image2d_to_buffer_float" && k->arg_names[i] == "output") { - output = *(cl_mem*)(k->args[i].data()); - } - } - } -} - -void Thneed::copy_inputs(float **finputs) { - //cl_int ret; - for (int idx = 0; idx < inputs.size(); ++idx) { - if (debug >= 1) printf("copying %lu -- %p -> %p\n", input_sizes[idx], finputs[idx], inputs[idx]); - if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]); - } -} - -void Thneed::copy_output(float *foutput) { - if (output != NULL) { - size_t sz; - clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - if (debug >= 1) printf("copying %lu for output %p -> %p\n", sz, output, foutput); - clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL); - } else { - printf("CAUTION: model output is NULL, does it have no outputs?\n"); - } -} - -void Thneed::wait() { - struct kgsl_device_waittimestamp_ctxtid wait; - wait.context_id = context_id; - wait.timestamp = timestamp; - wait.timeout = -1; - - uint64_t tb = nanos_since_boot(); - int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait); - uint64_t te = nanos_since_boot(); - - if (debug >= 1) printf("wait %d after %lu us\n", wret, (te-tb)/1000); -} - -void Thneed::execute(float **finputs, float *foutput, bool slow) { - uint64_t tb, te; - if (debug >= 1) tb = nanos_since_boot(); - - // ****** copy inputs - copy_inputs(finputs); - - // ****** set power constraint - int ret; - struct kgsl_device_constraint_pwrlevel pwrlevel; - pwrlevel.level = KGSL_CONSTRAINT_PWR_MAX; - - struct kgsl_device_constraint constraint; - constraint.type = KGSL_CONSTRAINT_PWRLEVEL; - constraint.context_id = context_id; - constraint.data = (void*)&pwrlevel; - constraint.size = sizeof(pwrlevel); - - struct kgsl_device_getproperty prop; - prop.type = KGSL_PROP_PWR_CONSTRAINT; - prop.value = (void*)&constraint; - prop.sizebytes = sizeof(constraint); - ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); - assert(ret == 0); - - // ****** run commands - int i = 0; - for (auto &it : cmds) { - ++i; - if (debug >= 1) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000); - it->exec(); - if ((i == cmds.size()) || slow) wait(); - } - - // ****** copy outputs - copy_output(foutput); - - // ****** unset power constraint - constraint.type = KGSL_CONSTRAINT_NONE; - constraint.data = NULL; - constraint.size = 0; - - ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); - assert(ret == 0); - - if (debug >= 1) { - te = nanos_since_boot(); - printf("model exec in %lu us\n", (te-tb)/1000); - } -} - -void Thneed::clinit() { - device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); - context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); - //cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, CL_QUEUE_PROFILING_ENABLE, 0}; - cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, 0, 0}; - command_queue = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); - printf("Thneed::clinit done\n"); -} - -cl_int Thneed::clexec() { - printf("Thneed::clexec: running %lu queued kernels\n", kq.size()); - for (auto &k : kq) { - if (record) ckq.push_back(k); - cl_int ret = k->exec(); - assert(ret == CL_SUCCESS); - } - return clFinish(command_queue); -} - -// *********** OpenCL interceptor *********** - -cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value) { - g_args_size[make_pair(kernel, arg_index)] = arg_size; - if (arg_value != NULL) { - g_args[make_pair(kernel, arg_index)] = string((char*)arg_value, arg_size); - } else { - g_args[make_pair(kernel, arg_index)] = string(""); - } - cl_int ret = clSetKernelArg(kernel, arg_index, arg_size, arg_value); - return ret; -} - -cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue, - cl_kernel kernel, - cl_uint work_dim, - const size_t *global_work_offset, - const size_t *global_work_size, - const size_t *local_work_size, - cl_uint num_events_in_wait_list, - const cl_event *event_wait_list, - cl_event *event) { - - Thneed *thneed = g_thneed; - - // SNPE doesn't use these - assert(num_events_in_wait_list == 0); - assert(global_work_offset == NULL); - assert(event_wait_list == NULL); - - cl_int ret = 0; - if (thneed != NULL && thneed->record) { - if (thneed->context == NULL) { - thneed->command_queue = command_queue; - clGetKernelInfo(kernel, CL_KERNEL_CONTEXT, sizeof(thneed->context), &thneed->context, NULL); - clGetContextInfo(thneed->context, CL_CONTEXT_DEVICES, sizeof(thneed->device_id), &thneed->device_id, NULL); - } - - // if we are recording, we don't actually enqueue the kernel - thneed->kq.push_back(unique_ptr(new CLQueuedKernel(thneed, kernel, work_dim, global_work_size, local_work_size))); - *event = NULL; - } else { - ret = clEnqueueNDRangeKernel(command_queue, kernel, work_dim, - global_work_offset, global_work_size, local_work_size, - num_events_in_wait_list, event_wait_list, event); - } - - return ret; -} - -cl_int thneed_clFinish(cl_command_queue command_queue) { - Thneed *thneed = g_thneed; - - if (thneed != NULL && thneed->record) { - if (thneed->run_optimizer) thneed->optimize(); - return thneed->clexec(); - } else { - return clFinish(command_queue); - } -} - -cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, const char **strings, const size_t *lengths, cl_int *errcode_ret) { - assert(count == 1); - cl_program ret = clCreateProgramWithSource(context, count, strings, lengths, errcode_ret); - g_program_source[ret] = strings[0]; - return ret; -} - -void *dlsym(void *handle, const char *symbol) { -#ifdef QCOM2 - void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen + DLSYM_OFFSET); -#else - #error "Unsupported platform for thneed" -#endif - if (memcmp("REAL_", symbol, 5) == 0) { - return my_dlsym(handle, symbol+5); - } else if (strcmp("clFinish", symbol) == 0) { - return (void*)thneed_clFinish; - } else if (strcmp("clEnqueueNDRangeKernel", symbol) == 0) { - return (void*)thneed_clEnqueueNDRangeKernel; - } else if (strcmp("clSetKernelArg", symbol) == 0) { - return (void*)thneed_clSetKernelArg; - } else if (strcmp("clCreateProgramWithSource", symbol) == 0) { - return (void*)thneed_clCreateProgramWithSource; - } else { - return my_dlsym(handle, symbol); - } -} - -// *********** CLQueuedKernel *********** - -CLQueuedKernel::CLQueuedKernel(Thneed *lthneed, - cl_kernel _kernel, - cl_uint _work_dim, - const size_t *_global_work_size, - const size_t *_local_work_size) { - thneed = lthneed; - kernel = _kernel; - work_dim = _work_dim; - assert(work_dim <= 3); - for (int i = 0; i < work_dim; i++) { - global_work_size[i] = _global_work_size[i]; - local_work_size[i] = _local_work_size[i]; - } - - char _name[0x100]; - clGetKernelInfo(kernel, CL_KERNEL_FUNCTION_NAME, sizeof(_name), _name, NULL); - name = string(_name); - clGetKernelInfo(kernel, CL_KERNEL_NUM_ARGS, sizeof(num_args), &num_args, NULL); - - // get args - for (int i = 0; i < num_args; i++) { - char arg_name[0x100]; - clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); - arg_names.push_back(string(arg_name)); - clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); - arg_types.push_back(string(arg_name)); - - args.push_back(g_args[make_pair(kernel, i)]); - args_size.push_back(g_args_size[make_pair(kernel, i)]); - } - - // get program - clGetKernelInfo(kernel, CL_KERNEL_PROGRAM, sizeof(program), &program, NULL); -} - -int CLQueuedKernel::get_arg_num(const char *search_arg_name) { - for (int i = 0; i < num_args; i++) { - if (arg_names[i] == search_arg_name) return i; - } - printf("failed to find %s in %s\n", search_arg_name, name.c_str()); - assert(false); -} - -cl_int CLQueuedKernel::exec() { - if (kernel == NULL) { - kernel = clCreateKernel(program, name.c_str(), NULL); - arg_names.clear(); - arg_types.clear(); - - for (int j = 0; j < num_args; j++) { - char arg_name[0x100]; - clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); - arg_names.push_back(string(arg_name)); - clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); - arg_types.push_back(string(arg_name)); - - cl_int ret; - if (args[j].size() != 0) { - assert(args[j].size() == args_size[j]); - ret = thneed_clSetKernelArg(kernel, j, args[j].size(), args[j].data()); - } else { - ret = thneed_clSetKernelArg(kernel, j, args_size[j], NULL); - } - assert(ret == CL_SUCCESS); - } - } - - if (thneed->debug >= 1) { - debug_print(thneed->debug >= 2); - } - - return clEnqueueNDRangeKernel(thneed->command_queue, - kernel, work_dim, NULL, global_work_size, local_work_size, 0, NULL, NULL); -} - -uint64_t CLQueuedKernel::benchmark() { - uint64_t ret = 0; - int old_record = thneed->record; - thneed->record = 0; - clFinish(thneed->command_queue); - // TODO: benchmarking at a lower level will make this more accurate - for (int i = 0; i < 10; i++) { - uint64_t sb = nanos_since_boot(); - exec(); - clFinish(thneed->command_queue); - uint64_t et = nanos_since_boot() - sb; - if (ret == 0 || et < ret) ret = et; - } - thneed->record = old_record; - return ret; -} - -void CLQueuedKernel::debug_print(bool verbose) { - printf("%p %56s -- ", kernel, name.c_str()); - for (int i = 0; i < work_dim; i++) { - printf("%4zu ", global_work_size[i]); - } - printf(" -- "); - for (int i = 0; i < work_dim; i++) { - printf("%4zu ", local_work_size[i]); - } - printf("\n"); - - if (verbose) { - for (int i = 0; i < num_args; i++) { - string arg = args[i]; - printf(" %s %s", arg_types[i].c_str(), arg_names[i].c_str()); - void *arg_value = (void*)arg.data(); - int arg_size = arg.size(); - if (arg_size == 0) { - printf(" (size) %d", args_size[i]); - } else if (arg_size == 1) { - printf(" = %d", *((char*)arg_value)); - } else if (arg_size == 2) { - printf(" = %d", *((short*)arg_value)); - } else if (arg_size == 4) { - if (arg_types[i] == "float") { - printf(" = %f", *((float*)arg_value)); - } else { - printf(" = %d", *((int*)arg_value)); - } - } else if (arg_size == 8) { - cl_mem val = (cl_mem)(*((uintptr_t*)arg_value)); - printf(" = %p", val); - if (val != NULL) { - if (arg_types[i] == "image2d_t" || arg_types[i] == "image1d_t") { - cl_image_format format; - size_t width, height, depth, array_size, row_pitch, slice_pitch; - cl_mem buf; - clGetImageInfo(val, CL_IMAGE_FORMAT, sizeof(format), &format, NULL); - assert(format.image_channel_order == CL_RGBA); - assert(format.image_channel_data_type == CL_HALF_FLOAT); - clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL); - clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL); - clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL); - clGetImageInfo(val, CL_IMAGE_DEPTH, sizeof(depth), &depth, NULL); - clGetImageInfo(val, CL_IMAGE_ARRAY_SIZE, sizeof(array_size), &array_size, NULL); - clGetImageInfo(val, CL_IMAGE_SLICE_PITCH, sizeof(slice_pitch), &slice_pitch, NULL); - assert(depth == 0); - assert(array_size == 0); - assert(slice_pitch == 0); - - clGetImageInfo(val, CL_IMAGE_BUFFER, sizeof(buf), &buf, NULL); - size_t sz; - clGetMemObjectInfo(buf, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - printf(" image %zu x %zu rp %zu @ %p buffer %zu", width, height, row_pitch, buf, sz); - } else { - size_t sz; - clGetMemObjectInfo(val, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - printf(" buffer %zu", sz); - } - } - } - printf("\n"); - } - } -} diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h index 0ccea59a3..65475ccf7 100644 --- a/selfdrive/modeld/thneed/thneed.h +++ b/selfdrive/modeld/thneed/thneed.h @@ -16,6 +16,8 @@ using namespace std; +cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value); + namespace json11 { class Json; } @@ -40,7 +42,6 @@ class CLQueuedKernel { const size_t *_global_work_size, const size_t *_local_work_size); cl_int exec(); - uint64_t benchmark(); void debug_print(bool verbose); int get_arg_num(const char *search_arg_name); cl_program program; @@ -89,12 +90,10 @@ class CachedCommand: public CachedIoctl { class Thneed { public: - Thneed(bool do_clinit=false); + Thneed(bool do_clinit=false, cl_context _context = NULL); void stop(); void execute(float **finputs, float *foutput, bool slow=false); void wait(); - int optimize(); - bool run_optimizer = false; vector input_clmem; vector inputs; @@ -110,13 +109,15 @@ class Thneed { bool record = false; int debug; int timestamp; + +#ifdef QCOM2 unique_ptr ram; vector > cmds; int fd; +#endif // all CL kernels - void find_inputs_outputs(); - void copy_inputs(float **finputs); + void copy_inputs(float **finputs, bool internal=false); void copy_output(float *foutput); cl_int clexec(); vector > kq; @@ -124,9 +125,8 @@ class Thneed { // pending CL kernels vector > ckq; - // loading and saving + // loading void load(const char *filename); - void save(const char *filename, bool save_binaries=false); private: void clinit(); }; diff --git a/selfdrive/modeld/thneed/thneed_common.cc b/selfdrive/modeld/thneed/thneed_common.cc new file mode 100644 index 000000000..ecdf1237e --- /dev/null +++ b/selfdrive/modeld/thneed/thneed_common.cc @@ -0,0 +1,216 @@ +#include "selfdrive/modeld/thneed/thneed.h" + +#include +#include +#include + +#include "common/clutil.h" +#include "common/timing.h" + +map, string> g_args; +map, int> g_args_size; +map g_program_source; + +void Thneed::stop() { + //printf("Thneed::stop: recorded %lu commands\n", cmds.size()); + record = false; +} + +void Thneed::clinit() { + device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); + if (context == NULL) context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); + //cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, CL_QUEUE_PROFILING_ENABLE, 0}; + cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, 0, 0}; + command_queue = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); + printf("Thneed::clinit done\n"); +} + +cl_int Thneed::clexec() { + if (debug >= 1) printf("Thneed::clexec: running %lu queued kernels\n", kq.size()); + for (auto &k : kq) { + if (record) ckq.push_back(k); + cl_int ret = k->exec(); + assert(ret == CL_SUCCESS); + } + return clFinish(command_queue); +} + +void Thneed::copy_inputs(float **finputs, bool internal) { + for (int idx = 0; idx < inputs.size(); ++idx) { + if (debug >= 1) printf("copying %lu -- %p -> %p (cl %p)\n", input_sizes[idx], finputs[idx], inputs[idx], input_clmem[idx]); + + if (internal) { + // if it's internal, using memcpy is fine since the buffer sync is cached in the ioctl layer + if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]); + } else { + if (finputs[idx] != NULL) CL_CHECK(clEnqueueWriteBuffer(command_queue, input_clmem[idx], CL_TRUE, 0, input_sizes[idx], finputs[idx], 0, NULL, NULL)); + } + } +} + +void Thneed::copy_output(float *foutput) { + if (output != NULL) { + size_t sz; + clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + if (debug >= 1) printf("copying %lu for output %p -> %p\n", sz, output, foutput); + CL_CHECK(clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL)); + } else { + printf("CAUTION: model output is NULL, does it have no outputs?\n"); + } +} + +// *********** CLQueuedKernel *********** + +CLQueuedKernel::CLQueuedKernel(Thneed *lthneed, + cl_kernel _kernel, + cl_uint _work_dim, + const size_t *_global_work_size, + const size_t *_local_work_size) { + thneed = lthneed; + kernel = _kernel; + work_dim = _work_dim; + assert(work_dim <= 3); + for (int i = 0; i < work_dim; i++) { + global_work_size[i] = _global_work_size[i]; + local_work_size[i] = _local_work_size[i]; + } + + char _name[0x100]; + clGetKernelInfo(kernel, CL_KERNEL_FUNCTION_NAME, sizeof(_name), _name, NULL); + name = string(_name); + clGetKernelInfo(kernel, CL_KERNEL_NUM_ARGS, sizeof(num_args), &num_args, NULL); + + // get args + for (int i = 0; i < num_args; i++) { + char arg_name[0x100] = {0}; + clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); + arg_names.push_back(string(arg_name)); + clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); + arg_types.push_back(string(arg_name)); + + args.push_back(g_args[make_pair(kernel, i)]); + args_size.push_back(g_args_size[make_pair(kernel, i)]); + } + + // get program + clGetKernelInfo(kernel, CL_KERNEL_PROGRAM, sizeof(program), &program, NULL); +} + +int CLQueuedKernel::get_arg_num(const char *search_arg_name) { + for (int i = 0; i < num_args; i++) { + if (arg_names[i] == search_arg_name) return i; + } + printf("failed to find %s in %s\n", search_arg_name, name.c_str()); + assert(false); +} + +cl_int CLQueuedKernel::exec() { + if (kernel == NULL) { + kernel = clCreateKernel(program, name.c_str(), NULL); + arg_names.clear(); + arg_types.clear(); + + for (int j = 0; j < num_args; j++) { + char arg_name[0x100] = {0}; + clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); + arg_names.push_back(string(arg_name)); + clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); + arg_types.push_back(string(arg_name)); + + cl_int ret; + if (args[j].size() != 0) { + assert(args[j].size() == args_size[j]); + ret = thneed_clSetKernelArg(kernel, j, args[j].size(), args[j].data()); + } else { + ret = thneed_clSetKernelArg(kernel, j, args_size[j], NULL); + } + assert(ret == CL_SUCCESS); + } + } + + if (thneed->debug >= 1) { + debug_print(thneed->debug >= 2); + } + + return clEnqueueNDRangeKernel(thneed->command_queue, + kernel, work_dim, NULL, global_work_size, local_work_size, 0, NULL, NULL); +} + +void CLQueuedKernel::debug_print(bool verbose) { + printf("%p %56s -- ", kernel, name.c_str()); + for (int i = 0; i < work_dim; i++) { + printf("%4zu ", global_work_size[i]); + } + printf(" -- "); + for (int i = 0; i < work_dim; i++) { + printf("%4zu ", local_work_size[i]); + } + printf("\n"); + + if (verbose) { + for (int i = 0; i < num_args; i++) { + string arg = args[i]; + printf(" %s %s", arg_types[i].c_str(), arg_names[i].c_str()); + void *arg_value = (void*)arg.data(); + int arg_size = arg.size(); + if (arg_size == 0) { + printf(" (size) %d", args_size[i]); + } else if (arg_size == 1) { + printf(" = %d", *((char*)arg_value)); + } else if (arg_size == 2) { + printf(" = %d", *((short*)arg_value)); + } else if (arg_size == 4) { + if (arg_types[i] == "float") { + printf(" = %f", *((float*)arg_value)); + } else { + printf(" = %d", *((int*)arg_value)); + } + } else if (arg_size == 8) { + cl_mem val = (cl_mem)(*((uintptr_t*)arg_value)); + printf(" = %p", val); + if (val != NULL) { + cl_mem_object_type obj_type; + clGetMemObjectInfo(val, CL_MEM_TYPE, sizeof(obj_type), &obj_type, NULL); + if (arg_types[i] == "image2d_t" || arg_types[i] == "image1d_t" || obj_type == CL_MEM_OBJECT_IMAGE2D) { + cl_image_format format; + size_t width, height, depth, array_size, row_pitch, slice_pitch; + cl_mem buf; + clGetImageInfo(val, CL_IMAGE_FORMAT, sizeof(format), &format, NULL); + assert(format.image_channel_order == CL_RGBA); + assert(format.image_channel_data_type == CL_HALF_FLOAT || format.image_channel_data_type == CL_FLOAT); + clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL); + clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL); + clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL); + clGetImageInfo(val, CL_IMAGE_DEPTH, sizeof(depth), &depth, NULL); + clGetImageInfo(val, CL_IMAGE_ARRAY_SIZE, sizeof(array_size), &array_size, NULL); + clGetImageInfo(val, CL_IMAGE_SLICE_PITCH, sizeof(slice_pitch), &slice_pitch, NULL); + assert(depth == 0); + assert(array_size == 0); + assert(slice_pitch == 0); + + clGetImageInfo(val, CL_IMAGE_BUFFER, sizeof(buf), &buf, NULL); + size_t sz = 0; + if (buf != NULL) clGetMemObjectInfo(buf, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + printf(" image %zu x %zu rp %zu @ %p buffer %zu", width, height, row_pitch, buf, sz); + } else { + size_t sz; + clGetMemObjectInfo(val, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + printf(" buffer %zu", sz); + } + } + } + printf("\n"); + } + } +} + +cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value) { + g_args_size[make_pair(kernel, arg_index)] = arg_size; + if (arg_value != NULL) { + g_args[make_pair(kernel, arg_index)] = string((char*)arg_value, arg_size); + } else { + g_args[make_pair(kernel, arg_index)] = string(""); + } + cl_int ret = clSetKernelArg(kernel, arg_index, arg_size, arg_value); + return ret; +} diff --git a/selfdrive/modeld/thneed/thneed_qcom2.cc b/selfdrive/modeld/thneed/thneed_qcom2.cc new file mode 100644 index 000000000..a29a82c8c --- /dev/null +++ b/selfdrive/modeld/thneed/thneed_qcom2.cc @@ -0,0 +1,283 @@ +#include "selfdrive/modeld/thneed/thneed.h" + +#include +#include + +#include +#include +#include +#include +#include + +#include "common/clutil.h" +#include "common/timing.h" + +Thneed *g_thneed = NULL; +int g_fd = -1; + +void hexdump(uint8_t *d, int len) { + assert((len%4) == 0); + printf(" dumping %p len 0x%x\n", d, len); + for (int i = 0; i < len/4; i++) { + if (i != 0 && (i%0x10) == 0) printf("\n"); + printf("%8x ", d[i]); + } + printf("\n"); +} + +// *********** ioctl interceptor *********** + +extern "C" { + +int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL; +#undef ioctl +int ioctl(int filedes, unsigned long request, void *argp) { + request &= 0xFFFFFFFF; // needed on QCOM2 + if (my_ioctl == NULL) my_ioctl = reinterpret_cast(dlsym(RTLD_NEXT, "ioctl")); + Thneed *thneed = g_thneed; + + // save the fd + if (request == IOCTL_KGSL_GPUOBJ_ALLOC) g_fd = filedes; + + // note that this runs always, even without a thneed object + if (request == IOCTL_KGSL_DRAWCTXT_CREATE) { + struct kgsl_drawctxt_create *create = (struct kgsl_drawctxt_create *)argp; + create->flags &= ~KGSL_CONTEXT_PRIORITY_MASK; + create->flags |= 1 << KGSL_CONTEXT_PRIORITY_SHIFT; // priority from 1-15, 1 is max priority + printf("IOCTL_KGSL_DRAWCTXT_CREATE: creating context with flags 0x%x\n", create->flags); + } + + if (thneed != NULL) { + if (request == IOCTL_KGSL_GPU_COMMAND) { + struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp; + if (thneed->record) { + thneed->timestamp = cmd->timestamp; + thneed->context_id = cmd->context_id; + thneed->cmds.push_back(unique_ptr(new CachedCommand(thneed, cmd))); + } + if (thneed->debug >= 1) { + printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx context_id: %u timestamp: %u numcmds: %d numobjs: %d\n", + thneed->cmds.size(), + cmd->flags, + cmd->context_id, cmd->timestamp, cmd->numcmds, cmd->numobjs); + } + } else if (request == IOCTL_KGSL_GPUOBJ_SYNC) { + struct kgsl_gpuobj_sync *cmd = (struct kgsl_gpuobj_sync *)argp; + struct kgsl_gpuobj_sync_obj *objs = (struct kgsl_gpuobj_sync_obj *)(cmd->objs); + + if (thneed->debug >= 2) { + printf("IOCTL_KGSL_GPUOBJ_SYNC count:%d ", cmd->count); + for (int i = 0; i < cmd->count; i++) { + printf(" -- offset:0x%lx len:0x%lx id:%d op:%d ", objs[i].offset, objs[i].length, objs[i].id, objs[i].op); + } + printf("\n"); + } + + if (thneed->record) { + thneed->cmds.push_back(unique_ptr(new + CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count)))); + } + } else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) { + struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp; + if (thneed->debug >= 1) { + printf("IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID: context_id: %d timestamp: %d timeout: %d\n", + cmd->context_id, cmd->timestamp, cmd->timeout); + } + } else if (request == IOCTL_KGSL_SETPROPERTY) { + if (thneed->debug >= 1) { + struct kgsl_device_getproperty *prop = (struct kgsl_device_getproperty *)argp; + printf("IOCTL_KGSL_SETPROPERTY: 0x%x sizebytes:%zu\n", prop->type, prop->sizebytes); + if (thneed->debug >= 2) { + hexdump((uint8_t *)prop->value, prop->sizebytes); + if (prop->type == KGSL_PROP_PWR_CONSTRAINT) { + struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value; + hexdump((uint8_t *)constraint->data, constraint->size); + } + } + } + } else if (request == IOCTL_KGSL_DRAWCTXT_CREATE || request == IOCTL_KGSL_DRAWCTXT_DESTROY) { + // this happens + } else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) { + // this happens + } else { + if (thneed->debug >= 1) { + printf("other ioctl %lx\n", request); + } + } + } + + int ret = my_ioctl(filedes, request, argp); + if (ret != 0) printf("ioctl returned %d with errno %d\n", ret, errno); + return ret; +} + +} + +// *********** GPUMalloc *********** + +GPUMalloc::GPUMalloc(int size, int fd) { + struct kgsl_gpuobj_alloc alloc; + memset(&alloc, 0, sizeof(alloc)); + alloc.size = size; + alloc.flags = 0x10000a00; + ioctl(fd, IOCTL_KGSL_GPUOBJ_ALLOC, &alloc); + void *addr = mmap64(NULL, alloc.mmapsize, 0x3, 0x1, fd, alloc.id*0x1000); + assert(addr != MAP_FAILED); + + base = (uint64_t)addr; + remaining = size; +} + +GPUMalloc::~GPUMalloc() { + // TODO: free the GPU malloced area +} + +void *GPUMalloc::alloc(int size) { + void *ret = (void*)base; + size = (size+0xff) & (~0xFF); + assert(size <= remaining); + remaining -= size; + base += size; + return ret; +} + +// *********** CachedSync, at the ioctl layer *********** + +void CachedSync::exec() { + struct kgsl_gpuobj_sync cmd; + + cmd.objs = (uint64_t)data.data(); + cmd.obj_len = data.length(); + cmd.count = data.length() / sizeof(struct kgsl_gpuobj_sync_obj); + + int ret = ioctl(thneed->fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd); + assert(ret == 0); +} + +// *********** CachedCommand, at the ioctl layer *********** + +CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) { + thneed = lthneed; + assert(cmd->numsyncs == 0); + + memcpy(&cache, cmd, sizeof(cache)); + + if (cmd->numcmds > 0) { + cmds = make_unique(cmd->numcmds); + memcpy(cmds.get(), (void *)cmd->cmdlist, sizeof(struct kgsl_command_object)*cmd->numcmds); + cache.cmdlist = (uint64_t)cmds.get(); + for (int i = 0; i < cmd->numcmds; i++) { + void *nn = thneed->ram->alloc(cmds[i].size); + memcpy(nn, (void*)cmds[i].gpuaddr, cmds[i].size); + cmds[i].gpuaddr = (uint64_t)nn; + } + } + + if (cmd->numobjs > 0) { + objs = make_unique(cmd->numobjs); + memcpy(objs.get(), (void *)cmd->objlist, sizeof(struct kgsl_command_object)*cmd->numobjs); + cache.objlist = (uint64_t)objs.get(); + for (int i = 0; i < cmd->numobjs; i++) { + void *nn = thneed->ram->alloc(objs[i].size); + memset(nn, 0, objs[i].size); + objs[i].gpuaddr = (uint64_t)nn; + } + } + + kq = thneed->ckq; + thneed->ckq.clear(); +} + +void CachedCommand::exec() { + cache.timestamp = ++thneed->timestamp; + int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache); + + if (thneed->debug >= 1) printf("CachedCommand::exec got %d\n", ret); + + if (thneed->debug >= 2) { + for (auto &it : kq) { + it->debug_print(false); + } + } + + assert(ret == 0); +} + +// *********** Thneed *********** + +Thneed::Thneed(bool do_clinit, cl_context _context) { + // TODO: QCOM2 actually requires a different context + //context = _context; + if (do_clinit) clinit(); + assert(g_fd != -1); + fd = g_fd; + ram = make_unique(0x80000, fd); + timestamp = -1; + g_thneed = this; + char *thneed_debug_env = getenv("THNEED_DEBUG"); + debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0; +} + +void Thneed::wait() { + struct kgsl_device_waittimestamp_ctxtid wait; + wait.context_id = context_id; + wait.timestamp = timestamp; + wait.timeout = -1; + + uint64_t tb = nanos_since_boot(); + int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait); + uint64_t te = nanos_since_boot(); + + if (debug >= 1) printf("wait %d after %lu us\n", wret, (te-tb)/1000); +} + +void Thneed::execute(float **finputs, float *foutput, bool slow) { + uint64_t tb, te; + if (debug >= 1) tb = nanos_since_boot(); + + // ****** copy inputs + copy_inputs(finputs, true); + + // ****** set power constraint + int ret; + struct kgsl_device_constraint_pwrlevel pwrlevel; + pwrlevel.level = KGSL_CONSTRAINT_PWR_MAX; + + struct kgsl_device_constraint constraint; + constraint.type = KGSL_CONSTRAINT_PWRLEVEL; + constraint.context_id = context_id; + constraint.data = (void*)&pwrlevel; + constraint.size = sizeof(pwrlevel); + + struct kgsl_device_getproperty prop; + prop.type = KGSL_PROP_PWR_CONSTRAINT; + prop.value = (void*)&constraint; + prop.sizebytes = sizeof(constraint); + ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); + assert(ret == 0); + + // ****** run commands + int i = 0; + for (auto &it : cmds) { + ++i; + if (debug >= 1) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000); + it->exec(); + if ((i == cmds.size()) || slow) wait(); + } + + // ****** copy outputs + copy_output(foutput); + + // ****** unset power constraint + constraint.type = KGSL_CONSTRAINT_NONE; + constraint.data = NULL; + constraint.size = 0; + + ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); + assert(ret == 0); + + if (debug >= 1) { + te = nanos_since_boot(); + printf("model exec in %lu us\n", (te-tb)/1000); + } +} diff --git a/selfdrive/modeld/thneed/weights_fixup.py b/selfdrive/modeld/thneed/weights_fixup.py deleted file mode 100755 index 539b1b5d3..000000000 --- a/selfdrive/modeld/thneed/weights_fixup.py +++ /dev/null @@ -1,146 +0,0 @@ -#!/usr/bin/env python3 -import os -import struct -import zipfile -import numpy as np -from tqdm import tqdm - -from common.basedir import BASEDIR -from selfdrive.modeld.thneed.lib import load_thneed, save_thneed - -# this is junk code, but it doesn't have deps -def load_dlc_weights(fn): - archive = zipfile.ZipFile(fn, 'r') - dlc_params = archive.read("model.params") - - def extract(rdat): - idx = rdat.find(b"\x00\x00\x00\x09\x04\x00\x00\x00") - rdat = rdat[idx+8:] - ll = struct.unpack("I", rdat[0:4])[0] - buf = np.frombuffer(rdat[4:4+ll*4], dtype=np.float32) - rdat = rdat[4+ll*4:] - dims = struct.unpack("I", rdat[0:4])[0] - buf = buf.reshape(struct.unpack("I"*dims, rdat[4:4+dims*4])) - if len(buf.shape) == 4: - buf = np.transpose(buf, (3,2,0,1)) - return buf - - def parse(tdat): - ll = struct.unpack("I", tdat[0:4])[0] + 4 - return (None, [extract(tdat[0:]), extract(tdat[ll:])]) - - ptr = 0x20 - def r4(): - nonlocal ptr - ret = struct.unpack("I", dlc_params[ptr:ptr+4])[0] - ptr += 4 - return ret - ranges = [] - cnt = r4() - for _ in range(cnt): - o = r4() + ptr - # the header is 0xC - plen, is_4, is_2 = struct.unpack("III", dlc_params[o:o+0xC]) - assert is_4 == 4 and is_2 == 2 - ranges.append((o+0xC, o+plen+0xC)) - ranges = sorted(ranges, reverse=True) - - return [parse(dlc_params[s:e]) for s,e in ranges] - -# this won't run on device without onnx -def load_onnx_weights(fn): - import onnx - from onnx import numpy_helper - - model = onnx.load(fn) - graph = model.graph # pylint: disable=maybe-no-member - init = {x.name:x for x in graph.initializer} - - onnx_layers = [] - for node in graph.node: - #print(node.name, node.op_type, node.input, node.output) - vals = [] - for inp in node.input: - if inp in init: - vals.append(numpy_helper.to_array(init[inp])) - if len(vals) > 0: - onnx_layers.append((node.name, vals)) - return onnx_layers - -def weights_fixup(target, source_thneed, dlc): - #onnx_layers = load_onnx_weights(os.path.join(BASEDIR, "models/supercombo.onnx")) - onnx_layers = load_dlc_weights(dlc) - jdat = load_thneed(source_thneed) - - bufs = {} - for o in jdat['objects']: - bufs[o['id']] = o - - thneed_layers = [] - for k in jdat['kernels']: - #print(k['name']) - vals = [] - for a in k['args']: - if a in bufs: - o = bufs[a] - if o['needs_load'] or ('buffer_id' in o and bufs[o['buffer_id']]['needs_load']): - #print(" ", o['arg_type']) - vals.append(o) - if len(vals) > 0: - thneed_layers.append((k['name'], vals)) - - assert len(thneed_layers) == len(onnx_layers) - - # fix up weights - for tl, ol in tqdm(zip(thneed_layers, onnx_layers), total=len(thneed_layers)): - #print(tl[0], ol[0]) - assert len(tl[1]) == len(ol[1]) - for o, onnx_weight in zip(tl[1], ol[1]): - if o['arg_type'] == "image2d_t": - obuf = bufs[o['buffer_id']] - saved_weights = np.frombuffer(obuf['data'], dtype=np.float16).reshape(o['height'], o['row_pitch']//2) - - if len(onnx_weight.shape) == 4: - # convolution - oc,ic,ch,cw = onnx_weight.shape - - if 'depthwise' in tl[0]: - assert ic == 1 - weights = np.transpose(onnx_weight.reshape(oc//4,4,ch,cw), (0,2,3,1)).reshape(o['height'], o['width']*4) - else: - weights = np.transpose(onnx_weight.reshape(oc//4,4,ic//4,4,ch,cw), (0,4,2,5,1,3)).reshape(o['height'], o['width']*4) - else: - # fc_Wtx - weights = onnx_weight - - new_weights = np.zeros((o['height'], o['row_pitch']//2), dtype=np.float32) - new_weights[:, :weights.shape[1]] = weights - - # weights shouldn't be too far off - err = np.mean((saved_weights.astype(np.float32) - new_weights)**2) - assert err < 1e-3 - rerr = np.mean(np.abs((saved_weights.astype(np.float32) - new_weights)/(new_weights+1e-12))) - assert rerr < 0.5 - - # fix should improve things - fixed_err = np.mean((new_weights.astype(np.float16).astype(np.float32) - new_weights)**2) - assert (err/fixed_err) >= 1 - - #print(" ", o['size'], onnx_weight.shape, o['row_pitch'], o['width'], o['height'], "err %.2fx better" % (err/fixed_err)) - - obuf['data'] = new_weights.astype(np.float16).tobytes() - - elif o['arg_type'] == "float*": - # unconverted floats are correct - new_weights = np.zeros(o['size']//4, dtype=np.float32) - new_weights[:onnx_weight.shape[0]] = onnx_weight - assert new_weights.tobytes() == o['data'] - #print(" ", o['size'], onnx_weight.shape) - - save_thneed(jdat, target) - -if __name__ == "__main__": - model_dir = os.path.join(BASEDIR, "selfdrive/modeld/models/") - weights_fixup(os.path.join(model_dir, "supercombo_fixed.thneed"), - os.path.join(model_dir, "supercombo.thneed"), - os.path.join(model_dir, "supercombo.dlc")) diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc index cabc58a46..f34131414 100644 --- a/selfdrive/modeld/transforms/transform.cc +++ b/selfdrive/modeld/transforms/transform.cc @@ -32,7 +32,7 @@ void transform_queue(Transform* s, const int zero = 0; // sampled using pixel center origin - // (because thats how fastcv and opencv does it) + // (because that's how fastcv and opencv does it) mat3 projection_y = projection; diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 9ff3125c1..e3f6a5094 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -29,10 +29,12 @@ class DRIVER_MONITOR_SETTINGS(): self._FACE_THRESHOLD = 0.7 self._EYE_THRESHOLD = 0.65 self._SG_THRESHOLD = 0.9 - self._BLINK_THRESHOLD = 0.87 + self._BLINK_THRESHOLD = 0.895 - self._EE_THRESH11 = 0.75 - self._EE_THRESH12 = 3.25 + self._EE_THRESH11 = 0.275 + self._EE_THRESH12 = 5.5 + self._EE_MAX_OFFSET1 = 0.06 + self._EE_MIN_OFFSET1 = 0.025 self._EE_THRESH21 = 0.01 self._EE_THRESH22 = 0.35 @@ -43,6 +45,7 @@ class DRIVER_MONITOR_SETTINGS(): self._POSE_YAW_THRESHOLD_SLACK = 0.5042 self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD self._PITCH_NATURAL_OFFSET = 0.029 # initial value before offset is learned + self._PITCH_NATURAL_THRESHOLD = 0.449 self._YAW_NATURAL_OFFSET = 0.097 # initial value before offset is learned self._PITCH_MAX_OFFSET = 0.124 self._PITCH_MIN_OFFSET = -0.0881 @@ -196,7 +199,7 @@ class DriverStatus(): self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit yaw_error = abs(yaw_error) - if pitch_error > self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch or \ + if pitch_error > (self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch if self.pose_calibrated else self.settings._PITCH_NATURAL_THRESHOLD) or \ yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw: distracted_types.append(DistractedType.DISTRACTED_POSE) @@ -204,14 +207,14 @@ class DriverStatus(): distracted_types.append(DistractedType.DISTRACTED_BLINK) if self.ee1_calibrated: - ee1_dist = self.eev1 > self.ee1_offseter.filtered_stat.M * self.settings._EE_THRESH12 + ee1_dist = self.eev1 > max(min(self.ee1_offseter.filtered_stat.M, self.settings._EE_MAX_OFFSET1), self.settings._EE_MIN_OFFSET1) * self.settings._EE_THRESH12 else: ee1_dist = self.eev1 > self.settings._EE_THRESH11 - if self.ee2_calibrated: - ee2_dist = self.eev2 < self.ee2_offseter.filtered_stat.M * self.settings._EE_THRESH22 - else: - ee2_dist = self.eev2 < self.settings._EE_THRESH21 - if ee1_dist or ee2_dist: + # if self.ee2_calibrated: + # ee2_dist = self.eev2 < self.ee2_offseter.filtered_stat.M * self.settings._EE_THRESH22 + # else: + # ee2_dist = self.eev2 < self.settings._EE_THRESH21 + if ee1_dist: distracted_types.append(DistractedType.DISTRACTED_E2E) return distracted_types @@ -257,12 +260,11 @@ class DriverStatus(): self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.eev1 = driver_data.notReadyProb[1] + self.eev1 = driver_data.notReadyProb[0] self.eev2 = driver_data.readyProb[0] self.distracted_types = self._get_distracted_types() - self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or - DistractedType.DISTRACTED_BLINK in self.distracted_types) and \ + self.driver_distracted = (DistractedType.DISTRACTED_E2E in self.distracted_types or DistractedType.DISTRACTED_POSE in self.distracted_types or DistractedType.DISTRACTED_BLINK in self.distracted_types) and \ driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 89a1c9bdf..4855b6359 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -120,6 +120,10 @@ class RouteEngine: cloudlog.warning(f"Calculating route {self.last_position} -> {destination}") self.nav_destination = destination + lang = self.params.get('LanguageSetting', encoding='utf8') + if lang is not None: + lang = lang.replace('main_', '') + params = { 'access_token': self.mapbox_token, 'annotations': 'maxspeed', @@ -128,6 +132,7 @@ class RouteEngine: 'steps': 'true', 'banner_instructions': 'true', 'alternatives': 'false', + 'language': lang, } if self.last_bearing is not None: @@ -135,7 +140,7 @@ class RouteEngine: url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}' try: - resp = requests.get(url, params=params) + resp = requests.get(url, params=params, timeout=10) resp.raise_for_status() r = resp.json() @@ -194,10 +199,10 @@ class RouteEngine: parse_banner_instructions(msg.navInstruction, step['bannerInstructions'], distance_to_maneuver_along_geometry) # Compute total remaining time and distance - remaning = 1.0 - along_geometry / max(step['distance'], 1) - total_distance = step['distance'] * remaning - total_time = step['duration'] * remaning - total_time_typical = step['duration_typical'] * remaning + remaining = 1.0 - along_geometry / max(step['distance'], 1) + total_distance = step['distance'] * remaining + total_time = step['duration'] * remaining + total_time_typical = step['duration_typical'] * remaining # Add up totals for future steps for i in range(self.step_idx + 1, len(self.route)): @@ -236,7 +241,7 @@ class RouteEngine: self.recompute_countdown = 0 else: cloudlog.warning("Destination reached") - Params().delete("NavDestination") + Params().remove("NavDestination") # Clear route if driving away from destination dist = self.nav_destination.distance_to(self.last_position) diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index db32887e7..8f26c0085 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -13,7 +13,7 @@ sensors = [ 'sensors/lsm6ds3_temp.cc', 'sensors/mmc5603nj_magn.cc', ] -libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj'] +libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj', 'pthread'] if arch == "larch64": libs.append('i2c') env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py new file mode 100755 index 000000000..f56af1c70 --- /dev/null +++ b/selfdrive/sensord/pigeond.py @@ -0,0 +1,313 @@ +#!/usr/bin/env python3 +import sys +import time +import signal +import serial +import struct +import requests +import urllib.parse +from datetime import datetime +from typing import List, Optional, Tuple + +from cereal import messaging +from common.params import Params +from system.swaglog import cloudlog +from system.hardware import TICI +from common.gpio import gpio_init, gpio_set +from system.hardware.tici.pins import GPIO + +UBLOX_TTY = "/dev/ttyHS0" + +UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00" +UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00" +UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00" +UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00" +UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03" +UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00" + +def set_power(enabled: bool) -> None: + gpio_init(GPIO.UBLOX_SAFEBOOT_N, True) + gpio_init(GPIO.UBLOX_PWR_EN, True) + gpio_init(GPIO.UBLOX_RST_N, True) + + gpio_set(GPIO.UBLOX_SAFEBOOT_N, True) + gpio_set(GPIO.UBLOX_PWR_EN, enabled) + gpio_set(GPIO.UBLOX_RST_N, enabled) + +def add_ubx_checksum(msg: bytes) -> bytes: + A = B = 0 + for b in msg[2:]: + A = (A + b) % 256 + B = (B + A) % 256 + return msg + bytes([A, B]) + +def get_assistnow_messages(token: bytes) -> List[bytes]: + # make request + # TODO: implement adding the last known location + r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({ + 'token': token, + 'gnss': 'gps,glo', + 'datatype': 'eph,alm,aux', + }, safe=':,'), timeout=5) + assert r.status_code == 200, "Got invalid status code" + dat = r.content + + # split up messages + msgs = [] + while len(dat) > 0: + assert dat[:2] == b"\xB5\x62" + msg_len = 6 + (dat[5] << 8 | dat[4]) + 2 + msgs.append(dat[:msg_len]) + dat = dat[msg_len:] + return msgs + + +class TTYPigeon(): + def __init__(self): + self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0) + + def send(self, dat: bytes) -> None: + self.tty.write(dat) + + def receive(self) -> bytes: + dat = b'' + while len(dat) < 0x1000: + d = self.tty.read(0x40) + dat += d + if len(d) == 0: + break + return dat + + def set_baud(self, baud: int) -> None: + self.tty.baudrate = baud + + def wait_for_ack(self, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK, timeout: float = 0.5) -> bool: + dat = b'' + st = time.monotonic() + while True: + dat += self.receive() + if ack in dat: + cloudlog.debug("Received ACK from ublox") + return True + elif nack in dat: + cloudlog.error("Received NACK from ublox") + return False + elif time.monotonic() - st > timeout: + cloudlog.error("No response from ublox") + raise TimeoutError('No response from ublox') + time.sleep(0.001) + + def send_with_ack(self, dat: bytes, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK) -> None: + self.send(dat) + self.wait_for_ack(ack, nack) + + def wait_for_backup_restore_status(self, timeout: float = 1.) -> int: + dat = b'' + st = time.monotonic() + while True: + dat += self.receive() + position = dat.find(UBLOX_BACKUP_RESTORE_MSG) + if position >= 0 and len(dat) >= position + 11: + return dat[position + 10] + elif time.monotonic() - st > timeout: + cloudlog.error("No backup restore response from ublox") + raise TimeoutError('No response from ublox') + time.sleep(0.001) + + def reset_device(self) -> bool: + # deleting the backup does not always work on first try (mostly on second try) + for _ in range(5): + # device cold start + self.send(b"\xb5\x62\x06\x04\x04\x00\xff\xff\x00\x00\x0c\x5d") + time.sleep(1) # wait for cold start + init_baudrate(self) + + # clear configuration + self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x00\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\x5b") + + # clear flash memory (almanac backup) + self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0") + + # try restoring backup to verify it got deleted + self.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60") + # 1: failed to restore, 2: could restore, 3: no backup + status = self.wait_for_backup_restore_status() + if status == 1 or status == 3: + return True + return False + +def init_baudrate(pigeon: TTYPigeon): + # ublox default setting on startup is 9600 baudrate + pigeon.set_baud(9600) + + # $PUBX,41,1,0007,0003,460800,0*15\r\n + pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A") + time.sleep(0.1) + pigeon.set_baud(460800) + + +def initialize_pigeon(pigeon: TTYPigeon) -> bool: + # try initializing a few times + for _ in range(10): + try: + + # setup port config + pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F") + pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35") + pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80") + pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85") + pigeon.send_with_ack(b"\xb5\x62\x06\x00\x00\x00\x06\x18") + pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x01\x08\x22") + pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x03\x0A\x24") + + # UBX-CFG-RATE (0x06 0x08) + pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10") + + # UBX-CFG-NAV5 (0x06 0x24) + pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63") + + # UBX-CFG-ODO (0x06 0x1E) + pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37") + pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C") + pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24") + + # UBX-CFG-NAV5 (0x06 0x24) + pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84") + pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81") + pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72") + pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3") + + # UBX-CFG-MSG (set message rate) + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70") + 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") + cloudlog.debug("pigeon configured") + + # try restoring almanac backup + pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60") + restore_status = pigeon.wait_for_backup_restore_status() + if restore_status == 2: + cloudlog.warning("almanac backup restored") + elif restore_status == 3: + cloudlog.warning("no almanac backup found") + else: + cloudlog.error(f"failed to restore almanac backup, status: {restore_status}") + + # sending time to ublox + t_now = datetime.utcnow() + if t_now >= datetime(2021, 6, 1): + cloudlog.warning("Sending current time to ublox") + + # UBX-MGA-INI-TIME_UTC + msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack(" Tuple[TTYPigeon, messaging.PubMaster]: + pigeon = None + + # register exit handler + signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon)) + pm = messaging.PubMaster(['ubloxRaw']) + + # power cycle ublox + set_power(False) + time.sleep(0.1) + set_power(True) + time.sleep(0.5) + + pigeon = TTYPigeon() + return pigeon, pm + +def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0): + + start_time = time.monotonic() + def end_condition(): + return True if duration == 0 else time.monotonic() - start_time < duration + + while end_condition(): + dat = pigeon.receive() + if len(dat) > 0: + if dat[0] == 0x00: + cloudlog.warning("received invalid data from ublox, re-initing!") + init_baudrate(pigeon) + initialize_pigeon(pigeon) + continue + + # send out to socket + msg = messaging.new_message('ubloxRaw', len(dat)) + msg.ubloxRaw = dat[:] + pm.send('ubloxRaw', msg) + else: + # prevent locking up a CPU core if ublox disconnects + time.sleep(0.001) + + +def main(): + assert TICI, "unsupported hardware for pigeond" + + pigeon, pm = create_pigeon() + init_baudrate(pigeon) + r = initialize_pigeon(pigeon) + Params().put_bool("UbloxAvailable", r) + + # start receiving data + run_receiving(pigeon, pm) + +if __name__ == "__main__": + main() diff --git a/selfdrive/sensord/rawgps/modemdiag.py b/selfdrive/sensord/rawgps/modemdiag.py index cc2bc5b26..5d72aeba9 100644 --- a/selfdrive/sensord/rawgps/modemdiag.py +++ b/selfdrive/sensord/rawgps/modemdiag.py @@ -1,5 +1,3 @@ -import os -import time import select from serial import Serial from crcmod import mkCrcFun @@ -11,18 +9,7 @@ class ModemDiag: self.pend = b'' def open_serial(self): - def op(): - return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0) - try: - serial = op() - except Exception: - # TODO: this is a hack to get around modemmanager's exclusive open - print("unlocking serial...") - os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/unbind\'') - os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/bind\'') - time.sleep(0.5) - os.system("sudo chmod 666 /dev/ttyUSB0") - serial = op() + serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True) serial.flush() serial.reset_input_buffer() serial.reset_output_buffer() diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 7c4582902..1c6505166 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -5,23 +5,34 @@ import signal import itertools import math import time +import subprocess from typing import NoReturn from struct import unpack_from, calcsize, pack -import cereal.messaging as messaging -from cereal import log -from system.swaglog import cloudlog -from laika.gps_time import GPSTime +from cereal import log +import cereal.messaging as messaging +from laika.gps_time import GPSTime +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 -from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv -from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv -from selfdrive.sensord.rawgps.structs import oemdre_measurement_report, oemdre_measurement_report_sv -from selfdrive.sensord.rawgps.structs import LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT -from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT +from selfdrive.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, + LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, + LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT) DEBUG = int(os.getenv("DEBUG", "0"))==1 +LOG_TYPES = [ + LOG_GNSS_GPS_MEASUREMENT_REPORT, + LOG_GNSS_GLONASS_MEASUREMENT_REPORT, + LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, + LOG_GNSS_POSITION_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT, +] + + miscStatusFields = { "multipathEstimateIsValid": 0, "directionIsValid": 1, @@ -65,59 +76,57 @@ measurementStatusGlonassFields = { "glonassTimeMarkValid": 17 } -def main() -> NoReturn: - unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) - unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True) - unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True) - unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True) +def try_setup_logs(diag, log_types): + for _ in range(5): + try: + setup_logs(diag, log_types) + break + except Exception: + cloudlog.exception("setup logs failed, trying again") + else: + raise Exception(f"setup logs failed, {log_types=}") - unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) - unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) +def at_cmd(cmd: str) -> None: + for _ in range(5): + try: + subprocess.check_call(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True) + break + except subprocess.CalledProcessError: + cloudlog.exception("rawgps.mmcli_command_failed") + else: + raise Exception(f"failed to execute mmcli command {cmd=}") - log_types = [ - LOG_GNSS_GPS_MEASUREMENT_REPORT, - LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, - ] - pub_types = ['qcomGnss'] - unpack_position, _ = dict_unpacker(position_report) - log_types.append(LOG_GNSS_POSITION_REPORT) - pub_types.append("gpsLocation") - # connect to modem - diag = ModemDiag() +def gps_enabled() -> bool: + try: + p = subprocess.check_output("mmcli -m any --command=\"AT+QGPS?\"", shell=True) + return b"QGPS: 1" in p + except subprocess.CalledProcessError as exc: + raise Exception("failed to execute QGPS mmcli command") from exc - # NV enable OEMDRE +def setup_quectel(diag: ModemDiag): + # enable OEMDRE in the NV # TODO: it has to reboot for this to take effect DIAG_NV_READ_F = 38 DIAG_NV_WRITE_F = 39 NV_GNSS_OEM_FEATURE_MASK = 7165 + send_recv(diag, DIAG_NV_WRITE_F, pack(' NoReturn: GPSDIAG_OEM_DRE_ON = 1 # gpsdiag_OemControlReqType - opcode, payload = send_recv(diag, DIAG_SUBSYS_CMD_F, pack(' NoReturn: + unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) + unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True) + + unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True) + unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True) + + unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) + unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) + + unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True) + unpack_position, _ = dict_unpacker(position_report) + + unpack_position, _ = dict_unpacker(position_report) + + # wait for ModemManager to come up + cloudlog.warning("waiting for modem to come up") + while True: + ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) + if ret == 0: + break + time.sleep(0.1) + + # connect to modem + diag = ModemDiag() + + def cleanup(sig, frame): + cloudlog.warning(f"caught sig {sig}, disabling quectel gps") + teardown_quectel(diag) + cloudlog.warning("quectel cleanup done") + sys.exit(0) + signal.signal(signal.SIGINT, cleanup) + signal.signal(signal.SIGTERM, cleanup) + + setup_quectel(diag) + cloudlog.warning("quectel setup done") + + pm = messaging.PubMaster(['qcomGnss', 'gpsLocation']) while 1: opcode, payload = diag.recv() - assert opcode == DIAG_LOG_F + if opcode != DIAG_LOG_F: + cloudlog.error(f"Unhandled opcode: {opcode}") + continue + (pending_msgs, log_outer_length), inner_log_packet = unpack_from(' 0: cloudlog.debug("have %d pending messages" % pending_msgs) assert log_outer_length == len(inner_log_packet) + (log_inner_length, log_type, log_time), log_payload = unpack_from(' NoReturn: pm.send('gpsLocation', msg) - if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: + elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: + msg = messaging.new_message('qcomGnss') + dat = unpack_svpoly(log_payload) + dat = relist(dat) + gnss = msg.qcomGnss + gnss.logTs = log_time + gnss.init('drSvPoly') + poly = gnss.drSvPoly + for k,v in dat.items(): + if k == "version": + assert v == 2 + elif k == "flags": + pass + else: + setattr(poly, k, v) + pm.send('qcomGnss', msg) + + elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: msg = messaging.new_message('qcomGnss') gnss = msg.qcomGnss diff --git a/selfdrive/sensord/rawgps/structs.py b/selfdrive/sensord/rawgps/structs.py index 4bc9eca87..97e3d3d60 100644 --- a/selfdrive/sensord/rawgps/structs.py +++ b/selfdrive/sensord/rawgps/structs.py @@ -56,6 +56,29 @@ oemdre_measurement_report = """ uint8_t source; """ +oemdre_svpoly_report = """ + uint8_t version; + uint16_t sv_id; + int8_t frequency_index; + uint8_t flags; + uint16_t iode; + double t0; + double xyz0[3]; + double xyzN[9]; + float other[4]; + float position_uncertainty; + float iono_delay; + float iono_dot; + float sbas_iono_delay; + float sbas_iono_dot; + float tropo_delay; + float elevation; + float elevation_dot; + float elevation_uncertainty; + double velocity_coeff[12]; +""" + + oemdre_measurement_report_sv = """ uint8_t sv_id; uint8_t unkn; @@ -311,3 +334,21 @@ def dict_unpacker(ss, camelcase = False): nams = [name_to_camelcase(x) for x in nams] sz = calcsize(st) return lambda x: dict(zip(nams, unpack_from(st, x))), sz + +def relist(dat): + list_keys = set() + for key in dat.keys(): + if '[' in key: + list_keys.add(key.split('[')[0]) + list_dict = {} + for list_key in list_keys: + list_dict[list_key] = [] + i = 0 + while True: + key = list_key + f'[{i}]' + if key not in dat: + break + list_dict[list_key].append(dat[key]) + del dat[key] + i += 1 + return {**dat, **list_dict} diff --git a/selfdrive/sensord/rawgps/test_rawgps.py b/selfdrive/sensord/rawgps/test_rawgps.py new file mode 100755 index 000000000..5bd083395 --- /dev/null +++ b/selfdrive/sensord/rawgps/test_rawgps.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +import json +import time +import unittest +import subprocess + +import cereal.messaging as messaging +from system.hardware import TICI +from selfdrive.manager.process_config import managed_processes + + +class TestRawgpsd(unittest.TestCase): + @classmethod + def setUpClass(cls): + if not TICI: + raise unittest.SkipTest + + def tearDown(self): + managed_processes['rawgpsd'].stop() + + def test_startup_time(self): + for _ in range(5): + sm = messaging.SubMaster(['qcomGnss']) + managed_processes['rawgpsd'].start() + + start_time = time.monotonic() + for __ in range(10): + sm.update(1 * 1000) + if sm.updated['qcomGnss']: + break + assert sm.rcv_frame['qcomGnss'] > 0, "rawgpsd didn't start outputting messages in time" + + et = time.monotonic() - start_time + assert et < 5, f"rawgpsd took {et:.1f}s to start" + managed_processes['rawgpsd'].stop() + + def test_turns_off_gnss(self): + for s in (0.1, 0.5, 1, 5): + managed_processes['rawgpsd'].start() + time.sleep(s) + managed_processes['rawgpsd'].stop() + + ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8') + loc_status = json.loads(ls) + assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'} + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index d17e3fe61..78b3ac526 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -4,6 +4,7 @@ #include "common/swaglog.h" #include "common/timing.h" +#include "common/util.h" BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} @@ -23,6 +24,13 @@ int BMX055_Accel::init() { goto fail; } + ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE); + if (ret < 0) { + goto fail; + } + // bmx055 accel has a 1.3ms wakeup time from deep suspend mode + util::sleep_for(10); + // High bandwidth // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE); // if (ret < 0) { @@ -34,6 +42,7 @@ int BMX055_Accel::init() { if (ret < 0) { goto fail; } + ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ); if (ret < 0) { goto fail; @@ -43,7 +52,17 @@ fail: return ret; } -void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { +int BMX055_Accel::shutdown() { + // enter deep suspend mode (lowest power mode) + int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND); + if (ret < 0) { + LOGE("Could not move BMX055 ACCEL in deep suspend mode!") + } + + return ret; +} + +bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer)); @@ -55,6 +74,7 @@ void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { float y = -read_12_bit(buffer[2], buffer[3]) * scale; float z = read_12_bit(buffer[4], buffer[5]) * scale; + auto event = msg.initEvent().initAccelerometer2(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(1); event.setSensor(SENSOR_ACCELEROMETER); @@ -66,4 +86,5 @@ void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_accel.h b/selfdrive/sensord/sensors/bmx055_accel.h index 86ec419cd..8ef660a99 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.h +++ b/selfdrive/sensord/sensors/bmx055_accel.h @@ -10,6 +10,7 @@ #define BMX055_ACCEL_I2C_REG_X_LSB 0x02 #define BMX055_ACCEL_I2C_REG_TEMP 0x08 #define BMX055_ACCEL_I2C_REG_BW 0x10 +#define BMX055_ACCEL_I2C_REG_PMU 0x11 #define BMX055_ACCEL_I2C_REG_HBW 0x13 #define BMX055_ACCEL_I2C_REG_FIFO 0x3F @@ -18,6 +19,8 @@ #define BMX055_ACCEL_HBW_ENABLE 0b10000000 #define BMX055_ACCEL_HBW_DISABLE 0b00000000 +#define BMX055_ACCEL_DEEP_SUSPEND 0b00100000 +#define BMX055_ACCEL_NORMAL_MODE 0b00000000 #define BMX055_ACCEL_BW_7_81HZ 0b01000 #define BMX055_ACCEL_BW_15_63HZ 0b01001 @@ -33,5 +36,6 @@ class BMX055_Accel : public I2CSensor { public: BMX055_Accel(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index 74b22d8fe..9d70b9e43 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -4,6 +4,7 @@ #include #include "common/swaglog.h" +#include "common/util.h" #define DEG2RAD(x) ((x) * M_PI / 180.0) @@ -26,6 +27,13 @@ int BMX055_Gyro::init() { goto fail; } + ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE); + if (ret < 0) { + goto fail; + } + // bmx055 gyro has a 30ms wakeup time from deep suspend mode + util::sleep_for(50); + // High bandwidth // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE); // if (ret < 0) { @@ -54,7 +62,17 @@ fail: return ret; } -void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { +int BMX055_Gyro::shutdown() { + // enter deep suspend mode (lowest power mode) + int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND); + if (ret < 0) { + LOGE("Could not move BMX055 GYRO in deep suspend mode!") + } + + return ret; +} + +bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer)); @@ -66,6 +84,7 @@ void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + auto event = msg.initEvent().initGyroscope2(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(1); event.setSensor(SENSOR_GYRO_UNCALIBRATED); @@ -77,4 +96,5 @@ void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_gyro.h b/selfdrive/sensord/sensors/bmx055_gyro.h index ed0c16ff0..80b93f128 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.h +++ b/selfdrive/sensord/sensors/bmx055_gyro.h @@ -10,6 +10,7 @@ #define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02 #define BMX055_GYRO_I2C_REG_RANGE 0x0F #define BMX055_GYRO_I2C_REG_BW 0x10 +#define BMX055_GYRO_I2C_REG_LPM1 0x11 #define BMX055_GYRO_I2C_REG_HBW 0x13 #define BMX055_GYRO_I2C_REG_FIFO 0x3F @@ -18,6 +19,8 @@ #define BMX055_GYRO_HBW_ENABLE 0b10000000 #define BMX055_GYRO_HBW_DISABLE 0b00000000 +#define BMX055_GYRO_DEEP_SUSPEND 0b00100000 +#define BMX055_GYRO_NORMAL_MODE 0b00000000 #define BMX055_GYRO_RANGE_2000 0b000 #define BMX055_GYRO_RANGE_1000 0b001 @@ -33,5 +36,6 @@ class BMX055_Gyro : public I2CSensor { public: BMX055_Gyro(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index a2c793eff..394b1e83d 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -155,6 +155,16 @@ int BMX055_Magn::init() { return ret; } +int BMX055_Magn::shutdown() { + // move to suspend mode + int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0); + if (ret < 0) { + LOGE("Could not move BMX055 MAGN in suspend mode!") + } + + return ret; +} + bool BMX055_Magn::perform_self_test() { uint8_t buffer[8]; int16_t x, y; @@ -213,7 +223,7 @@ bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t * } -void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[8]; int16_t _x, _y, x, y, z; @@ -221,7 +231,10 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); assert(len == sizeof(buffer)); - if (parse_xyz(buffer, &_x, &_y, &z)) { + bool parsed = parse_xyz(buffer, &_x, &_y, &z); + if (parsed) { + + auto event = msg.initEvent().initMagnetometer(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(2); event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); @@ -247,4 +260,6 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { // at a 100 Hz. When reading the registers we have to check the ready bit // To verify the measurement was completed this cycle. set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED); + + return parsed; } diff --git a/selfdrive/sensord/sensors/bmx055_magn.h b/selfdrive/sensord/sensors/bmx055_magn.h index d60fd5515..e4a79bc7e 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.h +++ b/selfdrive/sensord/sensors/bmx055_magn.h @@ -59,5 +59,6 @@ class BMX055_Magn : public I2CSensor{ public: BMX055_Magn(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/selfdrive/sensord/sensors/bmx055_temp.cc index 85bdea9e6..bdb34f150 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.cc +++ b/selfdrive/sensord/sensors/bmx055_temp.cc @@ -28,7 +28,7 @@ fail: return ret; } -void BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[1]; int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer)); @@ -36,9 +36,12 @@ void BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { float temp = 23.0f + int8_t(buffer[0]) / 2.0f; + auto event = msg.initEvent().initTemperatureSensor(); event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(1); event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); event.setTimestamp(start_time); event.setTemperature(temp); + + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_temp.h b/selfdrive/sensord/sensors/bmx055_temp.h index 5ffaa8fb6..0b6802dea 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.h +++ b/selfdrive/sensord/sensors/bmx055_temp.h @@ -8,5 +8,6 @@ class BMX055_Temp : public I2CSensor { public: BMX055_Temp(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/file_sensor.cc b/selfdrive/sensord/sensors/file_sensor.cc index 812a41fa8..a74ae1ae1 100644 --- a/selfdrive/sensord/sensors/file_sensor.cc +++ b/selfdrive/sensord/sensors/file_sensor.cc @@ -2,8 +2,7 @@ #include -FileSensor::FileSensor(std::string filename) : file(filename) { -} +FileSensor::FileSensor(std::string filename) : file(filename) {} int FileSensor::init() { return file.is_open() ? 0 : 1; @@ -12,3 +11,7 @@ int FileSensor::init() { FileSensor::~FileSensor() { file.close(); } + +bool FileSensor::has_interrupt_enabled() { + return false; +} \ No newline at end of file diff --git a/selfdrive/sensord/sensors/file_sensor.h b/selfdrive/sensord/sensors/file_sensor.h index c5b4643e1..39d695167 100644 --- a/selfdrive/sensord/sensors/file_sensor.h +++ b/selfdrive/sensord/sensors/file_sensor.h @@ -14,5 +14,6 @@ public: FileSensor(std::string filename); ~FileSensor(); int init(); - virtual void get_event(cereal::SensorEventData::Builder &event) = 0; + bool has_interrupt_enabled(); + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; }; diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc index 40dfa4a73..f563f93d2 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.cc +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -15,8 +15,13 @@ int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) { return int32_t(combined) / (1 << 4); } +I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) : + bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {} -I2CSensor::I2CSensor(I2CBus *bus) : bus(bus) { +I2CSensor::~I2CSensor() { + if (gpio_fd != -1) { + close(gpio_fd); + } } int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) { @@ -26,3 +31,20 @@ int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len int I2CSensor::set_register(uint register_address, uint8_t data) { return bus->set_register(get_device_address(), register_address, data); } + +int I2CSensor::init_gpio() { + if (shared_gpio || gpio_nr == 0) { + return 0; + } + + gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr); + if (gpio_fd < 0) { + return -1; + } + + return 0; +} + +bool I2CSensor::has_interrupt_enabled() { + return gpio_nr != 0; +} diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/selfdrive/sensord/sensors/i2c_sensor.h index 7832475a9..0de2a9873 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.h +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -1,9 +1,13 @@ #pragma once #include +#include #include "cereal/gen/cpp/log.capnp.h" + #include "common/i2c.h" +#include "common/gpio.h" + #include "selfdrive/sensord/sensors/constants.h" #include "selfdrive/sensord/sensors/sensor.h" @@ -15,12 +19,18 @@ int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0); class I2CSensor : public Sensor { private: I2CBus *bus; + int gpio_nr; + bool shared_gpio; virtual uint8_t get_device_address() = 0; public: - I2CSensor(I2CBus *bus); + I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + ~I2CSensor(); int read_register(uint register_address, uint8_t *buffer, uint8_t len); int set_register(uint register_address, uint8_t data); + int init_gpio(); + bool has_interrupt_enabled(); virtual int init() = 0; - virtual void get_event(cereal::SensorEventData::Builder &event) = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; + virtual int shutdown() = 0; }; diff --git a/selfdrive/sensord/sensors/light_sensor.cc b/selfdrive/sensord/sensors/light_sensor.cc index 449734368..58c602ea3 100644 --- a/selfdrive/sensord/sensors/light_sensor.cc +++ b/selfdrive/sensord/sensors/light_sensor.cc @@ -5,7 +5,9 @@ #include "common/timing.h" #include "selfdrive/sensord/sensors/constants.h" -void LightSensor::get_event(cereal::SensorEventData::Builder &event) { +LightSensor::LightSensor(std::string filename) : FileSensor(filename) {} + +bool LightSensor::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); file.clear(); file.seekg(0); @@ -13,10 +15,13 @@ void LightSensor::get_event(cereal::SensorEventData::Builder &event) { int value; file >> value; + auto event = msg.initEvent().initLightSensor(); event.setSource(cereal::SensorEventData::SensorSource::RPR0521); event.setVersion(1); event.setSensor(SENSOR_LIGHT); event.setType(SENSOR_TYPE_LIGHT); event.setTimestamp(start_time); event.setLight(value); + + return true; } diff --git a/selfdrive/sensord/sensors/light_sensor.h b/selfdrive/sensord/sensors/light_sensor.h index faf901d41..7ed1c1f70 100644 --- a/selfdrive/sensord/sensors/light_sensor.h +++ b/selfdrive/sensord/sensors/light_sensor.h @@ -3,6 +3,7 @@ class LightSensor : public FileSensor { public: - LightSensor(std::string filename) : FileSensor(filename){}; - void get_event(cereal::SensorEventData::Builder &event); + LightSensor(std::string filename); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index bcac7de9f..c19e3de7e 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -1,15 +1,132 @@ #include "lsm6ds3_accel.h" #include +#include +#include #include "common/swaglog.h" #include "common/timing.h" +#include "common/util.h" -LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus) : I2CSensor(bus) {} +LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} + +void LSM6DS3_Accel::wait_for_data_ready() { + uint8_t drdy = 0; + uint8_t buffer[6]; + + do { + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_ACCEL_DRDY_XLDA; + } while (drdy == 0); + + read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); +} + +void LSM6DS3_Accel::read_and_avg_data(float* out_buf) { + uint8_t drdy = 0; + uint8_t buffer[6]; + + float scaling = 0.061f; + if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { + scaling = 0.122f; + } + + for (int i = 0; i < 5; i++) { + do { + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_ACCEL_DRDY_XLDA; + } while (drdy == 0); + + int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + for (int j = 0; j < 3; j++) { + out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling; + } + } + + for (int i = 0; i < 3; i++) { + out_buf[i] /= 5.0f; + } +} + +int LSM6DS3_Accel::self_test(int test_type) { + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + float test_val[3] = {0}; + uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz + + // prepare sensor for self-test + + // enable block data update and automatic increment + int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU); + if (ret < 0) { + return ret; + } + + if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { + ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ; + } + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(100); + wait_for_data_ready(); + read_and_avg_data(val_st_off); + + // enable Self Test positive (or negative) + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(100); + wait_for_data_ready(); + read_and_avg_data(val_st_on); + + // disable sensor + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0); + if (ret < 0) { + return ret; + } + + // disable self test + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0); + if (ret < 0) { + return ret; + } + + // calculate the mg values for self test + for (int i = 0; i < 3; i++) { + test_val[i] = fabs(val_st_on[i] - val_st_off[i]); + } + + // verify test result + for (int i = 0; i < 3; i++) { + if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) || + (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) { + return -1; + } + } + + return ret; +} int LSM6DS3_Accel::init() { int ret = 0; uint8_t buffer[1]; + uint8_t value = 0; + bool do_self_test = false; + + const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST"); + if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { + do_self_test = true; + } ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1); if(ret < 0) { @@ -27,20 +144,98 @@ int LSM6DS3_Accel::init() { source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; } - // TODO: set scale and bandwith. Default is +- 2G, 50 Hz + ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 accel positive self-test failed!"); + if (do_self_test) goto fail; + } + + ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 accel negative self-test failed!"); + if (do_self_test) goto fail; + } + + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + + // enable continuous update, and automatic increase + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC); + if (ret < 0) { + goto fail; + } + + // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ); if (ret < 0) { goto fail; } + ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for accel on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_ACCEL_INT1_DRDY_XL; + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); fail: return ret; } -void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { +int LSM6DS3_Accel::shutdown() { + int ret = 0; + + // disable data ready interrupt for accel on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL); + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + LOGE("Could not disable lsm6ds3 acceleration interrupt!") + goto fail; + } + + // enable power-down mode + value = 0; + ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= 0x0F; + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value); + if (ret < 0) { + LOGE("Could not power-down lsm6ds3 accelerometer!") + goto fail; + } + +fail: + return ret; +} + +bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) { + + // INT1 shared with gyro, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) { + return false; + } - uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); assert(len == sizeof(buffer)); @@ -50,15 +245,17 @@ void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { float y = read_16_bit(buffer[2], buffer[3]) * scale; float z = read_16_bit(buffer[4], buffer[5]) * scale; + auto event = msg.initEvent().initAccelerometer(); event.setSource(source); event.setVersion(1); event.setSensor(SENSOR_ACCELEROMETER); event.setType(SENSOR_TYPE_ACCELEROMETER); - event.setTimestamp(start_time); + event.setTimestamp(ts); float xyz[] = {y, -x, z}; auto svec = event.initAcceleration(); svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.h b/selfdrive/sensord/sensors/lsm6ds3_accel.h index 4a6b68744..c3f66f580 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.h +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.h @@ -6,21 +6,44 @@ #define LSM6DS3_ACCEL_I2C_ADDR 0x6A // Registers of the chip +#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B #define LSM6DS3_ACCEL_I2C_REG_ID 0x0F +#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D #define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 +#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12 +#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14 +#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18 +#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E #define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28 // Constants -#define LSM6DS3_ACCEL_CHIP_ID 0x69 -#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A -#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) - +#define LSM6DS3_ACCEL_CHIP_ID 0x69 +#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A +#define LSM6DS3_ACCEL_FS_4G (0b10 << 2) +#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4) +#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1 +#define LSM6DS3_ACCEL_DRDY_XLDA 0b1 +#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7) +#define LSM6DS3_ACCEL_IF_INC 0b00000100 +#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100 +#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000 +#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01 +#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10 +#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f +#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f class LSM6DS3_Accel : public I2CSensor { uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + + // self test functions + int self_test(int test_type); + void wait_for_data_ready(); + void read_and_avg_data(float* val_st_off); public: - LSM6DS3_Accel(I2CBus *bus); + LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index c7711d34e..f306be0fe 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -2,18 +2,120 @@ #include #include +#include #include "common/swaglog.h" #include "common/timing.h" +#include "common/util.h" #define DEG2RAD(x) ((x) * M_PI / 180.0) +LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} -LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus) : I2CSensor(bus) {} +void LSM6DS3_Gyro::wait_for_data_ready() { + uint8_t drdy = 0; + uint8_t buffer[6]; + + do { + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_GYRO_DRDY_GDA; + } while (drdy == 0); + + read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); +} + +void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) { + uint8_t drdy = 0; + uint8_t buffer[6]; + + for (int i = 0; i < 5; i++) { + do { + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_GYRO_DRDY_GDA; + } while (drdy == 0); + + int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + for (int j = 0; j < 3; j++) { + out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f; + } + } + + // calculate the mg average values + for (int i = 0; i < 3; i++) { + out_buf[i] /= 5.0f; + } +} + +int LSM6DS3_Gyro::self_test(int test_type) { + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + float test_val[3] = {0}; + + // prepare sensor for self-test + + // full scale: 2000dps, ODR: 208Hz + int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(150); + wait_for_data_ready(); + read_and_avg_data(val_st_off); + + // enable Self Test positive (or negative) + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(50); + wait_for_data_ready(); + read_and_avg_data(val_st_on); + + // disable sensor + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0); + if (ret < 0) { + return ret; + } + + // disable self test + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0); + if (ret < 0) { + return ret; + } + + // calculate the mg values for self test + for (int i = 0; i < 3; i++) { + test_val[i] = fabs(val_st_on[i] - val_st_off[i]); + } + + // verify test result + for (int i = 0; i < 3; i++) { + if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) || + (test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) { + return -1; + } + } + + return ret; +} int LSM6DS3_Gyro::init() { int ret = 0; uint8_t buffer[1]; + uint8_t value = 0; + bool do_self_test = false; + + const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST"); + if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { + do_self_test = true; + } ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1); if(ret < 0) { @@ -31,20 +133,92 @@ int LSM6DS3_Gyro::init() { source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; } + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + + ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST); + if (ret < 0 ) { + LOGE("LSM6DS3 gyro positive self-test failed!"); + if (do_self_test) goto fail; + } + + ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 gyro negative self-test failed!"); + if (do_self_test) goto fail; + } + // TODO: set scale. Default is +- 250 deg/s ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ); if (ret < 0) { goto fail; } + ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for gyro on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_GYRO_INT1_DRDY_G; + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); fail: return ret; } -void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { +int LSM6DS3_Gyro::shutdown() { + int ret = 0; + + // disable data ready interrupt for gyro on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_GYRO_INT1_DRDY_G); + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + LOGE("Could not disable lsm6ds3 gyroscope interrupt!") + goto fail; + } + + // enable power-down mode + value = 0; + ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= 0x0F; + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value); + if (ret < 0) { + LOGE("Could not power-down lsm6ds3 gyroscope!") + goto fail; + } + +fail: + return ret; +} + +bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { + + // INT1 shared with accel, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) { + return false; + } - uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); assert(len == sizeof(buffer)); @@ -54,15 +228,17 @@ void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + auto event = msg.initEvent().initGyroscope(); event.setSource(source); event.setVersion(2); event.setSensor(SENSOR_GYRO_UNCALIBRATED); event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); - event.setTimestamp(start_time); + event.setTimestamp(ts); float xyz[] = {y, -x, z}; auto svec = event.initGyroUncalibrated(); svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.h b/selfdrive/sensord/sensors/lsm6ds3_gyro.h index d7e8f0025..220e6b0ce 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.h +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.h @@ -6,21 +6,40 @@ #define LSM6DS3_GYRO_I2C_ADDR 0x6A // Registers of the chip +#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B #define LSM6DS3_GYRO_I2C_REG_ID 0x0F +#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D #define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 +#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14 +#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E #define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 +#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2) +#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2) // Constants -#define LSM6DS3_GYRO_CHIP_ID 0x69 -#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A -#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_GYRO_CHIP_ID 0x69 +#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A +#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2) +#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4) +#define LSM6DS3_GYRO_INT1_DRDY_G 0b10 +#define LSM6DS3_GYRO_DRDY_GDA 0b10 +#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7) +#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f +#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f class LSM6DS3_Gyro : public I2CSensor { uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + + // self test functions + int self_test(int test_type); + void wait_for_data_ready(); + void read_and_avg_data(float* val_st_off); public: - LSM6DS3_Gyro(I2CBus *bus); + LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc index 1dd179d69..7e1c4d4c8 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -31,8 +31,7 @@ fail: return ret; } -void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { - +bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[2]; int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer)); @@ -41,10 +40,12 @@ void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f; float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale; + auto event = msg.initEvent().initTemperatureSensor(); event.setSource(source); event.setVersion(1); event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); event.setTimestamp(start_time); event.setTemperature(temp); + return true; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.h b/selfdrive/sensord/sensors/lsm6ds3_temp.h index 8188f4670..1d6bcc228 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.h +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.h @@ -21,5 +21,6 @@ class LSM6DS3_Temp : public I2CSensor { public: LSM6DS3_Temp(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/selfdrive/sensord/sensors/mmc5603nj_magn.cc index 7c654ce7a..7a9b7a298 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.cc +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.cc @@ -51,8 +51,35 @@ fail: return ret; } -void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { +int MMC5603NJ_Magn::shutdown() { + int ret = 0; + // disable auto reset of measurements + uint8_t value = 0; + ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN); + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value); + if (ret < 0) { + goto fail; + } + + // set ODR to 0 to leave continuous mode + ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); + if (ret < 0) { + goto fail; + } + return ret; + +fail: + LOGE("Could not disable mmc5603nj auto set reset") + return ret; +} + +bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[9]; int len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer)); @@ -63,6 +90,7 @@ void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale; float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale; + auto event = msg.initEvent().initMagnetometer(); event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ); event.setVersion(1); event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); @@ -74,4 +102,5 @@ void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.h b/selfdrive/sensord/sensors/mmc5603nj_magn.h index 58840bbf2..a364c7c37 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.h +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.h @@ -25,5 +25,6 @@ class MMC5603NJ_Magn : public I2CSensor { public: MMC5603NJ_Magn(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/sensor.h b/selfdrive/sensord/sensors/sensor.h index 3fb58ad2a..603aa3586 100644 --- a/selfdrive/sensord/sensors/sensor.h +++ b/selfdrive/sensord/sensors/sensor.h @@ -1,10 +1,18 @@ #pragma once -#include "cereal/gen/cpp/log.capnp.h" +#include "cereal/messaging/messaging.h" class Sensor { public: + int gpio_fd = -1; + uint64_t init_delay = 500e6; // default dealy 500ms virtual ~Sensor() {}; virtual int init() = 0; - virtual void get_event(cereal::SensorEventData::Builder &event) = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; + virtual bool has_interrupt_enabled() = 0; + virtual int shutdown() = 0; + + virtual bool is_data_valid(uint64_t st, uint64_t ct) { + return (ct - st) > init_delay; + } }; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 65fe43f65..2279cf253 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -3,6 +3,9 @@ #include #include #include +#include +#include +#include #include "cereal/messaging/messaging.h" #include "common/i2c.h" @@ -24,30 +27,96 @@ #define I2C_BUS_IMU 1 ExitHandler do_exit; +std::mutex pm_mutex; +uint64_t init_ts = 0; -int sensor_loop() { - I2CBus *i2c_bus_imu; +void interrupt_loop(std::vector& sensors, + std::map& sensor_service) +{ + PubMaster pm_int({"gyroscope", "accelerometer"}); - try { - i2c_bus_imu = new I2CBus(I2C_BUS_IMU); - } catch (std::exception &e) { - LOGE("I2CBus init failed"); - return -1; + int fd = sensors[0]->gpio_fd; + struct pollfd fd_list[1] = {0}; + fd_list[0].fd = fd; + fd_list[0].events = POLLIN | POLLPRI; + + while (!do_exit) { + int err = poll(fd_list, 1, 100); + if (err == -1) { + if (errno == EINTR) { + continue; + } + return; + } else if (err == 0) { + LOGE("poll timed out"); + continue; + } + + if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) { + LOGE("no poll events set"); + continue; + } + + // Read all events + struct gpioevent_data evdata[16]; + err = read(fd, evdata, sizeof(evdata)); + if (err < 0 || err % sizeof(*evdata) != 0) { + LOGE("error reading event data %d", err); + continue; + } + + int num_events = err / sizeof(*evdata); + uint64_t offset = nanos_since_epoch() - nanos_since_boot(); + uint64_t ts = evdata[num_events - 1].timestamp - offset; + + for (Sensor *sensor : sensors) { + MessageBuilder msg; + if (!sensor->get_event(msg, ts)) { + continue; + } + + if (!sensor->is_data_valid(init_ts, ts)) { + continue; + } + + pm_int.send(sensor_service[sensor].c_str(), msg); + } } + // poweroff sensors, disable interrupts + for (Sensor *sensor : sensors) { + sensor->shutdown(); + } +} + +int sensor_loop(I2CBus *i2c_bus_imu) { BMX055_Accel bmx055_accel(i2c_bus_imu); BMX055_Gyro bmx055_gyro(i2c_bus_imu); BMX055_Magn bmx055_magn(i2c_bus_imu); BMX055_Temp bmx055_temp(i2c_bus_imu); - LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu); - LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu); + LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu, GPIO_LSM_INT); + LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu, GPIO_LSM_INT, true); // GPIO shared with accel LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu); MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu); LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw"); + std::map sensor_service = { + {&bmx055_accel, "accelerometer2"}, + {&bmx055_gyro, "gyroscope2"}, + {&bmx055_magn, "magnetometer"}, + {&bmx055_temp, "temperatureSensor"}, + + {&lsm6ds3_accel, "accelerometer"}, + {&lsm6ds3_gyro, "gyroscope"}, + {&lsm6ds3_temp, "temperatureSensor"}, + + {&mmc5603nj_magn, "magnetometer"}, + {&light, "lightSensor"} + }; + // Sensor init std::vector> sensors_init; // Sensor, required sensors_init.push_back({&bmx055_accel, false}); @@ -67,19 +136,22 @@ int sensor_loop() { // Initialize sensors std::vector sensors; - for (auto &sensor : sensors_init) { - int err = sensor.first->init(); + for (auto &[sensor, required] : sensors_init) { + int err = sensor->init(); if (err < 0) { - // Fail on required sensors - if (sensor.second) { + if (required) { LOGE("Error initializing sensors"); return -1; } } else { - if (sensor.first == &bmx055_magn || sensor.first == &mmc5603nj_magn) { + + if (sensor == &bmx055_magn || sensor == &mmc5603nj_magn) { has_magnetometer = true; } - sensors.push_back(sensor.first); + + if (!sensor->has_interrupt_enabled()) { + sensors.push_back(sensor); + } } } @@ -88,29 +160,55 @@ int sensor_loop() { return -1; } - PubMaster pm({"sensorEvents"}); + // increase interrupt quality by pinning interrupt and process to core 1 + setpriority(PRIO_PROCESS, 0, -18); + util::set_core_affinity({1}); + std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'"); + PubMaster pm_non_int({"gyroscope2", "accelerometer2", "temperatureSensor", + "lightSensor", "magnetometer"}); + init_ts = nanos_since_boot(); + + // thread for reading events via interrupts + std::vector lsm_interrupt_sensors = {&lsm6ds3_accel, &lsm6ds3_gyro}; + std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors), + std::ref(sensor_service)); + + // polling loop for non interrupt handled sensors while (!do_exit) { std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); - const int num_events = sensors.size(); - MessageBuilder msg; - auto sensor_events = msg.initEvent().initSensorEvents(num_events); + for (Sensor *sensor : sensors) { + MessageBuilder msg; + if (!sensor->get_event(msg)) { + continue; + } - for (int i = 0; i < num_events; i++) { - auto event = sensor_events[i]; - sensors[i]->get_event(event); + if (!sensor->is_data_valid(init_ts, nanos_since_boot())) { + continue; + } + + pm_non_int.send(sensor_service[sensor].c_str(), msg); } - pm.send("sensorEvents", msg); - std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin)); } + + for (Sensor *sensor : sensors) { + sensor->shutdown(); + } + + lsm_interrupt_thread.join(); return 0; } int main(int argc, char *argv[]) { - setpriority(PRIO_PROCESS, 0, -18); - return sensor_loop(); + try { + auto i2c_bus_imu = std::make_unique(I2C_BUS_IMU); + return sensor_loop(i2c_bus_imu.get()); + } catch (std::exception &e) { + LOGE("I2CBus init failed"); + return -1; + } } diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index bf2f93e1c..02c7d7663 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -59,9 +59,10 @@ git fetch --verbose origin $GIT_COMMIT find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; git reset --hard $GIT_COMMIT git checkout $GIT_COMMIT -git clean -xdf +git clean -xdff +git submodule sync git submodule update --init --recursive -git submodule foreach --recursive "git reset --hard && git clean -xdf" +git submodule foreach --recursive "git reset --hard && git clean -xdff" git lfs pull (ulimit -n 65535 && git lfs prune) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 93598a1b0..4021e27de 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -21,21 +21,22 @@ from tools.lib.logreader import LogReader # Baseline CPU usage by process PROCS = { - "selfdrive.controls.controlsd": 35.0, + "selfdrive.controls.controlsd": 39.0, "./loggerd": 10.0, - "./encoderd": 12.5, + "./encoderd": 17.0, "./camerad": 14.5, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, "./_ui": 19.2, "selfdrive.locationd.paramsd": 9.0, - "./_sensord": 6.17, + "./_sensord": 12.0, "selfdrive.controls.radard": 4.5, "./_modeld": 4.48, "./boardd": 3.63, "./_dmonitoringmodeld": 5.0, "selfdrive.thermald.thermald": 3.87, "selfdrive.locationd.calibrationd": 2.0, + "selfdrive.locationd.torqued": 5.0, "./_soundd": 1.0, "selfdrive.monitoring.dmonitoringd": 4.0, "./proclogd": 1.54, @@ -119,8 +120,8 @@ 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[-1]) - cls.lr = list(LogReader(os.path.join(segs[-1], "rlog"))) + print(segs[-2]) + cls.lr = list(LogReader(os.path.join(segs[-2], "rlog"))) return # setup env @@ -186,6 +187,25 @@ class TestOnroad(unittest.TestCase): big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.] self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}") + def test_ui_timings(self): + result = "\n" + result += "------------------------------------------------\n" + result += "-------------- UI Draw Timing ------------------\n" + result += "------------------------------------------------\n" + + ts = [m.uiDebug.drawTimeMillis for m in self.lr if m.which() == '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" + result += f"mean {np.mean(ts):.2f}ms\n" + 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") diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 9f009d326..783456908 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -108,32 +108,19 @@ class PowerMonitoring: def get_car_battery_capacity(self) -> int: return int(self.car_battery_capacity_uWh) - # See if we need to disable charging - def should_disable_charging(self, ignition: bool, in_car: bool, offroad_timestamp: Optional[float]) -> bool: - if offroad_timestamp is None: - return False - - now = sec_since_boot() - disable_charging = False - disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S - disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) and (self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3)) - disable_charging |= (self.car_battery_capacity_uWh <= 0) - disable_charging &= not ignition - disable_charging &= (not self.params.get_bool("DisablePowerDown")) - disable_charging &= in_car - disable_charging |= self.params.get_bool("ForcePowerDown") - return disable_charging - # See if we need to shutdown - def should_shutdown(self, peripheralState, ignition, in_car, offroad_timestamp, started_seen): + def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: Optional[float], started_seen: bool): if offroad_timestamp is None: return False now = sec_since_boot() - panda_charging = (peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client) - should_shutdown = False - # Wait until we have shut down charging before powering down - should_shutdown |= (not panda_charging and self.should_disable_charging(ignition, in_car, offroad_timestamp)) + should_shutdown |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S + should_shutdown |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) and (self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3)) + should_shutdown |= (self.car_battery_capacity_uWh <= 0) + should_shutdown &= not ignition + should_shutdown &= (not self.params.get_bool("DisablePowerDown")) + should_shutdown &= in_car + 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 c765af664..89b81f06e 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -177,8 +177,8 @@ def thermald_thread(end_event, hw_queue): modem_temps=[], ) - current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) - temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) + all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) + offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) should_start_prev = False in_car = False engaged_prev = False @@ -239,27 +239,33 @@ def thermald_thread(end_event, hw_queue): msg.deviceState.modemTempC = last_hw_state.modem_temps msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() - msg.deviceState.usbOnline = HARDWARE.get_usb_present() - current_filter.update(msg.deviceState.batteryCurrent / 1e6) - max_comp_temp = temp_filter.update( - max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) - ) + # this one is only used for offroad + temp_sources = [ + msg.deviceState.memoryTempC, + max(msg.deviceState.cpuTempC), + max(msg.deviceState.gpuTempC), + ] + offroad_comp_temp = offroad_temp_filter.update(max(temp_sources)) + + # this drives the thermal status while onroad + temp_sources.append(max(msg.deviceState.pmicTempC)) + all_comp_temp = all_temp_filter.update(max(temp_sources)) if fan_controller is not None: - msg.deviceState.fanSpeedPercentDesired = fan_controller.update(max_comp_temp, onroad_conditions["ignition"]) + msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"]) is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) - if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: + if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP: # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 thermal_status = ThermalStatus.danger else: current_band = THERMAL_BANDS[thermal_status] band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) - if current_band.min_temp is not None and max_comp_temp < current_band.min_temp: + if current_band.min_temp is not None and all_comp_temp < current_band.min_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] - elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp: + elif current_band.max_temp is not None and all_comp_temp > current_band.max_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] # **** starting logic **** @@ -272,6 +278,7 @@ def thermald_thread(end_event, hw_queue): startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version + startup_conditions["offroad_min_time"] = (not started_seen) or ((off_ts is not None) and (sec_since_boot() - off_ts) > 5.) # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 @@ -332,7 +339,8 @@ def thermald_thread(end_event, hw_queue): started_seen = True else: if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev): - cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions) + cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True) + startup_conditions_prev = startup_conditions.copy() started_ts = None if off_ts is None: @@ -350,15 +358,11 @@ def thermald_thread(end_event, hw_queue): statlog.sample("som_power_draw", som_power_draw) msg.deviceState.somPowerDrawW = som_power_draw - # Check if we need to disable charging (handled by boardd) - msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(onroad_conditions["ignition"], in_car, off_ts) - # Check if we need to shut down - if power_monitor.should_shutdown(peripheralState, onroad_conditions["ignition"], in_car, off_ts, started_seen): + if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, off_ts, started_seen): cloudlog.warning(f"shutting device down, offroad since {off_ts}") params.put_bool("DoShutdown", True) - msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged msg.deviceState.started = started_ts is not None msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0)) @@ -370,7 +374,6 @@ def thermald_thread(end_event, hw_queue): pm.send("deviceState", msg) should_start_prev = should_start - startup_conditions_prev = startup_conditions.copy() # Log to statsd statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 0045e0766..61a575f14 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -62,47 +62,6 @@ def get_tombstones(): return files -def report_tombstone_android(fn): - f_size = os.path.getsize(fn) - if f_size > MAX_SIZE: - cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") - return - - with open(fn, encoding='ISO-8859-1') as f: - contents = f.read() - - message = " ".join(contents.split('\n')[5:7]) - - # Cut off pid/tid, since that varies per run - name_idx = message.find('name') - if name_idx >= 0: - message = message[name_idx:] - - executable = "" - start_exe_idx = message.find('>>> ') - end_exe_idx = message.find(' <<<') - if start_exe_idx >= 0 and end_exe_idx >= 0: - executable = message[start_exe_idx + 4:end_exe_idx] - - # Cut off fault addr - fault_idx = message.find(', fault addr') - if fault_idx >= 0: - message = message[:fault_idx] - - sentry.report_tombstone(fn, message, contents) - - # Copy crashlog to upload folder - clean_path = executable.replace('./', '').replace('/', '_') - date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") - - new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] - - crashlog_dir = os.path.join(ROOT, "crash") - mkdirs_exists_ok(crashlog_dir) - - shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) - - def report_tombstone_apport(fn): f_size = os.path.getsize(fn) if f_size > MAX_SIZE: @@ -199,7 +158,7 @@ def main() -> NoReturn: if fn.endswith(".crash"): report_tombstone_apport(fn) else: - report_tombstone_android(fn) + cloudlog.error(f"unknown crash type: {fn}") except Exception: cloudlog.exception(f"Error reporting tombstone {fn}") diff --git a/selfdrive/ui/.gitignore b/selfdrive/ui/.gitignore index 5f8a96edf..60eb4b43c 100644 --- a/selfdrive/ui/.gitignore +++ b/selfdrive/ui/.gitignore @@ -1,11 +1,11 @@ moc_* *.moc +translations/main_test_en.* + _mui watch3 installer/installers/* -replay/replay -replay/tests/test_replay qt/text qt/spinner qt/setup/setup diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 9e89576a5..669c21474 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -32,6 +32,7 @@ if maps: qt_env['CPPDEFINES'] += ["ENABLE_MAPS"] widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) +Export('widgets') qt_libs = [widgets, qt_util] + base_libs # build assets @@ -56,7 +57,8 @@ qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs) # build main UI qt_src = ["main.cc", "qt/sidebar.cc", "qt/onroad.cc", "qt/body.cc", "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", - "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc"] + "qt/offroad/software_settings.cc", "qt/offroad/onboarding.cc", + "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc"] qt_env.Program("_ui", qt_src + [asset_obj], LIBS=qt_libs) if GetOption('test'): qt_src.remove("main.cc") # replaced by test_runner @@ -78,7 +80,7 @@ if GetOption('extras'): if GetOption('extras'): - # buidl updater UI + # build updater UI qt_env.Program("qt/setup/updater", ["qt/setup/updater.cc", asset_obj], LIBS=qt_libs) # build mui diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index 0edeb252b..3f3c9a588 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -1,11 +1,10 @@ #include "selfdrive/ui/qt/home.h" -#include #include #include #include -#include "common/params.h" +#include "selfdrive/ui/qt/offroad/experimental_mode.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/widgets/drive_stats.h" #include "selfdrive/ui/qt/widgets/prime.h" @@ -24,7 +23,8 @@ HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { slayout = new QStackedLayout(); main_layout->addLayout(slayout); - home = new OffroadHome(); + home = new OffroadHome(this); + QObject::connect(home, &OffroadHome::openSettings, this, &HomeWindow::openSettings); slayout->addWidget(home); onroad = new OnroadWindow(this); @@ -41,6 +41,7 @@ HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { setAttribute(Qt::WA_NoSystemBackground); QObject::connect(uiState(), &UIState::uiUpdate, this, &HomeWindow::updateState); QObject::connect(uiState(), &UIState::offroadTransition, this, &HomeWindow::offroadTransition); + QObject::connect(uiState(), &UIState::offroadTransition, sidebar, &Sidebar::offroadTransition); } void HomeWindow::showSidebar(bool show) { @@ -108,22 +109,20 @@ OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) { header_layout->setContentsMargins(15, 15, 15, 0); header_layout->setSpacing(16); - date = new QLabel(); - header_layout->addWidget(date, 1, Qt::AlignHCenter | Qt::AlignLeft); - update_notif = new QPushButton(tr("UPDATE")); update_notif->setVisible(false); update_notif->setStyleSheet("background-color: #364DEF;"); QObject::connect(update_notif, &QPushButton::clicked, [=]() { center_layout->setCurrentIndex(1); }); - header_layout->addWidget(update_notif, 0, Qt::AlignHCenter | Qt::AlignRight); + header_layout->addWidget(update_notif, 0, Qt::AlignHCenter | Qt::AlignLeft); alert_notif = new QPushButton(); alert_notif->setVisible(false); alert_notif->setStyleSheet("background-color: #E22C2C;"); QObject::connect(alert_notif, &QPushButton::clicked, [=] { center_layout->setCurrentIndex(2); }); - header_layout->addWidget(alert_notif, 0, Qt::AlignHCenter | Qt::AlignRight); + header_layout->addWidget(alert_notif, 0, Qt::AlignHCenter | Qt::AlignLeft); - header_layout->addWidget(new QLabel(getBrandVersion()), 0, Qt::AlignHCenter | Qt::AlignRight); + version = new ElidedLabel(); + header_layout->addWidget(version, 0, Qt::AlignHCenter | Qt::AlignRight); main_layout->addLayout(header_layout); @@ -131,11 +130,24 @@ OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) { main_layout->addSpacing(25); center_layout = new QStackedLayout(); + // Vertical experimental button and drive stats layout + QWidget* statsAndExperimentalModeButtonWidget = new QWidget(this); + QVBoxLayout* statsAndExperimentalModeButton = new QVBoxLayout(statsAndExperimentalModeButtonWidget); + statsAndExperimentalModeButton->setSpacing(30); + statsAndExperimentalModeButton->setMargin(0); + + ExperimentalModeButton *experimental_mode = new ExperimentalModeButton(this); + QObject::connect(experimental_mode, &ExperimentalModeButton::openSettings, this, &OffroadHome::openSettings); + + statsAndExperimentalModeButton->addWidget(experimental_mode, 1); + statsAndExperimentalModeButton->addWidget(new DriveStats, 1); + + // Horizontal experimental + drive stats and setup widget QWidget* statsAndSetupWidget = new QWidget(this); QHBoxLayout* statsAndSetup = new QHBoxLayout(statsAndSetupWidget); statsAndSetup->setMargin(0); statsAndSetup->setSpacing(30); - statsAndSetup->addWidget(new DriveStats, 1); + statsAndSetup->addWidget(statsAndExperimentalModeButtonWidget, 1); statsAndSetup->addWidget(new SetupWidget); center_layout->addWidget(statsAndSetupWidget); @@ -183,7 +195,7 @@ void OffroadHome::hideEvent(QHideEvent *event) { } void OffroadHome::refresh() { - date->setText(QDateTime::currentDateTime().toString("dddd, MMMM d")); + version->setText(getBrand() + " " + QString::fromStdString(params.get("UpdaterCurrentDescription"))); bool updateAvailable = update_widget->refresh(); int alerts = alerts_widget->refresh(); diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index 94f133010..ed1c21546 100644 --- a/selfdrive/ui/qt/home.h +++ b/selfdrive/ui/qt/home.h @@ -7,10 +7,12 @@ #include #include +#include "common/params.h" #include "selfdrive/ui/qt/offroad/driverview.h" #include "selfdrive/ui/qt/body.h" #include "selfdrive/ui/qt/onroad.h" #include "selfdrive/ui/qt/sidebar.h" +#include "selfdrive/ui/qt/widgets/controls.h" #include "selfdrive/ui/qt/widgets/offroad_alerts.h" #include "selfdrive/ui/ui.h" @@ -20,13 +22,18 @@ class OffroadHome : public QFrame { public: explicit OffroadHome(QWidget* parent = 0); +signals: + void openSettings(int index = 0, const QString ¶m = ""); + private: void showEvent(QShowEvent *event) override; void hideEvent(QHideEvent *event) override; void refresh(); + Params params; + QTimer* timer; - QLabel* date; + ElidedLabel* version; QStackedLayout* center_layout; UpdateAlert *update_widget; OffroadAlert* alerts_widget; @@ -41,7 +48,7 @@ public: explicit HomeWindow(QWidget* parent = 0); signals: - void openSettings(); + void openSettings(int index = 0, const QString ¶m = ""); void closeSettings(); public slots: diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 046814d51..b5519dacc 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -204,7 +204,8 @@ void MapWindow::updateState(const UIState &s) { if (zoom_counter == 0) { m_map->setZoom(util::map_val(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM)); - } else { + zoom_counter = -1; + } else if (zoom_counter > 0) { zoom_counter--; } @@ -486,9 +487,9 @@ void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruct // for rhd, reflect direction and then flip if (is_rhd) { if (fn.contains("left")) { - fn.replace(QString("left"), QString("right")); + fn.replace("left", "right"); } else if (fn.contains("right")) { - fn.replace(QString("right"), QString("left")); + fn.replace("right", "left"); } } diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index ecba867ed..c3d5e9253 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -98,7 +98,7 @@ private: // Panning QPointF m_lastPos; int pan_counter = 0; - int zoom_counter = 0; + int zoom_counter = -1; // Position std::optional last_position; diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index f97137a7f..8d5d4e171 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -44,61 +44,31 @@ QMapbox::CoordinatesCollections model_to_collection( for (int i = 0; i < x.size(); i++) { Eigen::Vector3d point_ecef = ecef_from_local * Eigen::Vector3d(x[i], y[i], z[i]) + ecef; Geodetic point_geodetic = ecef2geodetic((ECEF){.x = point_ecef[0], .y = point_ecef[1], .z = point_ecef[2]}); - QMapbox::Coordinate coordinate(point_geodetic.lat, point_geodetic.lon); - coordinates.push_back(coordinate); + coordinates.push_back({point_geodetic.lat, point_geodetic.lon}); } - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; + return {QMapbox::CoordinatesCollection{coordinates}}; } -QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c) { - QMapbox::Coordinates coordinates; - coordinates.push_back(c); - - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; +QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c) { + QMapbox::Coordinates coordinates{c}; + return {QMapbox::CoordinatesCollection{coordinates}}; } QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader& coordinate_list) { QMapbox::Coordinates coordinates; - for (auto const &c: coordinate_list) { - QMapbox::Coordinate coordinate(c.getLatitude(), c.getLongitude()); - coordinates.push_back(coordinate); + coordinates.push_back({c.getLatitude(), c.getLongitude()}); } - - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; - + return {QMapbox::CoordinatesCollection{coordinates}}; } -QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list) { +QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList &coordinate_list) { QMapbox::Coordinates coordinates; - for (auto &c : coordinate_list) { - QMapbox::Coordinate coordinate(c.latitude(), c.longitude()); - coordinates.push_back(coordinate); + coordinates.push_back({c.latitude(), c.longitude()}); } - - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; + return {QMapbox::CoordinatesCollection{coordinates}}; } QList polyline_to_coordinate_list(const QString &polylineString) { @@ -143,7 +113,7 @@ QList polyline_to_coordinate_list(const QString &polylineString) return path; } -std::optional coordinate_from_param(std::string param) { +std::optional coordinate_from_param(const std::string ¶m) { QString json_str = QString::fromStdString(Params().get(param)); if (json_str.isEmpty()) return {}; diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index 2e1402cce..6bd5b0f06 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -21,10 +21,10 @@ QMapbox::CoordinatesCollections model_to_collection( const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF, const cereal::LiveLocationKalman::Measurement::Reader &positionECEF, const cereal::ModelDataV2::XYZTData::Reader &line); -QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c); +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(QList coordinate_list); +QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList &coordinate_list); QList polyline_to_coordinate_list(const QString &polylineString); -std::optional coordinate_from_param(std::string param); +std::optional coordinate_from_param(const std::string ¶m); double angle_difference(double angle1, double angle2); diff --git a/selfdrive/ui/qt/maps/map_settings.cc b/selfdrive/ui/qt/maps/map_settings.cc index d143b44e7..3205ca517 100644 --- a/selfdrive/ui/qt/maps/map_settings.cc +++ b/selfdrive/ui/qt/maps/map_settings.cc @@ -115,7 +115,9 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) { stack->addWidget(main_widget); stack->addWidget(no_prime_widget); - stack->setCurrentIndex(uiState()->prime_type ? 0 : 1); + connect(uiState(), &UIState::primeTypeChanged, [=](int prime_type) { + stack->setCurrentIndex(prime_type ? 0 : 1); + }); QVBoxLayout *wrapper = new QVBoxLayout(this); wrapper->addWidget(stack); @@ -156,6 +158,7 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) { void MapPanel::showEvent(QShowEvent *event) { updateCurrentRoute(); + refresh(); } void MapPanel::clear() { @@ -184,18 +187,24 @@ void MapPanel::updateCurrentRoute() { } void MapPanel::parseResponse(const QString &response, bool success) { - stack->setCurrentIndex((uiState()->prime_type || success) ? 0 : 1); + if (!success) return; - if (!success) { - return; + cur_destinations = response; + if (isVisible()) { + refresh(); } +} - QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8()); +void MapPanel::refresh() { + if (cur_destinations == prev_destinations) return; + + QJsonDocument doc = QJsonDocument::fromJson(cur_destinations.trimmed().toUtf8()); if (doc.isNull()) { qDebug() << "JSON Parse failed on navigation locations"; return; } + prev_destinations = cur_destinations; clear(); bool has_recents = false; diff --git a/selfdrive/ui/qt/maps/map_settings.h b/selfdrive/ui/qt/maps/map_settings.h index 962d12767..165673b7c 100644 --- a/selfdrive/ui/qt/maps/map_settings.h +++ b/selfdrive/ui/qt/maps/map_settings.h @@ -23,8 +23,10 @@ public: private: void showEvent(QShowEvent *event) override; + void refresh(); Params params; + QString prev_destinations, cur_destinations; QStackedWidget *stack; QPushButton *home_button, *work_button; QLabel *home_address, *work_address; diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index be8b84d45..1377bb3b2 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -12,11 +12,11 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { layout = new QStackedLayout(this); layout->setStackingMode(QStackedLayout::StackAll); - cameraView = new CameraViewWidget("camerad", VISION_STREAM_DRIVER, true, this); + cameraView = new CameraWidget("camerad", VISION_STREAM_DRIVER, true, this); layout->addWidget(cameraView); scene = new DriverViewScene(this); - connect(cameraView, &CameraViewWidget::vipcThreadFrameReceived, scene, &DriverViewScene::frameUpdated); + connect(cameraView, &CameraWidget::vipcThreadFrameReceived, scene, &DriverViewScene::frameUpdated); layout->addWidget(scene); layout->setCurrentWidget(scene); } @@ -35,6 +35,7 @@ void DriverViewScene::showEvent(QShowEvent* event) { } void DriverViewScene::hideEvent(QHideEvent* event) { + // TODO: stop vipc thread ? params.putBool("IsDriverViewEnabled", false); } diff --git a/selfdrive/ui/qt/offroad/driverview.h b/selfdrive/ui/qt/offroad/driverview.h index 5d090ad77..255857970 100644 --- a/selfdrive/ui/qt/offroad/driverview.h +++ b/selfdrive/ui/qt/offroad/driverview.h @@ -42,7 +42,7 @@ protected: void mouseReleaseEvent(QMouseEvent* e) override; private: - CameraViewWidget *cameraView; + CameraWidget *cameraView; DriverViewScene *scene; QStackedLayout *layout; }; diff --git a/selfdrive/ui/qt/offroad/experimental_mode.cc b/selfdrive/ui/qt/offroad/experimental_mode.cc new file mode 100644 index 000000000..f73149cdf --- /dev/null +++ b/selfdrive/ui/qt/offroad/experimental_mode.cc @@ -0,0 +1,75 @@ +#include "selfdrive/ui/qt/offroad/experimental_mode.h" + +#include +#include +#include +#include + +#include "selfdrive/ui/ui.h" + +ExperimentalModeButton::ExperimentalModeButton(QWidget *parent) : QPushButton(parent) { + chill_pixmap = QPixmap("../assets/img_couch.svg").scaledToWidth(img_width, Qt::SmoothTransformation); + experimental_pixmap = QPixmap("../assets/img_experimental_grey.svg").scaledToWidth(img_width, Qt::SmoothTransformation); + + // go to toggles and expand experimental mode description + connect(this, &QPushButton::clicked, [=]() { emit openSettings(2, "ExperimentalMode"); }); + + setFixedHeight(125); + QHBoxLayout *main_layout = new QHBoxLayout; + main_layout->setContentsMargins(horizontal_padding, 0, horizontal_padding, 0); + + mode_label = new QLabel; + mode_icon = new QLabel; + mode_icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + + main_layout->addWidget(mode_label, 1, Qt::AlignLeft); + main_layout->addWidget(mode_icon, 0, Qt::AlignRight); + + setLayout(main_layout); + + setStyleSheet(R"( + QPushButton { + border: none; + } + + QLabel { + font-size: 45px; + font-weight: 300; + text-align: left; + font-family: JetBrainsMono; + color: #000000; + } + )"); +} + +void ExperimentalModeButton::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setPen(Qt::NoPen); + p.setRenderHint(QPainter::Antialiasing); + + QPainterPath path; + path.addRoundedRect(rect(), 10, 10); + + // gradient + bool pressed = isDown(); + QLinearGradient gradient(rect().left(), 0, rect().right(), 0); + if (experimental_mode) { + gradient.setColorAt(0, QColor(255, 155, 63, pressed ? 0xcc : 0xff)); + gradient.setColorAt(1, QColor(219, 56, 34, pressed ? 0xcc : 0xff)); + } else { + gradient.setColorAt(0, QColor(20, 255, 171, pressed ? 0xcc : 0xff)); + gradient.setColorAt(1, QColor(35, 149, 255, pressed ? 0xcc : 0xff)); + } + p.fillPath(path, gradient); + + // vertical line + p.setPen(QPen(QColor(0, 0, 0, 0x4d), 3, Qt::SolidLine)); + int line_x = rect().right() - img_width - (2 * horizontal_padding); + p.drawLine(line_x, rect().bottom(), line_x, rect().top()); +} + +void ExperimentalModeButton::showEvent(QShowEvent *event) { + experimental_mode = params.getBool("ExperimentalMode"); + mode_icon->setPixmap(experimental_mode ? experimental_pixmap : chill_pixmap); + mode_label->setText(experimental_mode ? tr("EXPERIMENTAL MODE ON") : tr("CHILL MODE ON")); +} diff --git a/selfdrive/ui/qt/offroad/experimental_mode.h b/selfdrive/ui/qt/offroad/experimental_mode.h new file mode 100644 index 000000000..bfb7638bb --- /dev/null +++ b/selfdrive/ui/qt/offroad/experimental_mode.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +#include "common/params.h" + +class ExperimentalModeButton : public QPushButton { + Q_OBJECT + +public: + explicit ExperimentalModeButton(QWidget* parent = 0); + +signals: + void openSettings(int index = 0, const QString &toggle = ""); + +private: + void showEvent(QShowEvent *event) override; + + Params params; + bool experimental_mode; + int img_width = 100; + int horizontal_padding = 30; + QPixmap experimental_pixmap; + QPixmap chill_pixmap; + QLabel *mode_label; + QLabel *mode_icon; + +protected: + void paintEvent(QPaintEvent *event) override; +}; diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index c7341d198..d69d67ede 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -8,9 +8,11 @@ #include #include +#include "selfdrive/ui/ui.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/widgets/prime.h" #include "selfdrive/ui/qt/widgets/scrollview.h" @@ -150,17 +152,16 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid // Roaming toggle const bool roamingEnabled = params.getBool("GsmRoaming"); - ToggleControl *roamingToggle = new ToggleControl(tr("Enable Roaming"), "", "", roamingEnabled); - QObject::connect(roamingToggle, &SshToggle::toggleFlipped, [=](bool state) { + roamingToggle = new ToggleControl(tr("Enable Roaming"), "", "", roamingEnabled); + QObject::connect(roamingToggle, &ToggleControl::toggleFlipped, [=](bool state) { params.putBool("GsmRoaming", state); - wifi->updateGsmSettings(state, QString::fromStdString(params.get("GsmApn"))); + wifi->updateGsmSettings(state, QString::fromStdString(params.get("GsmApn")), params.getBool("GsmMetered")); }); list->addItem(roamingToggle); // APN settings - ButtonControl *editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT")); + editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT")); connect(editApnButton, &ButtonControl::clicked, [=]() { - const bool roamingEnabled = params.getBool("GsmRoaming"); const QString cur_apn = QString::fromStdString(params.get("GsmApn")); QString apn = InputDialog::getText(tr("Enter APN"), this, tr("leave blank for automatic configuration"), false, -1, cur_apn).trimmed(); @@ -169,12 +170,28 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid } else { params.put("GsmApn", apn.toStdString()); } - wifi->updateGsmSettings(roamingEnabled, apn); + wifi->updateGsmSettings(params.getBool("GsmRoaming"), apn, params.getBool("GsmMetered")); }); list->addItem(editApnButton); + // Metered toggle + const bool metered = params.getBool("GsmMetered"); + meteredToggle = new ToggleControl(tr("Cellular Metered"), tr("Prevent large data uploads when on a metered connection"), "", metered); + QObject::connect(meteredToggle, &SshToggle::toggleFlipped, [=](bool state) { + params.putBool("GsmMetered", state); + wifi->updateGsmSettings(params.getBool("GsmRoaming"), QString::fromStdString(params.get("GsmApn")), state); + }); + list->addItem(meteredToggle); + // Set initial config - wifi->updateGsmSettings(roamingEnabled, QString::fromStdString(params.get("GsmApn"))); + wifi->updateGsmSettings(roamingEnabled, QString::fromStdString(params.get("GsmApn")), metered); + + connect(uiState(), &UIState::primeTypeChanged, this, [=](int prime_type) { + bool gsmVisible = prime_type == PrimeType::NONE || prime_type == PrimeType::LITE; + roamingToggle->setVisible(gsmVisible); + editApnButton->setVisible(gsmVisible); + meteredToggle->setVisible(gsmVisible); + }); main_layout->addWidget(new ScrollView(list, this)); main_layout->addStretch(1); @@ -207,9 +224,12 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi) checkmark = QPixmap(ASSET_PATH + "offroad/icon_checkmark.svg").scaledToWidth(49, Qt::SmoothTransformation); circled_slash = QPixmap(ASSET_PATH + "img_circled_slash.svg").scaledToWidth(49, Qt::SmoothTransformation); - QLabel *scanning = new QLabel(tr("Scanning for networks...")); - scanning->setStyleSheet("font-size: 65px;"); - main_layout->addWidget(scanning, 0, Qt::AlignCenter); + scanningLabel = new QLabel(tr("Scanning for networks...")); + scanningLabel->setStyleSheet("font-size: 65px;"); + main_layout->addWidget(scanningLabel, 0, Qt::AlignCenter); + + list_layout = new QVBoxLayout; + main_layout->addLayout(list_layout); setStyleSheet(R"( QScrollBar::handle:vertical { @@ -257,14 +277,12 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi) void WifiUI::refresh() { // TODO: don't rebuild this every time - clearLayout(main_layout); + clearLayout(list_layout); + + bool is_empty = wifi->seenNetworks.isEmpty(); + scanningLabel->setVisible(is_empty); + if (is_empty) return; - if (wifi->seenNetworks.size() == 0) { - QLabel *scanning = new QLabel(tr("Scanning for networks...")); - scanning->setStyleSheet("font-size: 65px;"); - main_layout->addWidget(scanning, 0, Qt::AlignCenter); - return; - } QList sortedNetworks = wifi->seenNetworks.values(); std::sort(sortedNetworks.begin(), sortedNetworks.end(), compare_by_strength); @@ -296,7 +314,7 @@ void WifiUI::refresh() { QPushButton *forgetBtn = new QPushButton(tr("FORGET")); forgetBtn->setObjectName("forgetBtn"); QObject::connect(forgetBtn, &QPushButton::clicked, [=]() { - if (ConfirmationDialog::confirm(tr("Forget Wi-Fi Network \"%1\"?").arg(QString::fromUtf8(network.ssid)), this)) { + if (ConfirmationDialog::confirm(tr("Forget Wi-Fi Network \"%1\"?").arg(QString::fromUtf8(network.ssid)), tr("Forget"), this)) { wifi->forgetConnection(network.ssid); } }); @@ -327,6 +345,6 @@ void WifiUI::refresh() { list->addItem(hlayout); } - main_layout->addWidget(list); - main_layout->addStretch(1); + list_layout->addWidget(list); + list_layout->addStretch(1); } diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/offroad/networking.h index e78d65ef0..79cbcc349 100644 --- a/selfdrive/ui/qt/offroad/networking.h +++ b/selfdrive/ui/qt/offroad/networking.h @@ -17,6 +17,8 @@ public: private: WifiManager *wifi = nullptr; + QVBoxLayout *list_layout = nullptr; + QLabel *scanningLabel = nullptr; QVBoxLayout* main_layout; QPixmap lock; QPixmap checkmark; @@ -38,6 +40,9 @@ public: private: LabelControl* ipLabel; ToggleControl* tetheringToggle; + ToggleControl* roamingToggle; + ButtonControl* editApnButton; + ToggleControl* meteredToggle; WifiManager* wifi = nullptr; Params params; diff --git a/selfdrive/ui/qt/offroad/networkmanager.h b/selfdrive/ui/qt/offroad/networkmanager.h index 52d85c16a..31b33fc9f 100644 --- a/selfdrive/ui/qt/offroad/networkmanager.h +++ b/selfdrive/ui/qt/offroad/networkmanager.h @@ -36,3 +36,10 @@ const int NM_DEVICE_TYPE_WIFI = 2; const int NM_DEVICE_TYPE_MODEM = 8; const int NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT = 8; const int DBUS_TIMEOUT = 100; + +// https://developer-old.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NMMetered +const int NM_METERED_UNKNOWN = 0; +const int NM_METERED_YES = 1; +const int NM_METERED_NO = 2; +const int NM_METERED_GUESS_YES = 3; +const int NM_METERED_GUESS_NO = 4; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 5e0e87e64..01cb0ea72 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -27,14 +27,28 @@ #include "selfdrive/ui/qt/widgets/input.h" TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { - // param, title, desc, icon - std::vector> toggles{ + // param, title, desc, icon, confirm + std::vector> toggle_defs{ { "OpenpilotEnabledToggle", tr("Enable openpilot"), tr("Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off."), "../assets/offroad/icon_openpilot.png", }, + { + "ExperimentalMode", + tr("Experimental Mode"), + "", + "../assets/img_experimental_white.svg", + }, + { + "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.")), + "../assets/offroad/icon_speed_limit.png", + }, { "IsLdwEnabled", tr("Enable Lane Departure Warnings"), @@ -55,7 +69,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { }, { "DisengageOnAccelerator", - tr("Disengage On Accelerator Pedal"), + tr("Disengage on Accelerator Pedal"), tr("When enabled, pressing the accelerator pedal will disengage openpilot."), "../assets/offroad/icon_disengage_on_accelerator.svg", }, @@ -73,25 +87,83 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "../assets/offroad/icon_road.png", }, #endif - }; - Params params; - - if (params.getBool("DisableRadar_Allow")) { - toggles.push_back({ - "DisableRadar", - tr("openpilot Longitudinal Control"), - tr("openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB!"), - "../assets/offroad/icon_speed_limit.png", - }); - } - - for (auto &[param, title, desc, icon] : toggles) { + for (auto &[param, title, desc, icon] : toggle_defs) { auto toggle = new ParamControl(param, title, desc, icon, this); + bool locked = params.getBool((param + "Lock").toStdString()); toggle->setEnabled(!locked); + addItem(toggle); + toggles[param.toStdString()] = toggle; + } + + // Toggles with confirmation dialogs + toggles["ExperimentalMode"]->setActiveIcon("../assets/img_experimental.svg"); + toggles["ExperimentalMode"]->setConfirmation(true, true); + toggles["ExperimentalLongitudinalEnabled"]->setConfirmation(true, false); + + connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { + updateToggles(); + }); +} + +void TogglesPanel::expandToggleDescription(const QString ¶m) { + toggles[param.toStdString()]->showDescription(); +} + +void TogglesPanel::showEvent(QShowEvent *event) { + updateToggles(); +} + +void TogglesPanel::updateToggles() { + auto e2e_toggle = toggles["ExperimentalMode"]; + auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"]; + const QString e2e_description = tr("\ + openpilot defaults to driving in chill mode.\ + Experimental mode enables alpha-level features that aren't ready for chill mode. \ + Experimental features are listed below: \ +
\ +

🌮 End-to-End Longitudinal Control 🌮

\ + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. \ + Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. \ +
\ +

New Driving Visualization

\ + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.\ + "); + + auto cp_bytes = params.get("CarParamsPersistent"); + if (!cp_bytes.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + if (!CP.getExperimentalLongitudinalAvailable()) { + params.remove("ExperimentalLongitudinalEnabled"); + } + op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable()); + + const bool op_long = CP.getOpenpilotLongitudinalControl() && !CP.getExperimentalLongitudinalAvailable(); + const bool exp_long_enabled = CP.getExperimentalLongitudinalAvailable() && params.getBool("ExperimentalLongitudinalEnabled"); + if (op_long || exp_long_enabled) { + // normal description and toggle + e2e_toggle->setEnabled(true); + e2e_toggle->setDescription(e2e_description); + } else { + // no long for now + e2e_toggle->setEnabled(false); + params.remove("ExperimentalMode"); + + const QString no_long = tr("Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control."); + const QString exp_long = tr("Enable experimental longitudinal control to allow experimental mode."); + e2e_toggle->setDescription("" + (CP.getExperimentalLongitudinalAvailable() ? exp_long : no_long) + "

" + e2e_description); + } + + e2e_toggle->refresh(); + } else { + e2e_toggle->setDescription(e2e_description); + op_long_toggle->setVisible(false); } } @@ -108,9 +180,9 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { addItem(dcamBtn); auto resetCalibBtn = new ButtonControl(tr("Reset Calibration"), tr("RESET"), ""); - connect(resetCalibBtn, &ButtonControl::showDescription, this, &DevicePanel::updateCalibDescription); + connect(resetCalibBtn, &ButtonControl::showDescriptionEvent, this, &DevicePanel::updateCalibDescription); connect(resetCalibBtn, &ButtonControl::clicked, [&]() { - if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), this)) { + if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) { params.remove("CalibrationParams"); } }); @@ -119,7 +191,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { if (!params.getBool("Passive")) { auto retrainingBtn = new ButtonControl(tr("Review Training Guide"), tr("REVIEW"), tr("Review the rules, features, and limitations of openpilot")); connect(retrainingBtn, &ButtonControl::clicked, [=]() { - if (ConfirmationDialog::confirm(tr("Are you sure you want to review the training guide?"), this)) { + if (ConfirmationDialog::confirm(tr("Are you sure you want to review the training guide?"), tr("Review"), this)) { emit reviewTrainingGuide(); } }); @@ -130,7 +202,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { auto regulatoryBtn = new ButtonControl(tr("Regulatory"), tr("VIEW"), ""); connect(regulatoryBtn, &ButtonControl::clicked, [=]() { const std::string txt = util::read_file("../assets/offroad/fcc.html"); - RichTextDialog::alert(QString::fromStdString(txt), this); + ConfirmationDialog::rich(QString::fromStdString(txt), this); }); addItem(regulatoryBtn); } @@ -138,8 +210,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { auto translateBtn = new ButtonControl(tr("Change Language"), tr("CHANGE"), ""); connect(translateBtn, &ButtonControl::clicked, [=]() { QMap langs = getSupportedLanguages(); - QString currentLang = QString::fromStdString(Params().get("LanguageSetting")); - QString selection = MultiOptionDialog::getSelection(tr("Select a language"), langs.keys(), langs.key(currentLang), this); + QString selection = MultiOptionDialog::getSelection(tr("Select a language"), langs.keys(), langs.key(uiState()->language), this); if (!selection.isEmpty()) { // put language setting, exit Qt UI, and trigger fast restart Params().put("LanguageSetting", langs[selection].toStdString()); @@ -208,7 +279,7 @@ void DevicePanel::updateCalibDescription() { void DevicePanel::reboot() { if (!uiState()->engaged()) { - if (ConfirmationDialog::confirm(tr("Are you sure you want to reboot?"), this)) { + if (ConfirmationDialog::confirm(tr("Are you sure you want to reboot?"), tr("Reboot"), this)) { // Check engaged again in case it changed while the dialog was open if (!uiState()->engaged()) { Params().putBool("DoReboot", true); @@ -221,7 +292,7 @@ void DevicePanel::reboot() { void DevicePanel::poweroff() { if (!uiState()->engaged()) { - if (ConfirmationDialog::confirm(tr("Are you sure you want to power off?"), this)) { + if (ConfirmationDialog::confirm(tr("Are you sure you want to power off?"), tr("Power Off"), this)) { // Check engaged again in case it changed while the dialog was open if (!uiState()->engaged()) { Params().putBool("DoShutdown", true); @@ -232,88 +303,16 @@ void DevicePanel::poweroff() { } } -SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { - gitBranchLbl = new LabelControl(tr("Git Branch")); - gitCommitLbl = new LabelControl(tr("Git Commit")); - osVersionLbl = new LabelControl(tr("OS Version")); - versionLbl = new LabelControl(tr("Version"), "", QString::fromStdString(params.get("ReleaseNotes")).trimmed()); - lastUpdateLbl = new LabelControl(tr("Last Update Check"), "", tr("The last time openpilot successfully checked for an update. The updater only runs while the car is off.")); - updateBtn = new ButtonControl(tr("Check for Update"), ""); - connect(updateBtn, &ButtonControl::clicked, [=]() { - if (params.getBool("IsOffroad")) { - fs_watch->addPath(QString::fromStdString(params.getParamPath("LastUpdateTime"))); - fs_watch->addPath(QString::fromStdString(params.getParamPath("UpdateFailedCount"))); - updateBtn->setText(tr("CHECKING")); - updateBtn->setEnabled(false); - } - std::system("pkill -1 -f selfdrive.updated"); - }); - connect(uiState(), &UIState::offroadTransition, updateBtn, &QPushButton::setEnabled); - - branchSwitcherBtn = new ButtonControl(tr("Switch Branch"), tr("ENTER"), tr("The new branch will be pulled the next time the updater runs.")); - connect(branchSwitcherBtn, &ButtonControl::clicked, [=]() { - QString branch = InputDialog::getText(tr("Enter branch name"), this, tr("The new branch will be pulled the next time the updater runs."), - false, -1, QString::fromStdString(params.get("SwitchToBranch"))); - if (branch.isEmpty()) { - params.remove("SwitchToBranch"); - } else { - params.put("SwitchToBranch", branch.toStdString()); - } - std::system("pkill -1 -f selfdrive.updated"); - }); - connect(uiState(), &UIState::offroadTransition, branchSwitcherBtn, &QPushButton::setEnabled); - - auto uninstallBtn = new ButtonControl(tr("Uninstall %1").arg(getBrand()), tr("UNINSTALL")); - connect(uninstallBtn, &ButtonControl::clicked, [&]() { - if (ConfirmationDialog::confirm(tr("Are you sure you want to uninstall?"), this)) { - params.putBool("DoUninstall", true); - } - }); - connect(uiState(), &UIState::offroadTransition, uninstallBtn, &QPushButton::setEnabled); - - QWidget *widgets[] = {versionLbl, lastUpdateLbl, updateBtn, branchSwitcherBtn, gitBranchLbl, gitCommitLbl, osVersionLbl, uninstallBtn}; - for (QWidget* w : widgets) { - if (w == branchSwitcherBtn && params.getBool("IsTestedBranch")) { - continue; - } - addItem(w); - } - - fs_watch = new QFileSystemWatcher(this); - QObject::connect(fs_watch, &QFileSystemWatcher::fileChanged, [=](const QString path) { - if (path.contains("UpdateFailedCount") && std::atoi(params.get("UpdateFailedCount").c_str()) > 0) { - lastUpdateLbl->setText(tr("failed to fetch update")); - updateBtn->setText(tr("CHECK")); - updateBtn->setEnabled(true); - } else if (path.contains("LastUpdateTime")) { - updateLabels(); - } - }); -} - -void SoftwarePanel::showEvent(QShowEvent *event) { - updateLabels(); -} - -void SoftwarePanel::updateLabels() { - QString lastUpdate = ""; - auto tm = params.get("LastUpdateTime"); - if (!tm.empty()) { - lastUpdate = timeAgo(QDateTime::fromString(QString::fromStdString(tm + "Z"), Qt::ISODate)); - } - - versionLbl->setText(getBrandVersion()); - lastUpdateLbl->setText(lastUpdate); - updateBtn->setText(tr("CHECK")); - updateBtn->setEnabled(true); - gitBranchLbl->setText(QString::fromStdString(params.get("GitBranch"))); - gitCommitLbl->setText(QString::fromStdString(params.get("GitCommit")).left(10)); - osVersionLbl->setText(QString::fromStdString(Hardware::get_os_version()).trimmed()); -} - void SettingsWindow::showEvent(QShowEvent *event) { - panel_widget->setCurrentIndex(0); - nav_btns->buttons()[0]->setChecked(true); + setCurrentPanel(0); +} + +void SettingsWindow::setCurrentPanel(int index, const QString ¶m) { + panel_widget->setCurrentIndex(index); + nav_btns->buttons()[index]->setChecked(true); + if (!param.isEmpty()) { + emit expandToggleDescription(param); + } } SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { @@ -354,10 +353,13 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QObject::connect(device, &DevicePanel::reviewTrainingGuide, this, &SettingsWindow::reviewTrainingGuide); QObject::connect(device, &DevicePanel::showDriverView, this, &SettingsWindow::showDriverView); + TogglesPanel *toggles = new TogglesPanel(this); + QObject::connect(this, &SettingsWindow::expandToggleDescription, toggles, &TogglesPanel::expandToggleDescription); + QList> panels = { {tr("Device"), device}, {tr("Network"), new Networking(this)}, - {tr("Toggles"), new TogglesPanel(this)}, + {tr("Toggles"), toggles}, {tr("Software"), new SoftwarePanel(this)}, }; diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index 45efe255c..c63be4e13 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -17,6 +17,7 @@ class SettingsWindow : public QFrame { public: explicit SettingsWindow(QWidget *parent = 0); + void setCurrentPanel(int index, const QString ¶m = ""); protected: void showEvent(QShowEvent *event) override; @@ -25,6 +26,7 @@ signals: void closeSettings(); void reviewTrainingGuide(); void showDriverView(); + void expandToggleDescription(const QString ¶m); private: QPushButton *sidebar_alert_widget; @@ -54,6 +56,16 @@ class TogglesPanel : public ListWidget { Q_OBJECT public: explicit TogglesPanel(SettingsWindow *parent); + void showEvent(QShowEvent *event) override; + +public slots: + void expandToggleDescription(const QString ¶m); + +private: + Params params; + std::map toggles; + + void updateToggles(); }; class SoftwarePanel : public ListWidget { @@ -64,14 +76,15 @@ public: private: void showEvent(QShowEvent *event) override; void updateLabels(); + void checkForUpdates(); - LabelControl *gitBranchLbl; - LabelControl *gitCommitLbl; - LabelControl *osVersionLbl; + bool is_onroad = false; + + QLabel *onroadLbl; LabelControl *versionLbl; - LabelControl *lastUpdateLbl; - ButtonControl *updateBtn; - ButtonControl *branchSwitcherBtn; + ButtonControl *installBtn; + ButtonControl *downloadBtn; + ButtonControl *targetBranchBtn; Params params; QFileSystemWatcher *fs_watch; diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc new file mode 100644 index 000000000..12d62e63f --- /dev/null +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -0,0 +1,156 @@ +#include "selfdrive/ui/qt/offroad/settings.h" + +#include +#include +#include + +#include +#include + +#include "common/params.h" +#include "common/util.h" +#include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/widgets/input.h" +#include "system/hardware/hw.h" + + +void SoftwarePanel::checkForUpdates() { + std::system("pkill -SIGUSR1 -f selfdrive.updated"); +} + +SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { + onroadLbl = new QLabel(tr("Updates are only downloaded while the car is off.")); + onroadLbl->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left; padding-top: 30px; padding-bottom: 30px;"); + addItem(onroadLbl); + + // current version + versionLbl = new LabelControl(tr("Current Version"), ""); + addItem(versionLbl); + + // download update btn + downloadBtn = new ButtonControl(tr("Download"), tr("CHECK")); + connect(downloadBtn, &ButtonControl::clicked, [=]() { + downloadBtn->setEnabled(false); + if (downloadBtn->text() == tr("CHECK")) { + checkForUpdates(); + } else { + std::system("pkill -SIGHUP -f selfdrive.updated"); + } + }); + addItem(downloadBtn); + + // install update btn + installBtn = new ButtonControl(tr("Install Update"), tr("INSTALL")); + connect(installBtn, &ButtonControl::clicked, [=]() { + installBtn->setEnabled(false); + params.putBool("DoReboot", true); + }); + addItem(installBtn); + + // branch selecting + targetBranchBtn = new ButtonControl(tr("Target Branch"), tr("SELECT")); + connect(targetBranchBtn, &ButtonControl::clicked, [=]() { + auto current = params.get("GitBranch"); + QStringList branches = QString::fromStdString(params.get("UpdaterAvailableBranches")).split(","); + for (QString b : {current.c_str(), "devel-staging", "devel", "master-ci", "master"}) { + auto i = branches.indexOf(b); + if (i >= 0) { + branches.removeAt(i); + branches.insert(0, b); + } + } + + QString cur = QString::fromStdString(params.get("UpdaterTargetBranch")); + QString selection = MultiOptionDialog::getSelection(tr("Select a branch"), branches, cur, this); + if (!selection.isEmpty()) { + params.put("UpdaterTargetBranch", selection.toStdString()); + targetBranchBtn->setValue(QString::fromStdString(params.get("UpdaterTargetBranch"))); + checkForUpdates(); + } + }); + if (!params.getBool("IsTestedBranch")) { + addItem(targetBranchBtn); + } + + // uninstall button + auto uninstallBtn = new ButtonControl(tr("Uninstall %1").arg(getBrand()), tr("UNINSTALL")); + connect(uninstallBtn, &ButtonControl::clicked, [&]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to uninstall?"), tr("Uninstall"), this)) { + params.putBool("DoUninstall", true); + } + }); + addItem(uninstallBtn); + + fs_watch = new QFileSystemWatcher(this); + QObject::connect(fs_watch, &QFileSystemWatcher::fileChanged, [=](const QString path) { + updateLabels(); + }); + + connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { + is_onroad = !offroad; + updateLabels(); + }); + + updateLabels(); +} + +void SoftwarePanel::showEvent(QShowEvent *event) { + // nice for testing on PC + installBtn->setEnabled(true); + + updateLabels(); +} + +void SoftwarePanel::updateLabels() { + // add these back in case the files got removed + fs_watch->addPath(QString::fromStdString(params.getParamPath("LastUpdateTime"))); + fs_watch->addPath(QString::fromStdString(params.getParamPath("UpdateFailedCount"))); + fs_watch->addPath(QString::fromStdString(params.getParamPath("UpdaterState"))); + fs_watch->addPath(QString::fromStdString(params.getParamPath("UpdateAvailable"))); + + if (!isVisible()) { + return; + } + + // updater only runs offroad + onroadLbl->setVisible(is_onroad); + downloadBtn->setVisible(!is_onroad); + + // download update + QString updater_state = QString::fromStdString(params.get("UpdaterState")); + bool failed = std::atoi(params.get("UpdateFailedCount").c_str()) > 0; + if (updater_state != "idle") { + downloadBtn->setEnabled(false); + downloadBtn->setValue(updater_state); + } else { + if (failed) { + downloadBtn->setText("CHECK"); + downloadBtn->setValue("failed to check for update"); + } else if (params.getBool("UpdaterFetchAvailable")) { + downloadBtn->setText("DOWNLOAD"); + downloadBtn->setValue("update available"); + } else { + QString lastUpdate = "never"; + auto tm = params.get("LastUpdateTime"); + if (!tm.empty()) { + lastUpdate = timeAgo(QDateTime::fromString(QString::fromStdString(tm + "Z"), Qt::ISODate)); + } + downloadBtn->setText("CHECK"); + downloadBtn->setValue("up to date, last checked " + lastUpdate); + } + downloadBtn->setEnabled(true); + } + targetBranchBtn->setValue(QString::fromStdString(params.get("UpdaterTargetBranch"))); + + // current + new versions + versionLbl->setText(QString::fromStdString(params.get("UpdaterCurrentDescription"))); + versionLbl->setDescription(QString::fromStdString(params.get("UpdaterCurrentReleaseNotes"))); + + installBtn->setVisible(!is_onroad && params.getBool("UpdateAvailable")); + installBtn->setValue(QString::fromStdString(params.get("UpdaterNewDescription"))); + installBtn->setDescription(QString::fromStdString(params.get("UpdaterNewReleaseNotes"))); + + update(); +} diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index fbb64b972..fde864558 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -345,7 +345,7 @@ NetworkType WifiManager::currentNetworkType() { return NetworkType::NONE; } -void WifiManager::updateGsmSettings(bool roaming, QString apn) { +void WifiManager::updateGsmSettings(bool roaming, QString apn, bool metered) { if (!lteConnectionPath.path().isEmpty()) { bool changes = false; bool auto_config = apn.isEmpty(); @@ -368,6 +368,13 @@ void WifiManager::updateGsmSettings(bool roaming, QString apn) { changes = true; } + int meteredInt = metered ? NM_METERED_UNKNOWN : NM_METERED_NO; + if (settings.value("connection").value("metered").toInt() != meteredInt) { + qWarning() << "Changing connection.metered to" << meteredInt; + settings["connection"]["metered"] = meteredInt; + changes = true; + } + if (changes) { call(lteConnectionPath.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "UpdateUnsaved", QVariant::fromValue(settings)); // update is temporary deactivateConnection(lteConnectionPath); @@ -408,16 +415,16 @@ void WifiManager::addTetheringConnection() { } void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { - int prime_type = uiState()->prime_type; - int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); + int prime_type = uiState()->prime_type; + int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); - if (!ipv4_forward) { - QTimer::singleShot(5000, this, [=] { - qWarning() << "net.ipv4.ip_forward = 0"; - std::system("sudo sysctl net.ipv4.ip_forward=0"); - }); - } - call->deleteLater(); + if (!ipv4_forward) { + QTimer::singleShot(5000, this, [=] { + qWarning() << "net.ipv4.ip_forward = 0"; + std::system("sudo sysctl net.ipv4.ip_forward=0"); + }); + } + call->deleteLater(); } void WifiManager::setTetheringEnabled(bool enabled) { diff --git a/selfdrive/ui/qt/offroad/wifiManager.h b/selfdrive/ui/qt/offroad/wifiManager.h index 07b982c2c..01f9cd6b6 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.h +++ b/selfdrive/ui/qt/offroad/wifiManager.h @@ -50,7 +50,7 @@ public: bool isKnownConnection(const QString &ssid); std::optional activateWifiConnection(const QString &ssid); NetworkType currentNetworkType(); - void updateGsmSettings(bool roaming, QString apn); + void updateGsmSettings(bool roaming, QString apn, bool metered); void connect(const Network &ssid, const QString &password = {}, const QString &username = {}); // Tethering functions diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 4414e602e..50f891dd5 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -18,7 +18,7 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { stacked_layout->setStackingMode(QStackedLayout::StackAll); main_layout->addLayout(stacked_layout); - nvg = new NvgWindow(VISION_STREAM_ROAD, this); + nvg = new AnnotatedCameraWidget(VISION_STREAM_ROAD, this); QWidget * split_wrapper = new QWidget; split = new QHBoxLayout(split_wrapper); @@ -26,6 +26,11 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { split->setSpacing(0); split->addWidget(nvg); + if (getenv("DUAL_CAMERA_VIEW")) { + CameraWidget *arCam = new CameraWidget("camerad", VISION_STREAM_ROAD, true, this); + split->insertWidget(0, arCam); + } + stacked_layout->addWidget(split_wrapper); alerts = new OnroadAlerts(this); @@ -95,10 +100,6 @@ void OnroadWindow::offroadTransition(bool offroad) { #endif alerts->updateAlert({}, bg); - - // update stream type - bool wide_cam = Params().getBool("WideCameraOnly"); - nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); } void OnroadWindow::paintEvent(QPaintEvent *event) { @@ -168,14 +169,16 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } } -// NvgWindow -NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraViewWidget("camerad", type, true, parent) { +AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { + pm = std::make_unique>({"uiDebug"}); + engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); + experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size - 5, img_size - 5}); dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); } -void NvgWindow::updateState(const UIState &s) { +void AnnotatedCameraWidget::updateState(const UIState &s) { const int SET_SPEED_NA = 255; const SubMaster &sm = *(s.sm); @@ -225,15 +228,9 @@ void NvgWindow::updateState(const UIState &s) { setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); } - - if (s.scene.calibration_valid) { - CameraViewWidget::updateCalibration(s.scene.view_from_calib); - } else { - CameraViewWidget::updateCalibration(DEFAULT_CALIBRATION); - } } -void NvgWindow::drawHud(QPainter &p) { +void AnnotatedCameraWidget::drawHud(QPainter &p) { p.save(); // Header gradient @@ -382,8 +379,9 @@ void NvgWindow::drawHud(QPainter &p) { // engage-ability icon if (engageable) { + SubMaster &sm = *(uiState()->sm); drawIcon(p, rect().right() - radius / 2 - bdr_s * 2, radius / 2 + int(bdr_s * 1.5), - engage_img, bg_colors[status], 1.0); + sm["controlsState"].getControlsState().getExperimentalMode() ? experimental_img : engage_img, blackColor(166), 1.0); } // dm icon @@ -395,7 +393,11 @@ void NvgWindow::drawHud(QPainter &p) { p.restore(); } -void NvgWindow::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { + +// Window that shows camera view and variety of +// info drawn on top + +void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { QRect real_rect = getTextRect(p, 0, text); real_rect.moveCenter({x, y - real_rect.height() / 2}); @@ -403,17 +405,18 @@ void NvgWindow::drawText(QPainter &p, int x, int y, const QString &text, int alp p.drawText(real_rect.x(), real_rect.bottom(), text); } -void NvgWindow::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) { +void AnnotatedCameraWidget::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) { + p.setOpacity(1.0); // bg dictates opacity of ellipse p.setPen(Qt::NoPen); p.setBrush(bg); p.drawEllipse(x - radius / 2, y - radius / 2, radius, radius); p.setOpacity(opacity); - p.drawPixmap(x - img_size / 2, y - img_size / 2, img); + p.drawPixmap(x - img.size().width() / 2, y - img.size().height() / 2, img); } -void NvgWindow::initializeGL() { - CameraViewWidget::initializeGL(); +void AnnotatedCameraWidget::initializeGL() { + CameraWidget::initializeGL(); qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION)); qInfo() << "OpenGL vendor:" << QString((const char*)glGetString(GL_VENDOR)); qInfo() << "OpenGL renderer:" << QString((const char*)glGetString(GL_RENDERER)); @@ -423,8 +426,8 @@ void NvgWindow::initializeGL() { setBackgroundColor(bg_colors[STATUS_DISENGAGED]); } -void NvgWindow::updateFrameMat() { - CameraViewWidget::updateFrameMat(); +void AnnotatedCameraWidget::updateFrameMat() { + CameraWidget::updateFrameMat(); UIState *s = uiState(); int w = width(), h = height(); @@ -441,10 +444,12 @@ void NvgWindow::updateFrameMat() { .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); } -void NvgWindow::drawLaneLines(QPainter &painter, const UIState *s) { +void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.save(); const UIScene &scene = s->scene; + SubMaster &sm = *(s->sm); + // lanelines for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) { painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp(scene.lane_line_probs[i], 0.0, 0.7))); @@ -459,26 +464,36 @@ void NvgWindow::drawLaneLines(QPainter &painter, const UIState *s) { // paint path QLinearGradient bg(0, height(), 0, height() / 4); - const auto &orientation = (*s->sm)["modelV2"].getModelV2().getOrientation(); - float orientation_future = 0; - if (orientation.getZ().size() > 16) { - orientation_future = std::abs(orientation.getZ()[16]); // 2.5 seconds - } - // straight: 112, in turns: 70 - float curve_hue = fmax(70, 112 - (orientation_future * 420)); - // FIXME: painter.drawPolygon can be slow if hue is not rounded - curve_hue = int(curve_hue * 100 + 0.5) / 100; + float start_hue, end_hue; + 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 + } + 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)); + bg.setColorAt(1.0, QColor::fromHslF(112 / 360., 1.0, 0.68, 0.0)); + } - bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4)); - bg.setColorAt(0.75 / 1.5, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.35)); - bg.setColorAt(1.0, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.0)); painter.setBrush(bg); painter.drawPolygon(scene.track_vertices); painter.restore(); } -void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd) { +void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd) { painter.save(); const float speedBuff = 10.; @@ -514,17 +529,64 @@ void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV painter.restore(); } -void NvgWindow::paintGL() { +void AnnotatedCameraWidget::paintGL() { UIState *s = uiState(); - const cereal::ModelDataV2::Reader &model = (*s->sm)["modelV2"].getModelV2(); - CameraViewWidget::setFrameId(model.getFrameId()); - CameraViewWidget::paintGL(); + SubMaster &sm = *(s->sm); + const double start_draw_t = millis_since_boot(); + const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2(); + + // draw camera frame + { + std::lock_guard lk(frame_lock); + + if (frames.empty()) { + if (skip_frame_count > 0) { + skip_frame_count--; + qDebug() << "skipping frame, not ready"; + return; + } + } else { + // skip drawing up to this many frames if we're + // missing camera frames. this smooths out the + // transitions from the narrow and wide cameras + skip_frame_count = 5; + } + + // 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; + } + 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; + if (s->scene.calibration_valid) { + auto calib = s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib; + CameraWidget::updateCalibration(calib); + } else { + CameraWidget::updateCalibration(DEFAULT_CALIBRATION); + } + CameraWidget::setFrameId(model.getFrameId()); + CameraWidget::paintGL(); + } QPainter painter(this); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); if (s->worldObjectsVisible()) { + if (sm.rcv_frame("modelV2") > s->scene.started_frame) { + update_model(s, sm["modelV2"].getModelV2()); + if (sm.rcv_frame("radarState") > s->scene.started_frame) { + update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); + } + } drawLaneLines(painter, s); @@ -548,10 +610,16 @@ void NvgWindow::paintGL() { LOGW("slow frame rate: %.2f fps", fps); } prev_draw_t = cur_draw_t; + + // publish debug msg + MessageBuilder msg; + auto m = msg.initEvent().initUiDebug(); + m.setDrawTimeMillis(cur_draw_t - start_draw_t); + pm->send("uiDebug", msg); } -void NvgWindow::showEvent(QShowEvent *event) { - CameraViewWidget::showEvent(event); +void AnnotatedCameraWidget::showEvent(QShowEvent *event) { + CameraWidget::showEvent(event); ui_update_params(uiState()); prev_draw_t = millis_since_boot(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 25920ccc6..9e1835597 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -25,7 +25,7 @@ private: }; // container window for the NVG UI -class NvgWindow : public CameraViewWidget { +class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT Q_PROPERTY(float speed MEMBER speed); Q_PROPERTY(QString speedUnit MEMBER speedUnit); @@ -43,7 +43,7 @@ class NvgWindow : public CameraViewWidget { Q_PROPERTY(int status MEMBER status); public: - explicit NvgWindow(VisionStreamType type, QWidget* parent = 0); + explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0); void updateState(const UIState &s); private: @@ -51,6 +51,7 @@ private: void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); QPixmap engage_img; + QPixmap experimental_img; QPixmap dm_img; const int radius = 192; const int img_size = (radius / 2) * 1.5; @@ -68,6 +69,10 @@ private: bool has_eu_speed_limit = false; bool v_ego_cluster_seen = false; int status = STATUS_DISENGAGED; + std::unique_ptr pm; + + int skip_frame_count = 0; + bool wide_cam_requested = false; protected: void paintGL() override; @@ -97,7 +102,7 @@ private: void paintEvent(QPaintEvent *event); void mousePressEvent(QMouseEvent* e) override; OnroadAlerts *alerts; - NvgWindow *nvg; + AnnotatedCameraWidget *nvg; QColor bg = bg_colors[STATUS_DISENGAGED]; QWidget *map = nullptr; QHBoxLayout* split; diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index a84542d29..212f19562 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -32,8 +32,9 @@ void Sidebar::drawMetric(QPainter &p, const QPair &label, QCol p.drawText(label_rect, Qt::AlignCenter, label.second); } -Sidebar::Sidebar(QWidget *parent) : QFrame(parent) { +Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed(false), settings_pressed(false) { home_img = loadPixmap("../assets/images/button_home.png", home_btn.size()); + flag_img = loadPixmap("../assets/images/button_flag.png", home_btn.size()); settings_img = loadPixmap("../assets/images/button_settings.png", settings_btn.size(), Qt::IgnoreAspectRatio); connect(this, &Sidebar::valueChanged, [=] { update(); }); @@ -47,17 +48,35 @@ Sidebar::Sidebar(QWidget *parent) : QFrame(parent) { pm = std::make_unique>({"userFlag"}); } +void Sidebar::mousePressEvent(QMouseEvent *event) { + if (onroad && home_btn.contains(event->pos())) { + flag_pressed = true; + update(); + } else if (settings_btn.contains(event->pos())) { + settings_pressed = true; + update(); + } +} + void Sidebar::mouseReleaseEvent(QMouseEvent *event) { + if (flag_pressed || settings_pressed) { + flag_pressed = settings_pressed = false; + update(); + } if (home_btn.contains(event->pos())) { MessageBuilder msg; msg.initEvent().initUserFlag(); pm->send("userFlag", msg); - } - if (settings_btn.contains(event->pos())) { + } else if (settings_btn.contains(event->pos())) { emit openSettings(); } } +void Sidebar::offroadTransition(bool offroad) { + onroad = !offroad; + update(); +} + void Sidebar::updateState(const UIState &s) { if (!isVisible()) return; @@ -102,11 +121,12 @@ void Sidebar::paintEvent(QPaintEvent *event) { p.fillRect(rect(), QColor(57, 57, 57)); - // static imgs - p.setOpacity(0.65); + // buttons + p.setOpacity(settings_pressed ? 0.65 : 1.0); p.drawPixmap(settings_btn.x(), settings_btn.y(), settings_img); + p.setOpacity(onroad && flag_pressed ? 0.65 : 1.0); + p.drawPixmap(home_btn.x(), home_btn.y(), onroad ? flag_img : home_img); p.setOpacity(1.0); - p.drawPixmap(home_btn.x(), home_btn.y(), home_img); // network int x = 58; diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index 0140673aa..fb96e1d54 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -3,7 +3,6 @@ #include #include -#include "common/params.h" #include "selfdrive/ui/ui.h" typedef QPair, QColor> ItemStatus; @@ -21,18 +20,21 @@ public: explicit Sidebar(QWidget* parent = 0); signals: - void openSettings(); + void openSettings(int index = 0, const QString ¶m = ""); void valueChanged(); public slots: + void offroadTransition(bool offroad); void updateState(const UIState &s); protected: void paintEvent(QPaintEvent *event) override; + void mousePressEvent(QMouseEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override; void drawMetric(QPainter &p, const QPair &label, QColor c, int y); - QPixmap home_img, settings_img; + QPixmap home_img, flag_img, settings_img; + bool onroad, flag_pressed, settings_pressed; const QMap network_type = { {cereal::DeviceState::NetworkType::NONE, tr("--")}, {cereal::DeviceState::NetworkType::WIFI, tr("Wi-Fi")}, @@ -49,7 +51,6 @@ protected: const QColor warning_color = QColor(218, 202, 37); const QColor danger_color = QColor(201, 34, 49); - Params params; ItemStatus connect_status, panda_status, temp_status; QString net_type; int net_strength = 0; diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 4be88aaf1..59903e337 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -21,10 +21,6 @@ QString getBrand() { return Params().getBool("Passive") ? QObject::tr("dashcam") : QObject::tr("openpilot"); } -QString getBrandVersion() { - return getBrand() + " v" + getVersion().left(14).trimmed(); -} - QString getUserAgent() { return "openpilot-" + getVersion(); } @@ -162,7 +158,7 @@ QPixmap loadPixmap(const QString &fileName, const QSize &size, Qt::AspectRatioMo } } -QRect getTextRect(QPainter &p, int flags, QString text) { +QRect getTextRect(QPainter &p, int flags, const QString &text) { QFontMetrics fm(p.font()); QRect init_rect = fm.boundingRect(text); return fm.boundingRect(init_rect, flags, text); diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h index f0e57526c..61a27a866 100644 --- a/selfdrive/ui/qt/util.h +++ b/selfdrive/ui/qt/util.h @@ -11,7 +11,6 @@ QString getVersion(); QString getBrand(); -QString getBrandVersion(); QString getUserAgent(); std::optional getDongleId(); QMap getSupportedLanguages(); @@ -24,6 +23,6 @@ void initApp(int argc, char *argv[]); QWidget* topWidget (QWidget* widget); QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); -QRect getTextRect(QPainter &p, int flags, QString text); +QRect getTextRect(QPainter &p, int flags, const QString &text); void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom); QColor interpColor(float xv, std::vector xp, std::vector fp); diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 63d15660a..347cdb1dc 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -93,15 +93,16 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) } // namespace -CameraViewWidget::CameraViewWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : - stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { +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); - connect(this, &CameraViewWidget::vipcThreadConnected, this, &CameraViewWidget::vipcConnected, Qt::BlockingQueuedConnection); - connect(this, &CameraViewWidget::vipcThreadFrameReceived, this, &CameraViewWidget::vipcFrameReceived); + QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection); + QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection); } -CameraViewWidget::~CameraViewWidget() { +CameraWidget::~CameraWidget() { makeCurrent(); + stopVipcThread(); if (isValid()) { glDeleteVertexArrays(1, &frame_vao); glDeleteBuffers(1, &frame_vbo); @@ -111,7 +112,7 @@ CameraViewWidget::~CameraViewWidget() { doneCurrent(); } -void CameraViewWidget::initializeGL() { +void CameraWidget::initializeGL() { initializeOpenGLFunctions(); program = std::make_unique(context()); @@ -124,7 +125,7 @@ void CameraViewWidget::initializeGL() { GLint frame_pos_loc = program->attributeLocation("aPosition"); GLint frame_texcoord_loc = program->attributeLocation("aTexCoord"); - auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); + auto [x1, x2, y1, y2] = requested_stream_type == VISION_STREAM_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; const float frame_coords[4][4] = { {-1.0, -1.0, x2, y1}, // bl @@ -161,9 +162,9 @@ void CameraViewWidget::initializeGL() { #endif } -void CameraViewWidget::showEvent(QShowEvent *event) { - frames.clear(); +void CameraWidget::showEvent(QShowEvent *event) { if (!vipc_thread) { + clearFrames(); vipc_thread = new QThread(); connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater); @@ -171,7 +172,7 @@ void CameraViewWidget::showEvent(QShowEvent *event) { } } -void CameraViewWidget::hideEvent(QHideEvent *event) { +void CameraWidget::stopVipcThread() { if (vipc_thread) { vipc_thread->requestInterruption(); vipc_thread->quit(); @@ -180,19 +181,24 @@ void CameraViewWidget::hideEvent(QHideEvent *event) { } } -void CameraViewWidget::updateFrameMat() { +void CameraWidget::updateFrameMat() { int w = width(), h = height(); if (zoomed_view) { - if (stream_type == VISION_STREAM_DRIVER) { + if (active_stream_type == VISION_STREAM_DRIVER) { frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); } else { - intrinsic_matrix = (stream_type == VISION_STREAM_WIDE_ROAD) ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; - zoom = (stream_type == VISION_STREAM_WIDE_ROAD) ? 2.5 : 1.1; - // Project point at "infinity" to compute x and y offsets // to ensure this ends up in the middle of the screen + // for narrow come and a little lower for wide cam. // TODO: use proper perspective transform? + if (active_stream_type == VISION_STREAM_WIDE_ROAD) { + intrinsic_matrix = ecam_intrinsic_matrix; + zoom = 2.0; + } else { + intrinsic_matrix = fcam_intrinsic_matrix; + zoom = 1.1; + } const vec3 inf = {{1000., 0., 0.}}; const vec3 Ep = matvecmul3(calibration, inf); const vec3 Kep = matvecmul3(intrinsic_matrix, Ep); @@ -206,11 +212,11 @@ void CameraViewWidget::updateFrameMat() { x_offset = std::clamp(x_offset_, -max_x_offset, max_x_offset); y_offset = std::clamp(y_offset_, -max_y_offset, max_y_offset); - float zx = zoom * 2 * intrinsic_matrix.v[2] / width(); - float zy = zoom * 2 * intrinsic_matrix.v[5] / height(); + float zx = zoom * 2 * intrinsic_matrix.v[2] / w; + float zy = zoom * 2 * intrinsic_matrix.v[5] / h; const mat4 frame_transform = {{ - zx, 0.0, 0.0, -x_offset / width() * 2, - 0.0, zy, 0.0, y_offset / height() * 2, + zx, 0.0, 0.0, -x_offset / w * 2, + 0.0, zy, 0.0, y_offset / h * 2, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; @@ -218,21 +224,21 @@ void CameraViewWidget::updateFrameMat() { } } else if (stream_width > 0 && stream_height > 0) { // fit frame to widget size - float widget_aspect_ratio = (float)width() / height(); + float widget_aspect_ratio = (float)w / h; float frame_aspect_ratio = (float)stream_width / stream_height; frame_mat = get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio); } } -void CameraViewWidget::updateCalibration(const mat3 &calib) { +void CameraWidget::updateCalibration(const mat3 &calib) { calibration = calib; - updateFrameMat(); } -void CameraViewWidget::paintGL() { +void CameraWidget::paintGL() { glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + std::lock_guard lk(frame_lock); if (frames.empty()) return; int frame_idx = frames.size() - 1; @@ -249,19 +255,23 @@ void CameraViewWidget::paintGL() { qDebug() << "Skipped frame" << frames[frame_idx].first; } prev_frame_id = frames[frame_idx].first; + VisionBuf *frame = frames[frame_idx].second; + assert(frame != nullptr); + + updateFrameMat(); glViewport(0, 0, width(), height()); glBindVertexArray(frame_vao); glUseProgram(program->programId()); glPixelStorei(GL_UNPACK_ALIGNMENT, 1); - VisionBuf *frame = frames[frame_idx].second; - #ifdef QCOM2 + // no frame copy glActiveTexture(GL_TEXTURE0); glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]); assert(glGetError() == GL_NO_ERROR); #else + // fallback to copy glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, textures[0]); @@ -286,9 +296,8 @@ void CameraViewWidget::paintGL() { glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); } -void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { +void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { makeCurrent(); - frames.clear(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; stream_stride = vipc_client->buffers[0].stride; @@ -335,30 +344,28 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { glTexImage2D(GL_TEXTURE_2D, 0, GL_RG8, stream_width/2, stream_height/2, 0, GL_RG, GL_UNSIGNED_BYTE, nullptr); assert(glGetError() == GL_NO_ERROR); #endif - - updateFrameMat(); } -void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) { - frames.push_back(std::make_pair(frame_id, buf)); - while (frames.size() > FRAME_BUFFER_SIZE) { - frames.pop_front(); - } +void CameraWidget::vipcFrameReceived() { update(); } -void CameraViewWidget::vipcThread() { - VisionStreamType cur_stream_type = stream_type; +void CameraWidget::vipcThread() { + VisionStreamType cur_stream = requested_stream_type; std::unique_ptr vipc_client; VisionIpcBufExtra meta_main = {0}; while (!QThread::currentThread()->isInterruptionRequested()) { - if (!vipc_client || cur_stream_type != stream_type) { - cur_stream_type = stream_type; - vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, false)); + if (!vipc_client || cur_stream != requested_stream_type) { + clearFrames(); + qDebug() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; + cur_stream = requested_stream_type;; + vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false)); } + active_stream_type = cur_stream; if (!vipc_client->connected) { + clearFrames(); if (!vipc_client->connect(false)) { QThread::msleep(100); continue; @@ -367,7 +374,14 @@ void CameraViewWidget::vipcThread() { } if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) { - emit vipcThreadFrameReceived(buf, meta_main.frame_id); + { + std::lock_guard lk(frame_lock); + frames.push_back(std::make_pair(meta_main.frame_id, buf)); + while (frames.size() > FRAME_BUFFER_SIZE) { + frames.pop_front(); + } + } + emit vipcThreadFrameReceived(); } } @@ -378,3 +392,8 @@ void CameraViewWidget::vipcThread() { egl_images.clear(); #endif } + +void CameraWidget::clearFrames() { + std::lock_guard lk(frame_lock); + frames.clear(); +} diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 016522b05..7cc3847f9 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -23,32 +24,34 @@ const int FRAME_BUFFER_SIZE = 5; static_assert(FRAME_BUFFER_SIZE <= YUV_BUFFER_COUNT); -class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { +class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions { Q_OBJECT public: using QOpenGLWidget::QOpenGLWidget; - explicit CameraViewWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr); - ~CameraViewWidget(); - void setStreamType(VisionStreamType type) { stream_type = type; } + explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr); + ~CameraWidget(); void setBackgroundColor(const QColor &color) { bg = color; } void setFrameId(int frame_id) { draw_frame_id = frame_id; } + void setStreamType(VisionStreamType type) { requested_stream_type = type; } + VisionStreamType getStreamType() { return active_stream_type; } signals: void clicked(); void vipcThreadConnected(VisionIpcClient *); - void vipcThreadFrameReceived(VisionBuf *, quint32); + void vipcThreadFrameReceived(); protected: void paintGL() override; void initializeGL() override; void resizeGL(int w, int h) override { updateFrameMat(); } void showEvent(QShowEvent *event) override; - void hideEvent(QHideEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); } virtual void updateFrameMat(); void updateCalibration(const mat3 &calib); void vipcThread(); + void clearFrames(); + void stopVipcThread(); bool zoomed_view; GLuint frame_vao, frame_vbo, frame_ibo; @@ -66,7 +69,8 @@ protected: int stream_width = 0; int stream_height = 0; int stream_stride = 0; - std::atomic stream_type; + std::atomic active_stream_type; + std::atomic requested_stream_type; QThread *vipc_thread = nullptr; // Calibration @@ -76,11 +80,12 @@ protected: mat3 calibration = DEFAULT_CALIBRATION; mat3 intrinsic_matrix = fcam_intrinsic_matrix; + std::recursive_mutex frame_lock; std::deque> frames; uint32_t draw_frame_id = 0; - int prev_frame_id = 0; + uint32_t prev_frame_id = 0; protected slots: void vipcConnected(VisionIpcClient *vipc_client); - void vipcFrameReceived(VisionBuf *vipc_client, uint32_t frame_id); + void vipcFrameReceived(); }; diff --git a/selfdrive/ui/qt/widgets/controls.cc b/selfdrive/ui/qt/widgets/controls.cc index 3264fd3aa..456cf748f 100644 --- a/selfdrive/ui/qt/widgets/controls.cc +++ b/selfdrive/ui/qt/widgets/controls.cc @@ -18,8 +18,6 @@ QFrame *horizontal_line(QWidget *parent) { } AbstractControl::AbstractControl(const QString &title, const QString &desc, const QString &icon, QWidget *parent) : QFrame(parent) { - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum); - QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setMargin(0); @@ -28,10 +26,10 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons hlayout->setSpacing(20); // left icon + icon_label = new QLabel(); if (!icon.isEmpty()) { - QPixmap pix(icon); - QLabel *icon_label = new QLabel(); - icon_label->setPixmap(pix.scaledToWidth(80, Qt::SmoothTransformation)); + icon_pixmap = QPixmap(icon).scaledToWidth(80, Qt::SmoothTransformation); + icon_label->setPixmap(icon_pixmap); icon_label->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); hlayout->addWidget(icon_label); } @@ -40,7 +38,13 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons title_label = new QPushButton(title); title_label->setFixedHeight(120); title_label->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left"); - hlayout->addWidget(title_label); + hlayout->addWidget(title_label, 1); + + // value next to control button + value = new ElidedLabel(); + value->setAlignment(Qt::AlignRight | Qt::AlignVCenter); + value->setStyleSheet("color: #aaaaaa"); + hlayout->addWidget(value); main_layout->addLayout(hlayout); @@ -54,7 +58,7 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons connect(title_label, &QPushButton::clicked, [=]() { if (!description->isVisible()) { - emit showDescription(); + emit showDescriptionEvent(); } if (!description->text().isEmpty()) { @@ -66,7 +70,7 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons } void AbstractControl::hideEvent(QHideEvent *e) { - if(description != nullptr) { + if (description != nullptr) { description->hide(); } } diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index 4245a9c04..770b9b92d 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -7,6 +7,7 @@ #include #include "common/params.h" +#include "selfdrive/ui/qt/widgets/input.h" #include "selfdrive/ui/qt/widgets/toggle.h" QFrame *horizontal_line(QWidget *parent = nullptr); @@ -45,8 +46,24 @@ public: title_label->setText(title); } + void setValue(const QString &val) { + value->setText(val); + } + + const QString getDescription() { + return description->text(); + } + + QLabel *icon_label; + QPixmap icon_pixmap; + +public slots: + void showDescription() { + description->setVisible(true); + }; + signals: - void showDescription(); + void showDescriptionEvent(); protected: AbstractControl(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr); @@ -54,6 +71,9 @@ protected: QHBoxLayout *hlayout; QPushButton *title_label; + +private: + ElidedLabel *value; QLabel *description = nullptr; }; @@ -105,7 +125,10 @@ public: QObject::connect(&toggle, &Toggle::stateChanged, this, &ToggleControl::toggleFlipped); } - void setEnabled(bool enabled) { toggle.setEnabled(enabled); toggle.update(); } + void setEnabled(bool enabled) { + toggle.setEnabled(enabled); + toggle.update(); + } signals: void toggleFlipped(bool state); @@ -121,20 +144,57 @@ class ParamControl : public ToggleControl { public: ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr) : ToggleControl(title, desc, icon, false, parent) { key = param.toStdString(); - QObject::connect(this, &ToggleControl::toggleFlipped, [=](bool state) { - params.putBool(key, state); + QObject::connect(this, &ParamControl::toggleFlipped, [=](bool state) { + QString content("

" + title + "


" + "

" + getDescription() + "

"); + ConfirmationDialog dialog(content, tr("Enable"), tr("Cancel"), true, this); + + bool confirmed = store_confirm && params.getBool(key + "Confirmed"); + if (!confirm || confirmed || !state || dialog.exec()) { + if (store_confirm && state) params.putBool(key + "Confirmed", true); + params.putBool(key, state); + setIcon(state); + } else { + toggle.togglePosition(); + } }); } - void showEvent(QShowEvent *event) override { - if (params.getBool(key) != toggle.on) { + void setConfirmation(bool _confirm, bool _store_confirm) { + confirm = _confirm; + store_confirm = _store_confirm; + }; + + void setActiveIcon(const QString &icon) { + active_icon_pixmap = QPixmap(icon).scaledToWidth(80, Qt::SmoothTransformation); + } + + void refresh() { + bool state = params.getBool(key); + if (state != toggle.on) { toggle.togglePosition(); + setIcon(state); } }; + void showEvent(QShowEvent *event) override { + refresh(); + }; + private: + void setIcon(bool state) { + if (state && !active_icon_pixmap.isNull()) { + icon_label->setPixmap(active_icon_pixmap); + } else if (!icon_pixmap.isNull()) { + icon_label->setPixmap(icon_pixmap); + } + }; + std::string key; Params params; + QPixmap active_icon_pixmap; + bool confirm = false; + bool store_confirm = false; }; class ListWidget : public QWidget { @@ -157,9 +217,12 @@ private: QPainter p(this); p.setPen(Qt::gray); for (int i = 0; i < inner_layout.count() - 1; ++i) { - QRect r = inner_layout.itemAt(i)->geometry(); - int bottom = r.bottom() + inner_layout.spacing() / 2; - p.drawLine(r.left() + 40, bottom, r.right() - 40, bottom); + QWidget *widget = inner_layout.itemAt(i)->widget(); + if (widget == nullptr || widget->isVisible()) { + QRect r = inner_layout.itemAt(i)->geometry(); + int bottom = r.bottom() + inner_layout.spacing() / 2; + p.drawLine(r.left() + 40, bottom, r.right() - 40, bottom); + } } } QVBoxLayout outer_layout; diff --git a/selfdrive/ui/qt/widgets/input.cc b/selfdrive/ui/qt/widgets/input.cc index d70382588..75453e1b9 100644 --- a/selfdrive/ui/qt/widgets/input.cc +++ b/selfdrive/ui/qt/widgets/input.cc @@ -183,17 +183,21 @@ void InputDialog::setMinLength(int length) { // ConfirmationDialog ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString &confirm_text, const QString &cancel_text, - QWidget *parent) : QDialogBase(parent) { + const bool rich, QWidget *parent) : QDialogBase(parent) { QFrame *container = new QFrame(this); - container->setStyleSheet("QFrame { border-radius: 0; background-color: #ECECEC; }"); + container->setStyleSheet(R"( + QFrame { background-color: #1B1B1B; color: #C9C9C9; } + #confirm_btn { background-color: #465BEA; } + #confirm_btn:pressed { background-color: #3049F4; } + )"); QVBoxLayout *main_layout = new QVBoxLayout(container); - main_layout->setContentsMargins(32, 120, 32, 32); + main_layout->setContentsMargins(32, rich ? 32 : 120, 32, 32); QLabel *prompt = new QLabel(prompt_text, this); prompt->setWordWrap(true); - prompt->setAlignment(Qt::AlignHCenter); - prompt->setStyleSheet("font-size: 70px; font-weight: bold; color: black;"); - main_layout->addWidget(prompt, 1, Qt::AlignTop | Qt::AlignHCenter); + prompt->setAlignment(rich ? Qt::AlignLeft : Qt::AlignHCenter); + prompt->setStyleSheet((rich ? "font-size: 42px; font-weight: light;" : "font-size: 70px; font-weight: bold;") + QString(" margin: 45px;")); + main_layout->addWidget(rich ? (QWidget*)new ScrollView(prompt, this) : (QWidget*)prompt, 1, Qt::AlignTop); // cancel + confirm buttons QHBoxLayout *btn_layout = new QHBoxLayout(); @@ -208,54 +212,29 @@ ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString if (confirm_text.length()) { QPushButton* confirm_btn = new QPushButton(confirm_text); + confirm_btn->setObjectName("confirm_btn"); btn_layout->addWidget(confirm_btn); QObject::connect(confirm_btn, &QPushButton::clicked, this, &ConfirmationDialog::accept); } QVBoxLayout *outer_layout = new QVBoxLayout(this); - outer_layout->setContentsMargins(210, 170, 210, 170); + int margin = rich ? 100 : 200; + outer_layout->setContentsMargins(margin, margin, margin, margin); outer_layout->addWidget(container); } bool ConfirmationDialog::alert(const QString &prompt_text, QWidget *parent) { - ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Ok"), "", parent); + ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Ok"), "", false, parent); return d.exec(); } -bool ConfirmationDialog::confirm(const QString &prompt_text, QWidget *parent) { - ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Ok"), tr("Cancel"), parent); +bool ConfirmationDialog::confirm(const QString &prompt_text, const QString &confirm_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, confirm_text, tr("Cancel"), false, parent); return d.exec(); } - -// RichTextDialog - -RichTextDialog::RichTextDialog(const QString &prompt_text, const QString &btn_text, - QWidget *parent) : QDialogBase(parent) { - QFrame *container = new QFrame(this); - container->setStyleSheet("QFrame { background-color: #1B1B1B; }"); - QVBoxLayout *main_layout = new QVBoxLayout(container); - main_layout->setContentsMargins(32, 32, 32, 32); - - QLabel *prompt = new QLabel(prompt_text, this); - prompt->setWordWrap(true); - prompt->setAlignment(Qt::AlignLeft); - prompt->setTextFormat(Qt::RichText); - prompt->setStyleSheet("font-size: 42px; font-weight: light; color: #C9C9C9; margin: 45px;"); - main_layout->addWidget(new ScrollView(prompt, this), 1, Qt::AlignTop); - - // confirm button - QPushButton* confirm_btn = new QPushButton(btn_text); - main_layout->addWidget(confirm_btn); - QObject::connect(confirm_btn, &QPushButton::clicked, this, &QDialog::accept); - - QVBoxLayout *outer_layout = new QVBoxLayout(this); - outer_layout->setContentsMargins(100, 100, 100, 100); - outer_layout->addWidget(container); -} - -bool RichTextDialog::alert(const QString &prompt_text, QWidget *parent) { - auto d = RichTextDialog(prompt_text, tr("Ok"), parent); +bool ConfirmationDialog::rich(const QString &prompt_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Ok"), "", true, parent); return d.exec(); } diff --git a/selfdrive/ui/qt/widgets/input.h b/selfdrive/ui/qt/widgets/input.h index 6c47a31d8..e6c0fba86 100644 --- a/selfdrive/ui/qt/widgets/input.h +++ b/selfdrive/ui/qt/widgets/input.h @@ -55,18 +55,10 @@ class ConfirmationDialog : public QDialogBase { public: explicit ConfirmationDialog(const QString &prompt_text, const QString &confirm_text, - const QString &cancel_text, QWidget* parent); - static bool alert(const QString &prompt_text, QWidget *parent); - static bool confirm(const QString &prompt_text, QWidget *parent); -}; - -// larger ConfirmationDialog for rich text -class RichTextDialog : public QDialogBase { - Q_OBJECT - -public: - explicit RichTextDialog(const QString &prompt_text, const QString &btn_text, QWidget* parent); + const QString &cancel_text, const bool rich, QWidget* parent); static bool alert(const QString &prompt_text, QWidget *parent); + static bool confirm(const QString &prompt_text, const QString &confirm_text, QWidget *parent); + static bool rich(const QString &prompt_text, QWidget *parent); }; class MultiOptionDialog : public QDialogBase { diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc index 937ea02f8..ceb823fb2 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.cc +++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc @@ -112,7 +112,7 @@ UpdateAlert::UpdateAlert(QWidget *parent) : AbstractAlert(true, parent) { bool UpdateAlert::refresh() { bool updateAvailable = params.getBool("UpdateAvailable"); if (updateAvailable) { - releaseNotes->setText(params.get("ReleaseNotes").c_str()); + releaseNotes->setText(params.get("UpdaterNewReleaseNotes").c_str()); } return updateAvailable; } diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index 541947526..da2f4e60d 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -18,21 +18,24 @@ using qrcodegen::QrCode; PairingQRWidget::PairingQRWidget(QWidget* parent) : QWidget(parent) { - QTimer* timer = new QTimer(this); - timer->start(5 * 60 * 1000); + timer = new QTimer(this); connect(timer, &QTimer::timeout, this, &PairingQRWidget::refresh); } void PairingQRWidget::showEvent(QShowEvent *event) { refresh(); + timer->start(5 * 60 * 1000); +} + +void PairingQRWidget::hideEvent(QHideEvent *event) { + timer->stop(); } void PairingQRWidget::refresh() { - if (isVisible()) { - QString pairToken = CommaApi::create_jwt({{"pair", true}}); - QString qrString = "https://connect.comma.ai/?pair=" + pairToken; - this->updateQrCode(qrString); - } + QString pairToken = CommaApi::create_jwt({{"pair", true}}); + QString qrString = "https://connect.comma.ai/?pair=" + pairToken; + this->updateQrCode(qrString); + update(); } void PairingQRWidget::updateQrCode(const QString &text) { @@ -274,7 +277,7 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { primeUser = new PrimeUserWidget; mainLayout->addWidget(primeUser); - mainLayout->setCurrentWidget(primeAd); + mainLayout->setCurrentWidget(uiState()->prime_type ? (QWidget*)primeUser : (QWidget*)primeAd); setFixedWidth(750); setStyleSheet(R"( @@ -296,11 +299,9 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { QObject::connect(repeater, &RequestRepeater::requestDone, this, &SetupWidget::replyFinished); } - hide(); // Only show when first request comes back } void SetupWidget::replyFinished(const QString &response, bool success) { - show(); if (!success) return; QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); @@ -311,11 +312,7 @@ void SetupWidget::replyFinished(const QString &response, bool success) { QJsonObject json = doc.object(); int prime_type = json["prime_type"].toInt(); - - if (uiState()->prime_type != prime_type) { - uiState()->prime_type = prime_type; - Params().put("PrimeType", std::to_string(prime_type)); - } + uiState()->prime_type = prime_type; if (!json["is_paired"].toBool()) { mainLayout->setCurrentIndex(0); diff --git a/selfdrive/ui/qt/widgets/prime.h b/selfdrive/ui/qt/widgets/prime.h index ce4baecfa..0a1d93250 100644 --- a/selfdrive/ui/qt/widgets/prime.h +++ b/selfdrive/ui/qt/widgets/prime.h @@ -25,8 +25,10 @@ public: private: QPixmap img; + QTimer *timer; void updateQrCode(const QString &text); void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; private slots: void refresh(); diff --git a/selfdrive/ui/qt/widgets/scrollview.cc b/selfdrive/ui/qt/widgets/scrollview.cc index bd4309d8d..553659301 100644 --- a/selfdrive/ui/qt/widgets/scrollview.cc +++ b/selfdrive/ui/qt/widgets/scrollview.cc @@ -3,6 +3,8 @@ #include #include +// TODO: disable horizontal scrolling and resize + ScrollView::ScrollView(QWidget *w, QWidget *parent) : QScrollArea(parent) { setWidget(w); setWidgetResizable(true); diff --git a/selfdrive/ui/qt/widgets/ssh_keys.cc b/selfdrive/ui/qt/widgets/ssh_keys.cc index f17604b3e..1097a8926 100644 --- a/selfdrive/ui/qt/widgets/ssh_keys.cc +++ b/selfdrive/ui/qt/widgets/ssh_keys.cc @@ -5,10 +5,6 @@ #include "selfdrive/ui/qt/widgets/input.h" SshControl::SshControl() : ButtonControl(tr("SSH Keys"), "", tr("Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.")) { - username_label.setAlignment(Qt::AlignRight | Qt::AlignVCenter); - username_label.setStyleSheet("color: #aaaaaa"); - hlayout->insertWidget(1, &username_label); - QObject::connect(this, &ButtonControl::clicked, [=]() { if (text() == tr("ADD")) { QString username = InputDialog::getText(tr("Enter your GitHub username"), this); @@ -30,10 +26,10 @@ SshControl::SshControl() : ButtonControl(tr("SSH Keys"), "", tr("Warning: This g void SshControl::refresh() { QString param = QString::fromStdString(params.get("GithubSshKeys")); if (param.length()) { - username_label.setText(QString::fromStdString(params.get("GithubUsername"))); + setValue(QString::fromStdString(params.get("GithubUsername"))); setText(tr("REMOVE")); } else { - username_label.setText(""); + setValue(""); setText(tr("ADD")); } setEnabled(true); diff --git a/selfdrive/ui/qt/widgets/ssh_keys.h b/selfdrive/ui/qt/widgets/ssh_keys.h index 01e2ab83c..920bd651e 100644 --- a/selfdrive/ui/qt/widgets/ssh_keys.h +++ b/selfdrive/ui/qt/widgets/ssh_keys.h @@ -27,8 +27,6 @@ public: private: Params params; - QLabel username_label; - void refresh(); void getUserKeys(const QString &username); }; diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index 04ce15ef2..198b1edbf 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -53,6 +53,7 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { QFontDatabase::addApplicationFont("../assets/fonts/Inter-Regular.ttf"); QFontDatabase::addApplicationFont("../assets/fonts/Inter-SemiBold.ttf"); QFontDatabase::addApplicationFont("../assets/fonts/Inter-Thin.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/JetBrainsMono-Medium.ttf"); // no outline to prevent the focus rectangle setStyleSheet(R"( @@ -64,8 +65,9 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { setAttribute(Qt::WA_NoSystemBackground); } -void MainWindow::openSettings() { +void MainWindow::openSettings(int index, const QString ¶m) { main_layout->setCurrentWidget(settingsWindow); + settingsWindow->setCurrentPanel(index, param); } void MainWindow::closeSettings() { diff --git a/selfdrive/ui/qt/window.h b/selfdrive/ui/qt/window.h index 0bd328aa8..71fc466c2 100644 --- a/selfdrive/ui/qt/window.h +++ b/selfdrive/ui/qt/window.h @@ -15,7 +15,7 @@ public: private: bool eventFilter(QObject *obj, QEvent *event) override; - void openSettings(); + void openSettings(int index = 0, const QString ¶m = ""); void closeSettings(); Device device; diff --git a/selfdrive/ui/tests/test_translations.py b/selfdrive/ui/tests/test_translations.py new file mode 100755 index 000000000..26d6c3934 --- /dev/null +++ b/selfdrive/ui/tests/test_translations.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +import json +import os +import re +import shutil +import unittest +import xml.etree.ElementTree as ET + +from selfdrive.ui.update_translations import TRANSLATIONS_DIR, LANGUAGES_FILE, update_translations + +TMP_TRANSLATIONS_DIR = os.path.join(TRANSLATIONS_DIR, "tmp") +UNFINISHED_TRANSLATION_TAG = "" not in cur_translations, + f"{file} ({name}) translation file has obsolete translations. Run selfdrive/ui/update_translations.py --vanish to remove them") + + def test_plural_translations(self): + """ + Tests: + - that any numerus (plural) translations marked "finished" have all plural forms non-empty + - that the correct format specifier is used (%n) + """ + for name, file in self.translation_files.items(): + with self.subTest(name=name, file=file): + tr_xml = ET.parse(os.path.join(TRANSLATIONS_DIR, f"{file}.ts")) + + for context in tr_xml.getroot(): + for message in context.iterfind("message"): + if message.get("numerus") == "yes": + translation = message.find("translation") + numerusform = [t.text for t in translation.findall("numerusform")] + + # Do not assert finished translations + if translation.get("type") == "unfinished": + continue + + self.assertNotIn(None, numerusform, "Ensure all plural translation forms are completed.") + self.assertTrue(all([re.search("%[0-9]+", t) is None for t in numerusform]), + "Plural translations must use %n, not %1, %2, etc.: {}".format(numerusform)) + + def test_no_locations(self): + for name, file in self.translation_files.items(): + with self.subTest(name=name, file=file): + for line in self._read_translation_file(TRANSLATIONS_DIR, file).splitlines(): + self.assertFalse(line.strip().startswith(LOCATION_TAG), + f"Line contains location tag: {line.strip()}, remove all line numbers.") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/ui/translations/languages.json b/selfdrive/ui/translations/languages.json index 2559b2d26..072d320c1 100644 --- a/selfdrive/ui/translations/languages.json +++ b/selfdrive/ui/translations/languages.json @@ -1,6 +1,6 @@ { "English": "main_en", - "Português": "main_pt", + "Português": "main_pt-BR", "中文（繁體）": "main_zh-CHT", "中文（简体）": "main_zh-CHS", "한국어": "main_ko", diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts new file mode 100644 index 000000000..07a84fca0 --- /dev/null +++ b/selfdrive/ui/translations/main_ar.ts @@ -0,0 +1,1136 @@ + + + + + AbstractAlert + + Close + أغلق + + + Snooze Update + تأخير التحديث + + + Reboot and Update + إعادة التشغيل والتحديث + + + + AdvancedNetworking + + Back + خلف + + + Enable Tethering + تمكين الربط + + + Tethering Password + كلمة مرور للربط + + + EDIT + تعديل + + + Enter new tethering password + أدخل كلمة مرور جديدة للربط + + + IP Address + عنوان IP + + + Enable Roaming + تمكين التجوال + + + APN Setting + إعداد APN + + + Enter APN + أدخل APN + + + leave blank for automatic configuration + اتركه فارغا للتكوين التلقائي + + + Cellular Metered + + + + Prevent large data uploads when on a metered connection + + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + الأعلى + + + SPEED + سرعة + + + LIMIT + حد + + + + ConfirmationDialog + + Ok + موافق + + + Cancel + إلغاء + + + + DeclinePage + + You must accept the Terms and Conditions in order to use openpilot. + يجب عليك قبول الشروط والأحكام من أجل استخدام openpilot. + + + Back + خلف + + + Decline, uninstall %1 + رفض ، قم بإلغاء تثبيت %1 + + + + DevicePanel + + Dongle ID + معرف دونجل + + + N/A + غير متاح + + + Serial + التسلسلي + + + Driver Camera + كاميرا السائق + + + PREVIEW + لمح + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + قم بمعاينة الكاميرا المواجهة للسائق للتأكد من أن نظام مراقبة السائق يتمتع برؤية جيدة. (يجب أن تكون السيارة معطلة) + + + Reset Calibration + إعادة ضبط المعايرة + + + RESET + إعادة تعيين + + + Are you sure you want to reset calibration? + هل أنت متأكد أنك تريد إعادة ضبط المعايرة؟ + + + Review Training Guide + مراجعة دليل التدريب + + + REVIEW + مراجعة + + + Review the rules, features, and limitations of openpilot + راجع القواعد والميزات والقيود الخاصة بـ openpilot + + + Are you sure you want to review the training guide? + هل أنت متأكد أنك تريد مراجعة دليل التدريب؟ + + + Regulatory + تنظيمية + + + VIEW + عرض + + + Change Language + تغيير اللغة + + + CHANGE + تغيير + + + Select a language + اختر لغة + + + Reboot + اعادة التشغيل + + + Power Off + أطفاء + + + 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 بالمعايرة بشكل مستمر ، ونادراً ما تكون إعادة الضبط مطلوبة. + + + Your device is pointed %1° %2 and %3° %4. + جهازك يشير %1° %2 و %3° %4. + + + down + لأسفل + + + up + إلى أعلى + + + left + إلى اليسار + + + right + إلى اليمين + + + Are you sure you want to reboot? + هل أنت متأكد أنك تريد إعادة التشغيل؟ + + + Disengage to Reboot + فك الارتباط لإعادة التشغيل + + + Are you sure you want to power off? + هل أنت متأكد أنك تريد إيقاف التشغيل؟ + + + Disengage to Power Off + فك الارتباط لإيقاف التشغيل + + + + DriveStats + + Drives + أرقام القيادة + + + Hours + ساعات + + + ALL TIME + في كل وقت + + + PAST WEEK + الأسبوع الماضي + + + KM + كم + + + Miles + اميال + + + + DriverViewScene + + camera starting + بدء تشغيل الكاميرا + + + + InputDialog + + Cancel + إلغاء + + + Need at least %n character(s)! + + تحتاج على الأقل %n حرف! + تحتاج على الأقل %n حرف! + تحتاج على الأقل %n احرف! + تحتاج على الأقل %n احرف! + تحتاج على الأقل %n احرف! + تحتاج على الأقل %n احرف! + + + + + Installer + + Installing... + جارٍ التثبيت ... + + + Receiving objects: + استقبال الكائنات: + + + Resolving deltas: + حل دلتا: + + + Updating files: + جارٍ تحديث الملفات: + + + + MapETA + + eta + eta + + + min + دق + + + hr + سع + + + km + كم + + + mi + مل + + + + MapInstructions + + km + كم + + + m + م + + + mi + مل + + + ft + قد + + + + MapPanel + + Current Destination + الوجهة الحالية + + + CLEAR + مسح + + + Recent Destinations + الوجهات الأخيرة + + + Try the Navigation Beta + جرب التنقل التجريبي + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + احصل على الاتجاهات خطوة بخطوة معروضة والمزيد باستخدام comma +الاشتراك الرئيسي. اشترك الآن: https://connect.comma.ai + + + No home +location set + لم يتم تعيين +موقع المنزل + + + No work +location set + لم يتم تعيين +موقع العمل + + + no recent destinations + لا توجد وجهات حديثة + + + + MapWindow + + Map Loading + تحميل الخريطة + + + Waiting for GPS + في انتظار GPS + + + + MultiOptionDialog + + Select + اختر + + + Cancel + إلغاء + + + + Networking + + Advanced + متقدم + + + Enter password + أدخل كلمة المرور + + + for "%1" + ل "%1" + + + Wrong password + كلمة مرور خاطئة + + + + OffroadHome + + UPDATE + تحديث + + + ALERTS + تنبيهات + + + ALERT + تنبيه + + + + PairingPopup + + Pair your device to your comma account + قم بإقران جهازك بحساب comma الخاص بك + + + Go to https://connect.comma.ai on your phone + اذهب إلى https://connect.comma.ai من هاتفك + + + Click "add new device" and scan the QR code on the right + انقر على "إضافة جهاز جديد" وامسح رمز الاستجابة السريعة على اليمين + + + Bookmark connect.comma.ai to your home screen to use it like an app + ضع إشارة مرجعية على connect.comma.ai على شاشتك الرئيسية لاستخدامه مثل أي تطبيق + + + + PrimeAdWidget + + Upgrade Now + قم بالترقية الآن + + + Become a comma prime member at connect.comma.ai + كن عضوًا comme prime في connect.comma.ai + + + PRIME FEATURES: + ميزات PRIME: + + + Remote access + الوصول عن بعد + + + 1 year of storage + سنة واحدة من التخزين + + + Developer perks + امتيازات المطور + + + + PrimeUserWidget + + ✓ SUBSCRIBED + ✓ مشترك + + + comma prime + comma prime + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + COMMA POINTS + COMMA POINTS + + + + QObject + + Reboot + اعادة التشغيل + + + Exit + أغلق + + + dashcam + dashcam + + + openpilot + openpilot + + + %n minute(s) ago + + منذ %n دقيقة + منذ %n دقيقة + منذ %n دقائق + منذ %n دقائق + منذ %n دقائق + منذ %n دقائق + + + + %n hour(s) ago + + منذ %n ساعة + منذ %n ساعة + منذ %n ساعات + منذ %n ساعات + منذ %n ساعات + منذ %n ساعات + + + + %n day(s) ago + + منذ %n يوم + منذ %n يوم + منذ %n ايام + منذ %n ايام + منذ %n ايام + منذ %n ايام + + + + + Reset + + Reset failed. Reboot to try again. + فشل إعادة التعيين. أعد التشغيل للمحاولة مرة أخرى. + + + 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 + إلغاء + + + Reboot + اعادة التشغيل + + + Confirm + تأكيد + + + Unable to mount data partition. Press confirm to reset your device. + تعذر تحميل قسم البيانات. اضغط على تأكيد لإعادة ضبط جهازك. + + + + RichTextDialog + + Ok + موافق + + + + SettingsWindow + + × + x + + + Device + جهاز + + + Network + شبكة الاتصال + + + Toggles + التبديل + + + Software + برمجة + + + Navigation + ملاحة + + + + Setup + + WARNING: Low Voltage + تحذير: الجهد المنخفض + + + Power your device in a car with a harness or proceed at your own risk. + قم بتشغيل جهازك في سيارة باستخدام أداة تثبيت أو المضي قدمًا على مسؤوليتك الخاصة. + + + Power off + اطفئ الجهاز + + + Continue + أكمل + + + Getting Started + ابدء + + + Before we get on the road, let’s finish installation and cover some details. + قبل أن ننطلق على الطريق ، دعنا ننتهي من التثبيت ونغطي بعض التفاصيل. + + + Connect to Wi-Fi + اتصل بشبكة Wi-Fi + + + Back + خلف + + + Continue without Wi-Fi + استمر بدون Wi-Fi + + + Waiting for internet + في انتظار الاتصال بالإنترنت + + + Choose Software to Install + اختر البرنامج المراد تثبيته + + + Dashcam + Dashcam + + + Custom Software + برامج مخصصة + + + Enter URL + إدخال عنوان الموقع + + + for Custom Software + للبرامج المخصصة + + + Downloading... + جارى التحميل... + + + Download Failed + فشل التنزيل + + + Ensure the entered URL is valid, and the device’s internet connection is good. + تأكد من أن عنوان موقع الويب الذي تم إدخاله صالح ، وأن اتصال الجهاز بالإنترنت جيد. + + + Reboot device + إعادة تشغيل الجهاز + + + Start over + ابدأ من جديد + + + + SetupWidget + + Finish Setup + إنهاء الإعداد + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + قم بإقران جهازك بفاصلة connect (connect.comma.ai) واطلب عرض comma prime الخاص بك. + + + Pair device + إقران الجهاز + + + + Sidebar + + CONNECT + الاتصال + + + OFFLINE + غير متصل + + + ONLINE + متصل + + + ERROR + خطأ + + + TEMP + درجة الحرارة + + + HIGH + عالي + + + GOOD + جيد + + + OK + موافق + + + VEHICLE + مركبة + + + NO + لا + + + PANDA + PANDA + + + GPS + GPS + + + SEARCH + بحث + + + -- + -- + + + Wi-Fi + Wi-Fi + + + ETH + ETH + + + 2G + 2G + + + 3G + 3G + + + LTE + LTE + + + 5G + 5G + + + + SoftwarePanel + + Git Branch + Git Branch + + + Git Commit + Git Commit + + + OS Version + إصدار نظام التشغيل + + + Version + إصدار + + + Last Update Check + التحقق من آخر تحديث + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + آخر مرة نجح برنامج openpilot في التحقق من التحديث. يعمل المحدث فقط أثناء إيقاف تشغيل السيارة. + + + Check for Update + فحص التحديثات + + + CHECKING + تدقيق + + + Switch Branch + تبديل الفرع + + + ENTER + أدخل + + + The new branch will be pulled the next time the updater runs. + سيتم سحب الفرع الجديد في المرة التالية التي يتم فيها تشغيل أداة التحديث. + + + Enter branch name + أدخل اسم الفرع + + + UNINSTALL + الغاء التثبيت + + + Uninstall %1 + الغاء التثبيت %1 + + + Are you sure you want to uninstall? + هل أنت متأكد أنك تريد إلغاء التثبيت؟ + + + failed to fetch update + فشل في جلب التحديث + + + CHECK + تأكد الان + + + Updates are only downloaded while the car is off. + + + + Current Version + + + + Download + + + + Install Update + + + + INSTALL + + + + Target Branch + + + + SELECT + + + + Select a branch + + + + + SshControl + + SSH Keys + SSH Keys + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + تحذير: هذا يمنح SSH الوصول إلى جميع المفاتيح العامة في إعدادات GitHub. لا تدخل أبدًا اسم مستخدم GitHub بخلاف اسم المستخدم الخاص بك. لن يطلب منك موظف comma أبدًا إضافة اسم مستخدم GitHub الخاص به. + + + ADD + أضف + + + Enter your GitHub username + أدخل اسم مستخدم GitHub الخاص بك + + + LOADING + جار التحميل + + + REMOVE + نزع + + + Username '%1' has no keys on GitHub + لا يحتوي اسم المستخدم '%1' على مفاتيح على GitHub + + + Request timed out + انتهت مهلة الطلب + + + Username '%1' doesn't exist on GitHub + اسم المستخدم '%1' غير موجود على GitHub + + + + SshToggle + + Enable SSH + تفعيل SSH + + + + TermsPage + + Terms & Conditions + البنود و الظروف + + + Decline + انحدار + + + Scroll to accept + قم بالتمرير للقبول + + + Agree + موافق + + + + TogglesPanel + + Enable openpilot + تمكين openpilot + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + استخدم نظام الطيار المفتوح للتحكم التكيفي في ثبات السرعة والحفاظ على مساعدة السائق. انتباهك مطلوب في جميع الأوقات لاستخدام هذه الميزة. يسري تغيير هذا الإعداد عند إيقاف تشغيل السيارة. + + + Enable Lane Departure Warnings + قم بتمكين تحذيرات مغادرة حارة السير + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + تلقي تنبيهات للتوجه مرة أخرى إلى الحارة عندما تنجرف سيارتك فوق خط المسار المكتشف دون تنشيط إشارة الانعطاف أثناء القيادة لمسافة تزيد عن 31 ميلاً في الساعة (50 كم / ساعة). + + + Use Metric System + استخدم النظام المتري + + + Display speed in km/h instead of mph. + عرض السرعة بالكيلو متر في الساعة بدلاً من ميل في الساعة. + + + Record and Upload Driver Camera + تسجيل وتحميل كاميرا السائق + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + قم بتحميل البيانات من الكاميرا المواجهة للسائق وساعد في تحسين خوارزمية مراقبة السائق. + + + Disengage on Accelerator Pedal + فك الارتباط على دواسة التسريع + + + When enabled, pressing the accelerator pedal will disengage openpilot. + عند التمكين ، سيؤدي الضغط على دواسة الوقود إلى فصل الطيار المفتوح. + + + Show ETA in 24h Format + إظهار الوقت المقدر للوصول بتنسيق 24 ساعة + + + Use 24h format instead of am/pm + استخدم تنسيق 24 ساعة بدلاً من صباحًا / مساءً + + + Show Map on Left Side of UI + إظهار الخريطة على الجانب الأيسر من واجهة المستخدم + + + Show map on left side when in split screen view. + إظهار الخريطة على الجانب الأيسر عندما تكون في طريقة عرض الشاشة المنقسمة. + + + openpilot Longitudinal Control + openpilot التحكم الطولي + + + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! + سوف يقوم برنامج openpilot بتعطيل رادار السيارة وسيتولى التحكم في الغاز والمكابح. تحذير: هذا يعطل AEB! + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + + + + Experimental openpilot Longitudinal Control + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + + + + openpilot longitudinal control is not currently available for this car. + + + + Enable experimental longitudinal control to enable this. + + + + + Updater + + Update Required + مطلوب التحديث + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + مطلوب تحديث نظام التشغيل. قم بتوصيل جهازك بشبكة Wi-Fi للحصول على أسرع تجربة تحديث. حجم التنزيل 1 غيغابايت تقريبًا. + + + Connect to Wi-Fi + اتصل بشبكة Wi-Fi + + + Install + ثبيت + + + Back + خلف + + + Loading... + جار التحميل... + + + Reboot + اعادة التشغيل + + + Update failed + فشل التحديث + + + + WifiUI + + Scanning for networks... + جارٍ البحث عن شبكات ... + + + CONNECTING... + جارٍ الاتصال ... + + + FORGET + نزع + + + Forget Wi-Fi Network "%1"? + نزع شبكة اWi-Fi "%1"? + + + diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index dd9f933dc..963eeadd8 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -4,17 +4,14 @@ AbstractAlert - Close 閉じる - Snooze Update 更新の一時停止 - Reboot and Update 再起動してアップデート @@ -22,67 +19,84 @@ AdvancedNetworking - Back 戻る - Enable Tethering ﾃｻﾞﾘﾝｸﾞを有効化 - Tethering Password ﾃｻﾞﾘﾝｸﾞﾊﾟｽﾜｰﾄﾞ - - EDIT 編集 - Enter new tethering password 新しいテザリングパスワードを入力 - IP Address IP アドレス - Enable Roaming ﾛｰﾐﾝｸﾞを有効化 - APN Setting APN 設定 - Enter APN APN を入力 - leave blank for automatic configuration - 空白のままにして、自動設定にします + 自動で設定するには、空白のままにしてください。 + + + Cellular Metered + 従量制通信設定 + + + Prevent large data uploads when on a metered connection + 大量のデータのアップロードを防止します。 + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + 最高速度 + + + SPEED + 速度 + + + LIMIT + 制限速度 ConfirmationDialog - - Ok OK - Cancel キャンセル @@ -90,17 +104,14 @@ DeclinePage - You must accept the Terms and Conditions in order to use openpilot. openpilot をご利用される前に、利用規約に同意する必要があります。 - Back 戻る - Decline, uninstall %1 拒否して %1 をｱﾝｲﾝｽﾄｰﾙ @@ -108,185 +119,157 @@ DevicePanel - Dongle ID ドングル番号 (Dongle ID) - N/A N/A - Serial シリアル番号 - Driver Camera 車内カメラ - PREVIEW - 見る + プレビュー - Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) 車内カメラをプレビューして、ドライバー監視システムの視界を確認ができます。(車両の電源を切る必要があります) - Reset Calibration ｷｬﾘﾌﾞﾚｰｼｮﾝをﾘｾｯﾄ - RESET ﾘｾｯﾄ - Are you sure you want to reset calibration? ｷｬﾘﾌﾞﾚｰｼｮﾝをリセットしてもよろしいですか？ - Review Training Guide - 入門書を見る + 使い方の確認 - REVIEW 見る - Review the rules, features, and limitations of openpilot openpilot の特徴を見る - Are you sure you want to review the training guide? - 入門書を見てもよろしいですか？ + 使い方の確認をしますか？ - Regulatory 認証情報 - VIEW 見る - Change Language 言語を変更 - CHANGE 変更 - Select a language 言語を選択 - Reboot 再起動 - Power Off 電源を切る - 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. - このデバイスは%2の%1°、%4の%3°に向けます。 + このデバイスは%2 %1°、%4 %3°の向きに設置されています。 - down 下 - up 上 - left 左 - right 右 - Are you sure you want to reboot? 再起動してもよろしいですか？ - Disengage to Reboot openpilot をキャンセルして再起動ができます - Are you sure you want to power off? シャットダウンしてもよろしいですか？ - Disengage to Power Off openpilot をキャンセルしてシャットダウンができます + + Reset + リセット + + + Review + 確認 + DriveStats - Drives 運転履歴 - Hours 時間 - ALL TIME 累計 - PAST WEEK 先週 - KM km - Miles マイル @@ -294,20 +277,28 @@ DriverViewScene - camera starting ｶﾒﾗを起動しています + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + + + + CHILL MODE ON + + + InputDialog - Cancel キャンセル - Need at least %n character(s)! %n文字以上でお願いします！ @@ -317,22 +308,18 @@ Installer - Installing... インストールしています... - Receiving objects: ｵﾌﾞｼﾞｪｸﾄをﾀﾞｳﾝﾛｰﾄﾞしています： - Resolving deltas: デルタを解決しています： - Updating files: ファイルを更新しています： @@ -340,27 +327,22 @@ MapETA - eta - 予定到着時間 + 到着予定時間 - min 分 - hr 時間 - km ｷﾛﾒｰﾄﾙ - mi ﾏｲﾙ @@ -368,22 +350,18 @@ MapInstructions - km ｷﾛﾒｰﾄﾙ - m ﾒｰﾄﾙ - mi ﾏｲﾙ - ft ﾌｨｰﾄ @@ -391,48 +369,40 @@ MapPanel - Current Destination 現在の目的地 - CLEAR 削除 - Recent Destinations 最近の目的地 - Try the Navigation Beta β版ﾅﾋﾞｹﾞｰｼｮﾝを試す - Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai より詳細な案内情報を得ることができます。 詳しくはこちら：https://connect.comma.ai - No home location set 自宅の住所はまだ 設定されていません - No work location set 職場の住所はまだ 設定されていません - no recent destinations 最近の目的地履歴がありません @@ -440,12 +410,10 @@ location set MapWindow - Map Loading マップを読み込んでいます - Waiting for GPS GPS信号を探しています @@ -453,12 +421,10 @@ location set MultiOptionDialog - Select 選択 - Cancel キャンセル @@ -466,72 +432,33 @@ location set Networking - Advanced 詳細 - Enter password パスワードを入力 - - for "%1" ﾈｯﾄﾜｰｸ名：%1 - Wrong password ﾊﾟｽﾜｰﾄﾞが間違っています - - NvgWindow - - - km/h - km/h - - - - mph - mph - - - - - MAX - 最高速度 - - - - - SPEED - 速度 - - - - - LIMIT - 制限速度 - - OffroadHome - UPDATE 更新 - ALERTS 警告 - ALERT 警告 @@ -539,55 +466,56 @@ location set PairingPopup - Pair your device to your comma account デバイスと comma アカウントを連携する - Go to https://connect.comma.ai on your phone - モバイルデバイスで「connect.comma.ai」にアクセスして + スマートフォンで「https://connect.comma.ai」にアクセスしてください。 - Click "add new device" and scan the QR code on the right - 「新しいデバイスを追加」を押すと、右側のQRコードをスキャンしてください + 「新しいデバイスを追加」を押し、右側のQRコードをスキャンしてください。 - Bookmark connect.comma.ai to your home screen to use it like an app - 「connect.comma.ai」をホーム画面に追加して、アプリのように使うことができます + 「connect.comma.ai」をホーム画面に追加して、アプリのように使うことができます。 + + + + ParamControl + + Cancel + キャンセル + + + Enable + を有効化 PrimeAdWidget - Upgrade Now 今すぐｱｯﾌﾟｸﾞﾚｰﾄ - Become a comma prime member at connect.comma.ai connect.comma.ai でﾌﾟﾗｲﾑ会員に登録できます - PRIME FEATURES: 特典： - Remote access リモートアクセス - 1 year of storage 一年間の保存期間 - Developer perks 開発者向け特典 @@ -595,22 +523,18 @@ location set PrimeUserWidget - ✓ SUBSCRIBED ✓ 入会しました - comma prime comma prime - CONNECT.COMMA.AI CONNECT.COMMA.AI - COMMA POINTS COMMA POINTS @@ -618,41 +542,34 @@ location set QObject - Reboot 再起動 - Exit 閉じる - dashcam ﾄﾞﾗｲﾌﾞﾚｺｰﾀﾞｰ - openpilot openpilot - %n minute(s) ago %n 分前 - %n hour(s) ago %n 時間前 - %n day(s) ago %n 日前 @@ -662,89 +579,65 @@ location set Reset - Reset failed. Reboot to try again. 初期化に失敗しました。再起動後に再試行してください。 - 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 キャンセル - Reboot 再起動 - Confirm 確認 - Unable to mount data partition. Press confirm to reset your device. 「data」パーティションをマウントできません。「確認」ボタンを押すとデバイスが初期化されます。 - - RichTextDialog - - - Ok - OK - - SettingsWindow - × × - Device デバイス - - Network ﾈｯﾄﾜｰｸ - Toggles - 切り替え + 機能設定 - Software ｿﾌﾄｳｪｱ - Navigation ﾅﾋﾞｹﾞｰｼｮﾝ @@ -752,105 +645,82 @@ location set Setup - WARNING: Low Voltage 警告：低電圧 - Power your device in a car with a harness or proceed at your own risk. - 自己責任でハーネスから電源を供給してください。 + 自己責任で実行を継続するか、ハーネスから電源を供給してください。 - Power off 電源を切る - - - Continue 続ける - Getting Started はじめに - Before we get on the road, let’s finish installation and cover some details. - その前に、インストールを完了し、いくつかの詳細を説明します。 + 道路に向かう前に、インストールを完了して使い方を確認しましょう。 - Connect to Wi-Fi Wi-Fi に接続 - - Back 戻る - Continue without Wi-Fi - Wi-Fi に未接続で続行 + Wi-Fi に接続せずに続行 - Waiting for internet インターネット接続を待機中 - Choose Software to Install - インストールするソフトウェアを選びます + インストールするソフトウェアを選択してください - Dashcam ﾄﾞﾗｲﾌﾞﾚｺｰﾀﾞｰ - Custom Software カスタムソフトウェア - Enter URL URL を入力 - for Custom Software カスタムソフトウェア - Downloading... ダウンロード中... - Download Failed ダウンロード失敗 - Ensure the entered URL is valid, and the device’s internet connection is good. 入力された URL を確認し、デバイスがインターネットに接続されていることを確認してください。 - Reboot device デバイスを再起動 - Start over 最初からやり直す @@ -858,17 +728,14 @@ location set SetupWidget - Finish Setup セットアップ完了 - Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. - デバイスを comma connect (connect.comma.ai)でペアリングし comma prime 特典を申請してください。 + デバイスを comma connect (connect.comma.ai)でペアリングし、comma primeの特典を申請してください。 - Pair device デバイスをペアリング @@ -876,106 +743,82 @@ location set Sidebar - - CONNECT 接続 - OFFLINE オフライン - - ONLINE オンライン - ERROR エラー - - - TEMP 温度 - HIGH 高温 - GOOD 最適 - OK OK - VEHICLE 車両 - NO NO - PANDA PANDA - GPS GPS - SEARCH 検索 - -- -- - Wi-Fi Wi-Fi - ETH ETH - 2G 2G - 3G 3G - LTE LTE - 5G 5G @@ -983,138 +826,93 @@ location set SoftwarePanel - - Git Branch - Git ブランチ + Updates are only downloaded while the car is off. + 車の電源がオフの間のみ、アップデートのダウンロードが行われます。 - - Git Commit - Git コミット + Current Version + 現在のバージョン - - OS Version - OS バージョン + Download + ダウンロード - - Version - バージョン + Install Update + アップデート - - Last Update Check - 最終更新確認 + INSTALL + インストール - - The last time openpilot successfully checked for an update. The updater only runs while the car is off. - openpilotが最後にアップデートの確認に成功してからの時間です。アップデート処理は、車の電源が切れているときのみ実行されます。 + Target Branch + 対象のブランチ - - Check for Update - 更新プログラムをチェック + SELECT + 選択 - - CHECKING - 確認中 + Select a branch + ブランチを選択 - - Switch Branch - ブランチの切り替え - - - - ENTER - 切替 - - - - - The new branch will be pulled the next time the updater runs. - updater を実行する時にブランチを切り替えます。 - - - - Enter branch name - ブランチ名を入力 - - - UNINSTALL - アンインストール + 実行 - Uninstall %1 %1をアンインストール - Are you sure you want to uninstall? アンインストールしてもよろしいですか？ - - failed to fetch update - 更新のﾀﾞｳﾝﾛｰﾄﾞにｴﾗｰが発生しました - - - - CHECK 確認 + + Uninstall + アンインストール + SshControl - SSH Keys SSH 鍵 - Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. - 警告: これは、GitHub の設定にあるすべての公開鍵への SSH アクセスを許可するものです。自分以外の GitHub のユーザー名を入力しないでください。コンマのスタッフが GitHub のユーザー名を追加するようお願いすることはありません。 + 警告: これは、GitHub の設定にあるすべての公開鍵への SSH アクセスを許可するものです。自分以外の GitHub のユーザー名を入力しないでください。commaのスタッフが GitHub のユーザー名を追加するようお願いすることはありません。 - - ADD 追加 - Enter your GitHub username GitHub のユーザー名を入力してください - LOADING - ローディング + 読み込み中 - REMOVE 削除 - Username '%1' has no keys on GitHub ユーザー名 “%1” は GitHub に鍵がありません - Request timed out リクエストタイムアウト - Username '%1' doesn't exist on GitHub ユーザー名 '%1' は GitHub に存在しません @@ -1122,7 +920,6 @@ location set SshToggle - Enable SSH SSH を有効化 @@ -1130,22 +927,18 @@ location set TermsPage - Terms & Conditions 利用規約 - Decline 拒否 - Scroll to accept スクロールして同意 - Agree 同意 @@ -1153,125 +946,121 @@ location set TogglesPanel - Enable openpilot openpilot を有効化 - Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. - アダプティブクルーズコントロールとレーンキーピングドライバーアシスト（openpilotシステム）。この機能を使用するには、常に注意が必要です。この設定を変更すると、車の電源が切れたときに有効になります。 + openpilotによるアダプティブクルーズコントロールとレーンキーピングドライバーアシストを利用します。この機能を利用する際は、常に前方への注意が必要です。この設定を変更すると、車の電源が切れた時に反映されます。 - Enable Lane Departure Warnings 車線逸脱警報機能を有効化 - Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). - 時速31マイル（50km）を超えるスピードで走行中、方向指示器を作動させずに検出された車線ライン上に車両が触れた場合、車線に戻るアラートを受信します。 + 時速31マイル（50km）を超えるスピードで走行中、ウインカーを作動させずに検出された車線ライン上に車両が触れた場合、手動で車線内に戻るように警告を行います。 - Use Metric System - メートル法を有効化 + メートル法を使用 - Display speed in km/h instead of mph. 速度は mph ではなく km/h で表示されます。 - Record and Upload Driver Camera 車内カメラの録画とアップロード - Upload data from the driver facing camera and help improve the driver monitoring algorithm. 車内カメラの映像をアップロードし、ドライバー監視システムのアルゴリズムの向上に役立てます。 - - Disengage On Accelerator Pedal - ｱｸｾﾙ踏むと openpilot をｷｬﾝｾﾙ + Experimental openpilot Longitudinal Control + 実験段階のopenpilotによるアクセル制御 + + + Disengage on Accelerator Pedal + ｱｸｾﾙを踏むと openpilot を中断 - When enabled, pressing the accelerator pedal will disengage openpilot. - 有効な場合は、アクセルを踏むと openpilot をキャンセルします。 + この機能を有効化すると、openpilotを利用中にアクセルを踏むとopenpilotによる運転サポートを中断します。 - Show ETA in 24h Format 24時間表示 - Use 24h format instead of am/pm AM/PM の代わりに24時間形式を使用します - Show Map on Left Side of UI ﾃﾞｨｽﾌﾟﾚｲの左側にﾏｯﾌﾟを表示 - Show map on left side when in split screen view. 分割画面表示の場合、ディスプレイの左側にマップを表示します。 - - openpilot Longitudinal Control - openpilot 縦方向制御 + Experimental Mode + 実験モード - - openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! - openpilot は、車のレーダーを無効化し、アクセルとブレーキの制御を引き継ぎます。注意：AEB を無効化にします！ + 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 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: <br> <h4>🌮 End-to-End Longitudinal Control 🌮</h4> Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. <br> <h4>New Driving Visualization</h4> The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + + + + Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. + + + + Enable experimental longitudinal control to allow experimental mode. + Updater - Update Required - 更新が必要です + アップデートが必要です - An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. - ｵﾍﾟﾚｰﾃｨﾝｸﾞｼｽﾃﾑのアップデートが必要です。Wi-Fi に接続することで、最速のアップデートを体験できます。ダウンロードサイズは約 1GB です。 + ｵﾍﾟﾚｰﾃｨﾝｸﾞｼｽﾃﾑのアップデートが必要です。Wi-Fi に接続してアップデートする事をお勧めします。ダウンロードサイズは約 1GB です。 - Connect to Wi-Fi Wi-Fi に接続 - Install インストール - Back 戻る - Loading... 読み込み中... - Reboot 再起動 - Update failed 更新失敗 @@ -1279,25 +1068,24 @@ location set WifiUI - - Scanning for networks... ネットワークをスキャン中... - CONNECTING... 接続中... - FORGET 削除 - Forget Wi-Fi Network "%1"? Wi-Fiﾈｯﾄﾜｰｸ%1を削除してもよろしいですか？ + + Forget + 削除 + diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index d4abe8619..9defc2c36 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -4,17 +4,14 @@ AbstractAlert - Close 닫기 - Snooze Update 업데이트 일시중지 - Reboot and Update 업데이트 및 재부팅 @@ -22,67 +19,84 @@ AdvancedNetworking - Back 뒤로 - Enable Tethering 테더링 사용 - Tethering Password 테더링 비밀번호 - - EDIT 편집 - Enter new tethering password 새 테더링 비밀번호를 입력하세요 - IP Address IP 주소 - Enable Roaming 로밍 사용 - APN Setting APN 설정 - Enter APN APN 입력 - leave blank for automatic configuration - 자동설정을 하려면 공백으로 두세요 + 자동설정하려면 공백으로 두세요 + + + Cellular Metered + 데이터 요금제 + + + Prevent large data uploads when on a metered connection + 데이터 요금제 연결 시 대용량 데이터 업로드 방지 + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + MAX + + + SPEED + SPEED + + + LIMIT + LIMIT ConfirmationDialog - - Ok 확인 - Cancel 취소 @@ -90,17 +104,14 @@ DeclinePage - You must accept the Terms and Conditions in order to use openpilot. - openpilot을 사용하려면 이용 약관에 동의해야 합니다. + openpilot을 사용하려면 이용약관에 동의해야 합니다. - Back 뒤로 - Decline, uninstall %1 거절, %1 제거 @@ -108,185 +119,157 @@ DevicePanel - Dongle ID Dongle ID - N/A N/A - Serial Serial - Driver Camera 운전자 카메라 - PREVIEW 미리보기 - Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) 운전자 모니터링이 좋은 가시성을 갖도록 운전자를 향한 카메라를 미리 봅니다. (차량연결은 해제되어있어야 합니다) - Reset Calibration - 캘리브레이션 재설정 + 캘리브레이션 - RESET 재설정 - Are you sure you want to reset calibration? 캘리브레이션을 재설정하시겠습니까? - Review Training Guide - 트레이닝 가이드 다시보기 + 트레이닝 가이드 - REVIEW 다시보기 - Review the rules, features, and limitations of openpilot openpilot의 규칙, 기능 및 제한 다시보기 - Are you sure you want to review the training guide? 트레이닝 가이드를 다시보시겠습니까? - Regulatory 규제 - VIEW 보기 - Change Language 언어 변경 - CHANGE 변경 - Select a language 언어를 선택하세요 - Reboot 재부팅 - Power Off 전원 종료 - 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은 지속적으로 보정되므로 리셋이 거의 필요하지 않습니다. + openpilot은 좌우측은 4° 이내, 위쪽은 5° 아래쪽은 8° 이내로 장치를 설치해야 합니다. openpilot은 지속적으로 보정되므로 리셋은 거의 필요하지 않습니다. - Your device is pointed %1° %2 and %3° %4. - 사용자의 장치가 %1° %2 및 %3° %4를 가리키고 있습니다. + 사용자의 장치가 %1° %2 및 %3° %4 위치에 설치되어있습니다. - down 아래로 - up 위로 - left 좌측으로 - right 우측으로 - Are you sure you want to reboot? 재부팅 하시겠습니까? - Disengage to Reboot 재부팅 하려면 해제하세요 - Are you sure you want to power off? 전원을 종료하시겠습니까? - Disengage to Power Off 전원을 종료하려면 해제하세요 + + Reset + 리셋 + + + Review + 다시보기 + DriveStats - Drives 주행 - Hours 시간 - ALL TIME 전체 - PAST WEEK 지난주 - KM Km - Miles Miles @@ -294,20 +277,28 @@ DriverViewScene - camera starting 카메라 시작중 + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + + + + CHILL MODE ON + + + InputDialog - Cancel 취소 - Need at least %n character(s)! 최소 %n 자가 필요합니다! @@ -317,22 +308,18 @@ Installer - Installing... 설치중... - Receiving objects: 수신중： - Resolving deltas: 델타병합： - Updating files: 파일갱신： @@ -340,27 +327,22 @@ MapETA - eta 도착 - min 분 - hr 시간 - km km - mi mi @@ -368,22 +350,18 @@ MapInstructions - km km - m m - mi mi - ft ft @@ -391,48 +369,40 @@ MapPanel - Current Destination 현재 목적지 - CLEAR 삭제 - Recent Destinations 최근 목적지 - Try the Navigation Beta 네비게이션(베타)를 사용해보세요 - Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai 자세한 경로안내를 원하시면 comma prime을 구독하세요. 등록：https://connect.comma.ai - No home location set 집 설정되지않음 - No work location set 회사 설정되지않음 - no recent destinations 최근 목적지 없음 @@ -440,12 +410,10 @@ location set MapWindow - Map Loading 지도 로딩 - Waiting for GPS GPS를 기다리는 중 @@ -453,12 +421,10 @@ location set MultiOptionDialog - Select 선택 - Cancel 취소 @@ -466,72 +432,33 @@ location set Networking - Advanced 고급 설정 - Enter password 비밀번호를 입력하세요 - - for "%1" - 하기위한 "%1" + "%1"에 접속하려면 인증이 필요합니다 - Wrong password 비밀번호가 틀렸습니다 - - NvgWindow - - - km/h - km/h - - - - mph - mph - - - - - MAX - MAX - - - - - SPEED - SPEED - - - - - LIMIT - LIMIT - - OffroadHome - UPDATE 업데이트 - ALERTS 알림 - ALERT 알림 @@ -539,55 +466,56 @@ location set PairingPopup - Pair your device to your comma account 장치를 콤마 계정과 페어링합니다 - Go to https://connect.comma.ai on your phone https://connect.comma.ai에 접속하세요 - Click "add new device" and scan the QR code on the right "새 장치 추가"를 클릭하고 오른쪽 QR 코드를 검색합니다 - Bookmark connect.comma.ai to your home screen to use it like an app connect.comma.ai을 앱처럼 사용하려면 홈 화면에 바로가기를 만드십시오 + + ParamControl + + Cancel + 취소 + + + Enable + 사용 + + PrimeAdWidget - Upgrade Now 지금 업그레이드 - Become a comma prime member at connect.comma.ai connect.comma.ai에서 comma prime에 가입합니다 - PRIME FEATURES: PRIME 기능： - Remote access 원격 접속 - 1 year of storage 1년간 저장 - Developer perks 개발자 혜택 @@ -595,22 +523,18 @@ location set PrimeUserWidget - ✓ SUBSCRIBED ✓ 구독함 - comma prime comma prime - CONNECT.COMMA.AI CONNECT.COMMA.AI - COMMA POINTS COMMA POINTS @@ -618,41 +542,34 @@ location set QObject - Reboot 재부팅 - Exit 종료 - dashcam dashcam - openpilot openpilot - %n minute(s) ago %n 분전 - %n hour(s) ago %n 시간전 - %n day(s) ago %n 일전 @@ -662,89 +579,65 @@ location set Reset - Reset failed. Reboot to try again. 초기화 실패. 재부팅후 다시 시도하세요. - 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 취소 - Reboot 재부팅 - Confirm 확인 - Unable to mount data partition. Press confirm to reset your device. 데이터 파티션을 마운트할 수 없습니다. 확인 버튼을 눌러 장치를 리셋합니다. - - RichTextDialog - - - Ok - 확인 - - SettingsWindow - × × - Device 장치 - - Network 네트워크 - Toggles 토글 - Software 소프트웨어 - Navigation 네비게이션 @@ -752,105 +645,82 @@ location set Setup - WARNING: Low Voltage 경고: 전압이 낮습니다 - Power your device in a car with a harness or proceed at your own risk. 하네스 보드에 차량의 전원을 연결하세요. - Power off 전원 종료 - - - Continue 계속 - Getting Started 설정 시작 - Before we get on the road, let’s finish installation and cover some details. 출발하기 전에 설정을 완료하고 몇 가지 세부 사항을 살펴보겠습니다. - Connect to Wi-Fi wifi 연결 - - Back 뒤로 - Continue without Wi-Fi - wifi 없이 계속 + wifi 연결없이 계속하기 - Waiting for internet 네트워크 접속을 기다립니다 - Choose Software to Install 설치할 소프트웨어를 선택하세요 - Dashcam Dashcam - Custom Software Custom Software - Enter URL URL 입력 - for Custom Software for Custom Software - Downloading... 다운로드중... - Download Failed 다운로드 실패 - Ensure the entered URL is valid, and the device’s internet connection is good. - 입력된 URL이 유효하고 장치의 인터넷 연결이 잘 되어 있는지 확인합니다. + 입력된 URL이 유효하고 장치의 네트워크 연결이 잘 되어 있는지 확인하세요. - Reboot device 재부팅 - Start over 다시 시작 @@ -858,17 +728,14 @@ location set SetupWidget - Finish Setup 설정 완료 - Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. 장치를 (connect.comma.ai)에서 페어링하고 comma prime 오퍼를 청구합니다. - Pair device 장치 페어링 @@ -876,106 +743,82 @@ location set Sidebar - - CONNECT 연결 - OFFLINE 오프라인 - - ONLINE 온라인 - ERROR 오류 - - - TEMP 온도 - HIGH 높음 - GOOD 좋음 - OK 경고 - VEHICLE 차량 - NO NO - PANDA PANDA - GPS GPS - SEARCH 검색중 - -- -- - Wi-Fi Wi-Fi - ETH 이더넷 - 2G 2G - 3G 3G - LTE LTE - 5G 5G @@ -983,138 +826,93 @@ location set SoftwarePanel - - Git Branch - Git 브렌치 + Updates are only downloaded while the car is off. + 업데이트는 차량 연결이 해제되어 있는 동안에만 다운로드됩니다. - - Git Commit - Git 커밋 + Current Version + 현재 버전 - - OS Version - OS 버전 + Download + 다운로드 - - Version - 버전 + Install Update + 업데이트 설치 - - Last Update Check - 최신 업데이트 검사 + INSTALL + 설치 - - The last time openpilot successfully checked for an update. The updater only runs while the car is off. - 최근에 openpilot이 업데이트를 성공적으로 확인했습니다. 업데이트 프로그램은 차량 연결이 해제되었을때만 작동합니다. + Target Branch + 대상 브랜치 - - Check for Update - 업데이트 확인 + SELECT + 선택 - - CHECKING - 확인중 + Select a branch + 브랜치 선택 - - Switch Branch - 브랜치 변경 - - - - ENTER - 입력하세요 - - - - - The new branch will be pulled the next time the updater runs. - 다음 업데이트 프로그램이 실행될 때 새 브랜치가 적용됩니다. - - - - Enter branch name - 브랜치명 입력 - - - UNINSTALL 제거 - Uninstall %1 %1 제거 - Are you sure you want to uninstall? 제거하시겠습니까? - - failed to fetch update - 업데이트를 가져올수없습니다 - - - - CHECK 확인 + + Uninstall + 삭제 + SshControl - SSH Keys SSH 키 - Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. - 경고：이렇게 하면 GitHub 설정의 모든 공용 키에 대한 SSH 액세스 권한이 부여됩니다. 자신의 사용자 이름이 아닌 GitHub 사용자 이름을 입력하지 마십시오. comma 직원은 GitHub 사용자 이름을 추가하도록 요청하지 않습니다. + 경고: 허용으로 설정하면 GitHub 설정의 모든 공용 키에 대한 SSH 액세스 권한이 부여됩니다. GitHub 사용자 ID 이외에는 입력하지 마십시오. comma에서는 GitHub ID를 추가하라는 요청을 하지 않습니다. - - ADD 추가 - Enter your GitHub username GitHub 사용자 ID - LOADING 로딩 - REMOVE 제거 - Username '%1' has no keys on GitHub '%1'의 키가 GitHub에 없습니다 - Request timed out 요청 시간 초과 - Username '%1' doesn't exist on GitHub '%1'은 GitHub에 없습니다 @@ -1122,7 +920,6 @@ location set SshToggle - Enable SSH SSH 사용 @@ -1130,22 +927,18 @@ location set TermsPage - Terms & Conditions 약관 - Decline 거절 - Scroll to accept 허용하려면 아래로 스크롤하세요 - Agree 동의 @@ -1153,125 +946,121 @@ location set TogglesPanel - Enable openpilot openpilot 사용 - Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. - 어댑티브 크루즈 컨트롤 및 차선 유지 운전자 보조를 위해 openpilot 시스템을 사용하십시오. 이 기능을 사용하려면 항상 주의를 기울여야 합니다. 이 설정을 변경하면 차량 전원이 꺼질 때 적용됩니다. + 어댑티브 크루즈 컨트롤 및 차선 유지 운전자 보조를 위해 openpilot 시스템을 사용하십시오. 이 기능을 사용하려면 항상 주의를 기울여야 합니다. 설정변경은 장치 재부팅후 적용됩니다. - Enable Lane Departure Warnings 차선 이탈 경고 사용 - Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). - 차량이 50km/h(31mph) 이상의 속도로 주행하는 동안 방향 지시등이 활성화되지 않은 상태에서 감지된 차선 위를 주행할 경우 차선이탈 경고를 사용합니다. + 차량이 50km/h(31mph) 이상의 속도로 주행하는 동안 방향지시등 없이 감지된 차선 위를 주행할 경우 차선이탈 경고를 표시합니다. - Use Metric System 미터법 사용 - Display speed in km/h instead of mph. mph 대신 km/h로 속도를 표시합니다. - Record and Upload Driver Camera 운전자 카메라 녹화 및 업로드 - Upload data from the driver facing camera and help improve the driver monitoring algorithm. 운전자 카메라에서 데이터를 업로드하고 운전자 모니터링 알고리즘을 개선합니다. - - Disengage On Accelerator Pedal + Experimental openpilot Longitudinal Control + openpilot 롱컨트롤 (실험적) + + + Disengage on Accelerator Pedal 가속페달 조작시 해제 - When enabled, pressing the accelerator pedal will disengage openpilot. 활성화된 경우 가속 페달을 누르면 openpilot이 해제됩니다. - Show ETA in 24h Format 24시간 형식으로 도착예정시간 표시 - Use 24h format instead of am/pm 오전/오후 대신 24시간 형식 사용 - Show Map on Left Side of UI UI 왼쪽에 지도 표시 - Show map on left side when in split screen view. 분할 화면 보기에서 지도를 왼쪽에 표시합니다. - - openpilot Longitudinal Control - openpilot Longitudinal Control + Experimental Mode + 실험적 모드 - - openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! - openpilot은 차량'의 레이더를 무력화시키고 가속페달과 브레이크의 제어를 인계받을 것이다. 경고: AEB를 비활성화합니다! + 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 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: <br> <h4>🌮 End-to-End Longitudinal Control 🌮</h4> Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. <br> <h4>New Driving Visualization</h4> The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + + + + Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. + + + + Enable experimental longitudinal control to allow experimental mode. + Updater - Update Required 업데이트 필요 - An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. - OS 업데이트가 필요합니다. 장치를 wifi에 연결하여 가장 빠른 업데이트 경험을 제공합니다. 다운로드 크기는 약 1GB입니다. + OS 업데이트가 필요합니다. 장치를 wifi에 연결하면 가장 빠른 업데이트 경험을 제공합니다. 다운로드 크기는 약 1GB입니다. - Connect to Wi-Fi wifi 연결 - Install 설치 - Back 뒤로 - Loading... 로딩중... - Reboot 재부팅 - Update failed 업데이트 실패 @@ -1279,25 +1068,24 @@ location set WifiUI - - Scanning for networks... 네트워크 검색 중... - CONNECTING... 연결중... - FORGET 저장안함 - Forget Wi-Fi Network "%1"? wifi 네트워크 저장안함 "%1"? + + Forget + 저장안함 + diff --git a/selfdrive/ui/translations/main_nl.ts b/selfdrive/ui/translations/main_nl.ts new file mode 100644 index 000000000..10651a416 --- /dev/null +++ b/selfdrive/ui/translations/main_nl.ts @@ -0,0 +1,1120 @@ + + + + + AbstractAlert + + Close + Sluit + + + Snooze Update + Update uitstellen + + + Reboot and Update + Opnieuw Opstarten en Updaten + + + + AdvancedNetworking + + Back + Terug + + + Enable Tethering + Tethering Inschakelen + + + Tethering Password + Tethering Wachtwoord + + + EDIT + AANPASSEN + + + Enter new tethering password + Voer nieuw tethering wachtwoord in + + + IP Address + IP Adres + + + Enable Roaming + Roaming Inschakelen + + + APN Setting + APN Instelling + + + Enter APN + Voer APN in + + + leave blank for automatic configuration + laat leeg voor automatische configuratie + + + Cellular Metered + + + + Prevent large data uploads when on a metered connection + + + + + AnnotatedCameraWidget + + km/h + km/u + + + mph + mph + + + MAX + MAX + + + SPEED + SPEED + + + LIMIT + LIMIT + + + + ConfirmationDialog + + Ok + Ok + + + Cancel + Annuleren + + + + DeclinePage + + You must accept the Terms and Conditions in order to use openpilot. + U moet de Algemene Voorwaarden accepteren om openpilot te gebruiken. + + + Back + Terug + + + Decline, uninstall %1 + Afwijzen, verwijder %1 + + + + DevicePanel + + Dongle ID + Dongle ID + + + N/A + Nvt + + + Serial + Serienummer + + + Driver Camera + Bestuurders Camera + + + PREVIEW + BEKIJKEN + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + Bekijk de naar de bestuurder gerichte camera om ervoor te zorgen dat het monitoren van de bestuurder goed zicht heeft. (Voertuig moet uitgschakeld zijn) + + + Reset Calibration + Kalibratie Resetten + + + RESET + RESET + + + Are you sure you want to reset calibration? + Weet u zeker dat u de kalibratie wilt resetten? + + + Review Training Guide + Doorloop de Training Opnieuw + + + REVIEW + BEKIJKEN + + + Review the rules, features, and limitations of openpilot + Bekijk de regels, functies en beperkingen van openpilot + + + Are you sure you want to review the training guide? + Weet u zeker dat u de training opnieuw wilt doorlopen? + + + Regulatory + Regelgeving + + + VIEW + BEKIJKEN + + + Change Language + Taal Wijzigen + + + CHANGE + WIJZIGEN + + + Select a language + Selecteer een taal + + + Reboot + Opnieuw Opstarten + + + Power Off + Uitschakelen + + + 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 vereist dat het apparaat binnen 4° links of rechts en binnen 5° omhoog of 8° omlaag wordt gemonteerd. openpilot kalibreert continu, resetten is zelden nodig. + + + Your device is pointed %1° %2 and %3° %4. + Uw apparaat is gericht op %1° %2 en %3° %4. + + + down + omlaag + + + up + omhoog + + + left + links + + + right + rechts + + + Are you sure you want to reboot? + Weet u zeker dat u opnieuw wilt opstarten? + + + Disengage to Reboot + Deactiveer openpilot om opnieuw op te starten + + + Are you sure you want to power off? + Weet u zeker dat u wilt uitschakelen? + + + Disengage to Power Off + Deactiveer openpilot om uit te schakelen + + + + DriveStats + + Drives + Ritten + + + Hours + Uren + + + ALL TIME + TOTAAL + + + PAST WEEK + AFGELOPEN WEEK + + + KM + Km + + + Miles + Mijl + + + + DriverViewScene + + camera starting + Camera wordt gestart + + + + InputDialog + + Cancel + Annuleren + + + Need at least %n character(s)! + + Heeft minstens %n karakter nodig! + Heeft minstens %n karakters nodig! + + + + + Installer + + Installing... + Installeren... + + + Receiving objects: + Objecten ontvangen: + + + Resolving deltas: + Deltas verwerken: + + + Updating files: + Bestanden bijwerken: + + + + MapETA + + eta + eta + + + min + min + + + hr + uur + + + km + km + + + mi + mi + + + + MapInstructions + + km + km + + + m + m + + + mi + mi + + + ft + ft + + + + MapPanel + + Current Destination + Huidige Bestemming + + + CLEAR + LEEGMAKEN + + + Recent Destinations + Recente Bestemmingen + + + Try the Navigation Beta + Probeer de Navigatie Bèta + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + Krijg stapsgewijze routebeschrijving en meer met een comma +prime abonnement. Meld u nu aan: https://connect.comma.ai + + + No home +location set + Geen thuislocatie +ingesteld + + + No work +location set + Geen werklocatie +ingesteld + + + no recent destinations + geen recente bestemmingen + + + + MapWindow + + Map Loading + Kaart wordt geladen + + + Waiting for GPS + Wachten op GPS + + + + MultiOptionDialog + + Select + Selecteer + + + Cancel + Annuleren + + + + Networking + + Advanced + Geavanceerd + + + Enter password + Voer wachtwoord in + + + for "%1" + voor "%1" + + + Wrong password + Verkeerd wachtwoord + + + + OffroadHome + + UPDATE + UPDATE + + + ALERTS + WAARSCHUWINGEN + + + ALERT + WAARSCHUWING + + + + PairingPopup + + Pair your device to your comma account + Koppel uw apparaat aan uw comma-account + + + Go to https://connect.comma.ai on your phone + Ga naar https://connect.comma.ai op uw telefoon + + + Click "add new device" and scan the QR code on the right + Klik op "add new device" en scan de QR-code aan de rechterkant + + + Bookmark connect.comma.ai to your home screen to use it like an app + Voeg connect.comma.ai toe op uw startscherm om het als een app te gebruiken + + + + PrimeAdWidget + + Upgrade Now + Upgrade nu + + + Become a comma prime member at connect.comma.ai + Word een comma prime lid op connect.comma.ai + + + PRIME FEATURES: + PRIME BEVAT: + + + Remote access + Toegang op afstand + + + 1 year of storage + 1 jaar lang opslag + + + Developer perks + Voordelen voor ontwikkelaars + + + + PrimeUserWidget + + ✓ SUBSCRIBED + ✓ GEABONNEERD + + + comma prime + comma prime + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + COMMA POINTS + COMMA PUNTEN + + + + QObject + + Reboot + Opnieuw Opstarten + + + Exit + Afsluiten + + + dashcam + dashcam + + + openpilot + openpilot + + + %n minute(s) ago + + %n minuut geleden + %n minuten geleden + + + + %n hour(s) ago + + %n uur geleden + %n uur geleden + + + + %n day(s) ago + + %n dag geleden + %n dagen geleden + + + + + Reset + + Reset failed. Reboot to try again. + Opnieuw instellen mislukt. Start opnieuw op om opnieuw te proberen. + + + Are you sure you want to reset your device? + Weet u zeker dat u uw apparaat opnieuw wilt instellen? + + + Resetting device... + Apparaat opnieuw instellen... + + + System Reset + Systeemreset + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + Systeemreset geactiveerd. Druk op bevestigen om alle inhoud en instellingen te wissen. Druk op Annuleren om het opstarten te hervatten. + + + Cancel + Annuleren + + + Reboot + Opnieuw Opstarten + + + Confirm + Bevestigen + + + Unable to mount data partition. Press confirm to reset your device. + Kan gegevenspartitie niet koppelen. Druk op bevestigen om uw apparaat te resetten. + + + + RichTextDialog + + Ok + Ok + + + + SettingsWindow + + × + × + + + Device + Apparaat + + + Network + Netwerk + + + Toggles + Opties + + + Software + Software + + + Navigation + Navigatie + + + + Setup + + WARNING: Low Voltage + WAARCHUWING: Lage Spanning + + + Power your device in a car with a harness or proceed at your own risk. + Voorzie uw apparaat van stroom in een auto met een harnas (car harness) of ga op eigen risico verder. + + + Power off + Uitschakelen + + + Continue + Doorgaan + + + Getting Started + Aan de slag + + + Before we get on the road, let’s finish installation and cover some details. + Laten we, voordat we op pad gaan, de installatie afronden en enkele details bespreken. + + + Connect to Wi-Fi + Maak verbinding met Wi-Fi + + + Back + Terug + + + Continue without Wi-Fi + Doorgaan zonder Wi-Fi + + + Waiting for internet + Wachten op internet + + + Choose Software to Install + Kies Software om te Installeren + + + Dashcam + Dashcam + + + Custom Software + Andere Software + + + Enter URL + Voer URL in + + + for Custom Software + voor Andere Software + + + Downloading... + Downloaden... + + + Download Failed + Downloaden Mislukt + + + Ensure the entered URL is valid, and the device’s internet connection is good. + Zorg ervoor dat de ingevoerde URL geldig is en dat de internetverbinding van het apparaat goed is. + + + Reboot device + Apparaat opnieuw opstarten + + + Start over + Begin opnieuw + + + + SetupWidget + + Finish Setup + Installatie voltooien + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Koppel uw apparaat met comma connect (connect.comma.ai) en claim uw comma prime-aanbieding. + + + Pair device + Apparaat koppelen + + + + Sidebar + + CONNECT + VERBINDING + + + OFFLINE + OFFLINE + + + ONLINE + ONLINE + + + ERROR + FOUT + + + TEMP + TEMP + + + HIGH + HOOG + + + GOOD + GOED + + + OK + OK + + + VEHICLE + VOERTUIG + + + NO + GEEN + + + PANDA + PANDA + + + GPS + GPS + + + SEARCH + ZOEKEN + + + -- + -- + + + Wi-Fi + Wi-Fi + + + ETH + ETH + + + 2G + 2G + + + 3G + 3G + + + LTE + 4G + + + 5G + 5G + + + + SoftwarePanel + + Git Branch + Git Branch + + + Git Commit + Git Commit + + + OS Version + OS Versie + + + Version + Versie + + + Last Update Check + Laatste Updatecontrole + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + De laatste keer dat openpilot met succes heeft gecontroleerd op een update. De updater werkt alleen als de auto is uitgeschakeld. + + + Check for Update + Controleer op Updates + + + CHECKING + CONTROLEER + + + Switch Branch + Branch Verwisselen + + + ENTER + INVOEREN + + + The new branch will be pulled the next time the updater runs. + Tijdens de volgende update wordt de nieuwe branch opgehaald. + + + Enter branch name + Voer branch naam in + + + Uninstall %1 + Verwijder %1 + + + UNINSTALL + VERWIJDER + + + Are you sure you want to uninstall? + Weet u zeker dat u de installatie ongedaan wilt maken? + + + failed to fetch update + ophalen van update mislukt + + + CHECK + CONTROLEER + + + Updates are only downloaded while the car is off. + + + + Current Version + + + + Download + + + + Install Update + + + + INSTALL + + + + Target Branch + + + + SELECT + + + + Select a branch + + + + + SshControl + + SSH Keys + SSH Sleutels + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + Waarschuwing: dit geeft SSH toegang tot alle openbare sleutels in uw GitHub-instellingen. Voer nooit een andere GitHub-gebruikersnaam in dan die van uzelf. Een medewerker van comma zal u NOOIT vragen om zijn GitHub-gebruikersnaam toe te voegen. + + + ADD + TOEVOEGEN + + + Enter your GitHub username + Voer uw GitHub gebruikersnaam in + + + LOADING + LADEN + + + REMOVE + VERWIJDEREN + + + Username '%1' has no keys on GitHub + Gebruikersnaam '%1' heeft geen SSH sleutels op GitHub + + + Request timed out + Time-out van aanvraag + + + Username '%1' doesn't exist on GitHub + Gebruikersnaam '%1' bestaat niet op GitHub + + + + SshToggle + + Enable SSH + SSH Inschakelen + + + + TermsPage + + Terms & Conditions + Algemene Voorwaarden + + + Decline + Afwijzen + + + Scroll to accept + Scroll om te accepteren + + + Agree + Akkoord + + + + TogglesPanel + + Enable openpilot + openpilot Inschakelen + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + Gebruik het openpilot-systeem voor adaptieve cruisecontrol en rijstrookassistentie. Uw aandacht is te allen tijde vereist om deze functie te gebruiken. Het wijzigen van deze instelling wordt van kracht wanneer de auto wordt uitgeschakeld. + + + Enable Lane Departure Warnings + Waarschuwingen bij Verlaten Rijstrook Inschakelen + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + Ontvang waarschuwingen om terug naar de rijstrook te sturen wanneer uw voertuig over een gedetecteerde rijstrookstreep drijft zonder dat de richtingaanwijzer wordt geactiveerd terwijl u harder rijdt dan 50 km/u (31 mph). + + + Use Metric System + Gebruik Metrisch Systeem + + + Display speed in km/h instead of mph. + Geef snelheid weer in km/u in plaats van mph. + + + Record and Upload Driver Camera + Opnemen en Uploaden van de Bestuurders Camera + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + Upload gegevens van de bestuurders camera en help het algoritme voor het monitoren van de bestuurder te verbeteren. + + + Disengage on Accelerator Pedal + Deactiveren Met Gaspedaal + + + When enabled, pressing the accelerator pedal will disengage openpilot. + Indien ingeschakeld, zal het indrukken van het gaspedaal openpilot deactiveren. + + + Show ETA in 24h Format + Toon verwachte aankomsttijd in 24-uurs formaat + + + Use 24h format instead of am/pm + Gebruik 24-uurs formaat in plaats van AM en PM + + + Show Map on Left Side of UI + Toon kaart aan linkerkant van het scherm + + + Show map on left side when in split screen view. + Toon kaart links in gesplitste schermweergave. + + + openpilot Longitudinal Control + openpilot Longitudinale Controle + + + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! + openpilot zal de radar van de auto uitschakelen en de controle over gas en remmen overnemen. Waarschuwing: hierdoor wordt AEB (automatische noodrem) uitgeschakeld! + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + + + + Experimental openpilot Longitudinal Control + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + + + + openpilot longitudinal control is not currently available for this car. + + + + Enable experimental longitudinal control to enable this. + + + + + Updater + + Update Required + Update Vereist + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + Een update van het besturingssysteem is vereist. Verbind je apparaat met Wi-Fi voor de snelste update-ervaring. De downloadgrootte is ongeveer 1 GB. + + + Connect to Wi-Fi + Maak verbinding met Wi-Fi + + + Install + Installeer + + + Back + Terug + + + Loading... + Aan het laden... + + + Reboot + Opnieuw Opstarten + + + Update failed + Update mislukt + + + + WifiUI + + Scanning for networks... + Scannen naar netwerken... + + + CONNECTING... + VERBINDEN... + + + FORGET + VERGETEN + + + Forget Wi-Fi Network "%1"? + Vergeet Wi-Fi Netwerk "%1"? + + + diff --git a/selfdrive/ui/translations/main_pl.ts b/selfdrive/ui/translations/main_pl.ts new file mode 100644 index 000000000..4f8b03ef5 --- /dev/null +++ b/selfdrive/ui/translations/main_pl.ts @@ -0,0 +1,1124 @@ + + + + + AbstractAlert + + Close + Zamknij + + + Snooze Update + Zaktualizuj później + + + Reboot and Update + Uruchom ponownie i zaktualizuj + + + + AdvancedNetworking + + Back + Wróć + + + Enable Tethering + Włącz hotspot osobisty + + + Tethering Password + Hasło do hotspotu + + + EDIT + EDYTUJ + + + Enter new tethering password + Wprowadź nowe hasło do hotspotu + + + IP Address + Adres IP + + + Enable Roaming + Włącz roaming danych + + + APN Setting + Ustawienia APN + + + Enter APN + Wprowadź APN + + + leave blank for automatic configuration + Pozostaw puste, aby użyć domyślnej konfiguracji + + + Cellular Metered + + + + Prevent large data uploads when on a metered connection + + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + MAX + + + SPEED + PRĘDKOŚĆ + + + LIMIT + OGRANICZENIE + + + + ConfirmationDialog + + Ok + Ok + + + Cancel + Anuluj + + + + DeclinePage + + You must accept the Terms and Conditions in order to use openpilot. + Aby korzystać z openpilota musisz zaakceptować regulamin. + + + Back + Wróć + + + Decline, uninstall %1 + Odrzuć, odinstaluj %1 + + + + DevicePanel + + Dongle ID + ID adaptera + + + N/A + N/A + + + Serial + Numer seryjny + + + Driver Camera + Kamera kierowcy + + + PREVIEW + PODGLĄD + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + Wyświetl podgląd z kamery skierowanej na kierowcę, aby upewnić się, że monitoring kierowcy ma dobry zakres widzenia. (pojazd musi być wyłączony) + + + Reset Calibration + Zresetuj kalibrację + + + RESET + ZRESETUJ + + + Are you sure you want to reset calibration? + Czy na pewno chcesz zresetować kalibrację? + + + Review Training Guide + Zapoznaj się z samouczkiem + + + REVIEW + ZAPOZNAJ SIĘ + + + Review the rules, features, and limitations of openpilot + Zapoznaj się z zasadami, funkcjami i ograniczeniami openpilota + + + Are you sure you want to review the training guide? + Czy na pewno chcesz się zapoznać z samouczkiem? + + + Regulatory + Regulacja + + + VIEW + WIDOK + + + Change Language + Zmień język + + + CHANGE + ZMIEŃ + + + Select a language + Wybierz język + + + Reboot + Uruchom ponownie + + + Power Off + Wyłącz + + + 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 wymaga, aby urządzenie było zamontowane z maksymalnym odchyłem 4° poziomo, 5° w górę oraz 8° w dół. openpilot jest ciągle kalibrowany, rzadko konieczne jest resetowania urządzenia. + + + Your device is pointed %1° %2 and %3° %4. + Twoje urządzenie jest skierowane %1° %2 oraz %3° %4. + + + down + w dół + + + up + w górę + + + left + w lewo + + + right + w prawo + + + Are you sure you want to reboot? + Czy na pewno chcesz uruchomić ponownie urządzenie? + + + Disengage to Reboot + Aby uruchomić ponownie, odłącz sterowanie + + + Are you sure you want to power off? + Czy na pewno chcesz wyłączyć urządzenie? + + + Disengage to Power Off + Aby wyłączyć urządzenie, odłącz sterowanie + + + + DriveStats + + Drives + Przejazdy + + + Hours + Godziny + + + ALL TIME + CAŁKOWICIE + + + PAST WEEK + OSTATNI TYDZIEŃ + + + KM + KM + + + Miles + Mile + + + + DriverViewScene + + camera starting + uruchamianie kamery + + + + InputDialog + + Cancel + Anuluj + + + Need at least %n character(s)! + + Wpisana wartość powinna składać się przynajmniej z %n znaku! + Wpisana wartość powinna skłądać się przynajmniej z %n znaków! + Wpisana wartość powinna skłądać się przynajmniej z %n znaków! + + + + + Installer + + Installing... + Instalowanie... + + + Receiving objects: + Odbieranie obiektów: + + + Resolving deltas: + Rozwiązywanie różnic: + + + Updating files: + Aktualizacja plików: + + + + MapETA + + eta + przewidywany czas + + + min + min + + + hr + godz + + + km + km + + + mi + mi + + + + MapInstructions + + km + km + + + m + m + + + mi + mi + + + ft + ft + + + + MapPanel + + Current Destination + Miejsce docelowe + + + CLEAR + WYCZYŚĆ + + + Recent Destinations + Ostatnie miejsca docelowe + + + Try the Navigation Beta + Wypróbuj nawigację w wersji beta + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + Odblokuj nawigację zakręt po zakęcie i wiele więcej subskrybując +comma prime. Zarejestruj się teraz: https://connect.comma.ai + + + No home +location set + Lokalizacja domu +nie została ustawiona + + + No work +location set + Miejsce pracy +nie zostało ustawione + + + no recent destinations + brak ostatnich miejsc docelowych + + + + MapWindow + + Map Loading + Ładowanie Mapy + + + Waiting for GPS + Oczekiwanie na sygnał GPS + + + + MultiOptionDialog + + Select + Wybierz + + + Cancel + Anuluj + + + + Networking + + Advanced + Zaawansowane + + + Enter password + Wprowadź hasło + + + for "%1" + do "%1" + + + Wrong password + Niepoprawne hasło + + + + OffroadHome + + UPDATE + UAKTUALNIJ + + + ALERTS + ALERTY + + + ALERT + ALERT + + + + PairingPopup + + Pair your device to your comma account + Sparuj swoje urzadzenie ze swoim kontem comma + + + Go to https://connect.comma.ai on your phone + Wejdź na stronę https://connect.comma.ai na swoim telefonie + + + Click "add new device" and scan the QR code on the right + Kliknij "add new device" i zeskanuj kod QR znajdujący się po prawej stronie + + + Bookmark connect.comma.ai to your home screen to use it like an app + Dodaj connect.comma.ai do zakładek na swoim ekranie początkowym, aby korzystać z niej jak z aplikacji + + + + PrimeAdWidget + + Upgrade Now + Uaktualnij teraz + + + Become a comma prime member at connect.comma.ai + Zostań członkiem comma prime na connect.comma.ai + + + PRIME FEATURES: + FUNKCJE PRIME: + + + Remote access + Zdalny dostęp + + + 1 year of storage + 1 rok przechowywania danych + + + Developer perks + Udogodnienia dla programistów + + + + PrimeUserWidget + + ✓ SUBSCRIBED + ✓ ZASUBSKRYBOWANO + + + comma prime + comma prime + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + COMMA POINTS + COMMA POINTS + + + + QObject + + Reboot + Uruchom Ponownie + + + Exit + Wyjdź + + + dashcam + wideorejestrator + + + openpilot + openpilot + + + %n minute(s) ago + + %n minutę temu + %n minuty temu + %n minut temu + + + + %n hour(s) ago + + % godzinę temu + %n godziny temu + %n godzin temu + + + + %n day(s) ago + + %n dzień temu + %n dni temu + %n dni temu + + + + + Reset + + Reset failed. Reboot to try again. + Wymazywanie zakończone niepowodzeniem. Aby spróbować ponownie, uruchom ponownie urządzenie. + + + Are you sure you want to reset your device? + Czy na pewno chcesz wymazać urządzenie? + + + Resetting device... + Wymazywanie urządzenia... + + + System Reset + Przywróć do ustawień fabrycznych + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + Przywracanie do ustawień fabrycznych. Wciśnij potwierdź, aby usunąć wszystkie dane oraz ustawienia. Wciśnij anuluj, aby wznowić uruchamianie. + + + Cancel + Anuluj + + + Reboot + Uruchom ponownie + + + Confirm + Potwiedź + + + Unable to mount data partition. Press confirm to reset your device. + Partycja nie została zamontowana poprawnie. Wciśnij potwierdź, aby uruchomić ponownie urządzenie. + + + + RichTextDialog + + Ok + Ok + + + + SettingsWindow + + × + x + + + Device + Urządzenie + + + Network + Sieć + + + Toggles + Przełączniki + + + Software + Oprogramowanie + + + Navigation + Nawigacja + + + + Setup + + WARNING: Low Voltage + OSTRZEŻENIE: Niskie Napięcie + + + Power your device in a car with a harness or proceed at your own risk. + Podłącz swoje urządzenie do zasilania poprzez podłączenienie go do pojazdu lub kontynuuj na własną odpowiedzialność. + + + Power off + Wyłącz + + + Continue + Kontynuuj + + + Getting Started + Zacznij + + + Before we get on the road, let’s finish installation and cover some details. + Zanim ruszysz w drogę, dokończ instalację i podaj kilka szczegółów. + + + Connect to Wi-Fi + Połącz z Wi-Fi + + + Back + Wróć + + + Continue without Wi-Fi + Kontynuuj bez połączenia z Wif-Fi + + + Waiting for internet + Oczekiwanie na połączenie sieciowe + + + Choose Software to Install + Wybierz oprogramowanie do instalacji + + + Dashcam + Wideorejestrator + + + Custom Software + Własne oprogramowanie + + + Enter URL + Wprowadź adres URL + + + for Custom Software + do własnego oprogramowania + + + Downloading... + Pobieranie... + + + Download Failed + Pobieranie nie powiodło się + + + Ensure the entered URL is valid, and the device’s internet connection is good. + Upewnij się, że wpisany adres URL jest poprawny, a połączenie internetowe działa poprawnie. + + + Reboot device + Uruchom ponownie + + + Start over + Zacznij od początku + + + + SetupWidget + + Finish Setup + Zakończ konfigurację + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Sparuj swoje urządzenie z comma connect (connect.comma.ai) i wybierz swoją ofertę comma prime. + + + Pair device + Sparuj urządzenie + + + + Sidebar + + CONNECT + POŁĄCZENIE + + + OFFLINE + OFFLINE + + + ONLINE + ONLINE + + + ERROR + BŁĄD + + + TEMP + TEMP + + + HIGH + WYSOKA + + + GOOD + DOBRA + + + OK + OK + + + VEHICLE + POJAZD + + + NO + BRAK + + + PANDA + PANDA + + + GPS + GPS + + + SEARCH + SZUKAJ + + + -- + -- + + + Wi-Fi + Wi-FI + + + ETH + ETH + + + 2G + 2G + + + 3G + 3G + + + LTE + LTE + + + 5G + 5G + + + + SoftwarePanel + + Git Branch + Gałąź Git + + + Git Commit + Git commit + + + OS Version + Wersja systemu + + + Version + Wersja + + + Last Update Check + Ostatnie sprawdzenie aktualizacji + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + Ostatni raz kiedy openpilot znalazł aktualizację. Aktualizator może być uruchomiony wyłącznie wtedy, kiedy pojazd jest wyłączony. + + + Check for Update + Sprawdź uaktualnienia + + + CHECKING + SPRAWDZANIE + + + Switch Branch + Zmień gąłąź + + + ENTER + WPROWADŹ + + + The new branch will be pulled the next time the updater runs. + Nowa gałąź będzie pobrana przy następnym uruchomieniu aktualizatora. + + + Enter branch name + Wprowadź nazwę gałęzi + + + Uninstall %1 + Odinstaluj %1 + + + UNINSTALL + ODINSTALUJ + + + Are you sure you want to uninstall? + Czy na pewno chcesz odinstalować? + + + failed to fetch update + pobieranie aktualizacji zakończone niepowodzeniem + + + CHECK + SPRAWDŹ + + + Updates are only downloaded while the car is off. + + + + Current Version + + + + Download + + + + Install Update + + + + INSTALL + + + + Target Branch + + + + SELECT + + + + Select a branch + + + + + SshControl + + SSH Keys + Klucze SSH + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + Ostrzeżenie: To spowoduje przekazanie dostępu do wszystkich Twoich publicznych kuczy z ustawień GitHuba. Nigdy nie wprowadzaj nazwy użytkownika innej niż swoja. Pracownik comma NIGDY nie poprosi o dodanie swojej nazwy uzytkownika. + + + ADD + DODAJ + + + Enter your GitHub username + Wpisz swoją nazwę użytkownika GitHub + + + LOADING + ŁADOWANIE + + + REMOVE + USUŃ + + + Username '%1' has no keys on GitHub + Użytkownik '%1' nie posiada żadnych kluczy na GitHubie + + + Request timed out + Limit czasu rządania + + + Username '%1' doesn't exist on GitHub + Użytkownik '%1' nie istnieje na GitHubie + + + + SshToggle + + Enable SSH + Włącz SSH + + + + TermsPage + + Terms & Conditions + Regulamin + + + Decline + Odrzuć + + + Scroll to accept + Przewiń w dół, aby zaakceptować + + + Agree + Zaakceptuj + + + + TogglesPanel + + Enable openpilot + Włącz openpilota + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + Użyj openpilota do zachowania bezpiecznego odstępu między pojazdami i do asystowania w utrzymywaniu pasa ruchu. Twoja pełna uwaga jest wymagana przez cały czas korzystania z tej funkcji. Ustawienie to może być wdrożone wyłącznie wtedy, gdy pojazd jest wyłączony. + + + Enable Lane Departure Warnings + Włącz ostrzeganie przed zmianą pasa ruchu + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + Otrzymuj alerty o powrocie na właściwy pas, kiedy Twój pojazd przekroczy linię bez włączonego kierunkowskazu jadąc powyżej 50 km/h (31 mph). + + + Use Metric System + Korzystaj z systemu metrycznego + + + Display speed in km/h instead of mph. + Wyświetl prędkość w km/h zamiast mph. + + + Record and Upload Driver Camera + Nagraj i prześlij nagranie z kamery kierowcy + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + Prześlij dane z kamery skierowanej na kierowcę i pomóż poprawiać algorytm monitorowania kierowcy. + + + Disengage on Accelerator Pedal + Odłącz poprzez naciśnięcie gazu + + + When enabled, pressing the accelerator pedal will disengage openpilot. + Po włączeniu, naciśnięcie na pedał gazu odłączy openpilota. + + + Show ETA in 24h Format + Pokaż oczekiwany czas dojazdu w formacie 24-godzinnym + + + Use 24h format instead of am/pm + Korzystaj z formatu 24-godzinnego zamiast 12-godzinnego + + + Show Map on Left Side of UI + Pokaż mapę po lewej stronie ekranu + + + Show map on left side when in split screen view. + Pokaż mapę po lewej stronie kiedy ekran jest podzielony. + + + openpilot Longitudinal Control + Kontrola wzdłużna openpilota + + + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! + openpilot wyłączy radar samochodu i przejmie kontrolę nad gazem i hamulcem. Ostrzeżenie: wyłączony zostanie system AEB! + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + + + + Experimental openpilot Longitudinal Control + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + + + + openpilot longitudinal control is not currently available for this car. + + + + Enable experimental longitudinal control to enable this. + + + + + Updater + + Update Required + Wymagana Aktualizacja + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + Wymagana aktualizacja systemu operacyjnego. Aby przyspieszyć proces aktualizacji połącz swoje urzeądzenie do Wi-Fi. Rozmiar pobieranej paczki wynosi około 1GB. + + + Connect to Wi-Fi + Połącz się z Wi-Fi + + + Install + Zainstaluj + + + Back + Wróć + + + Loading... + Ładowanie... + + + Reboot + Uruchom ponownie + + + Update failed + Aktualizacja nie powiodła się + + + + WifiUI + + Scanning for networks... + Wyszukiwanie sieci... + + + CONNECTING... + ŁĄCZENIE... + + + FORGET + ZAPOMNIJ + + + Forget Wi-Fi Network "%1"? + Czy chcesz zapomnieć sieć "%1"? + + + diff --git a/selfdrive/ui/translations/main_pt.ts b/selfdrive/ui/translations/main_pt-BR.ts similarity index 61% rename from selfdrive/ui/translations/main_pt.ts rename to selfdrive/ui/translations/main_pt-BR.ts index 3d5eda118..a1c966da4 100644 --- a/selfdrive/ui/translations/main_pt.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -4,17 +4,14 @@ AbstractAlert - Close Fechar - Snooze Update Adiar Atualização - Reboot and Update Reiniciar e Atualizar @@ -22,67 +19,84 @@ AdvancedNetworking - Back Voltar - Enable Tethering - Ativar Theter + Ativar Tether - Tethering Password - Senha Thetering + Senha Tethering - - EDIT EDITAR - Enter new tethering password - Insira nova senha thetering + Insira nova senha tethering - IP Address - IP Endereço + Endereço IP - Enable Roaming Ativar Roaming - APN Setting APN Config - Enter APN Insira APN - leave blank for automatic configuration deixe em branco para configuração automática + + Cellular Metered + Plano de Dados Limitado + + + Prevent large data uploads when on a metered connection + Evite grandes uploads de dados quando estiver em uma conexão limitada + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + LIMITE + + + SPEED + MAX + + + LIMIT + VELO + ConfirmationDialog - - Ok OK - Cancel Cancelar @@ -90,17 +104,14 @@ DeclinePage - You must accept the Terms and Conditions in order to use openpilot. Você precisa aceitar os Termos e Condições para utilizar openpilot. - Back Voltar - Decline, uninstall %1 Rejeitar, desintalar %1 @@ -108,185 +119,157 @@ DevicePanel - Dongle ID Dongle ID - N/A N/A - Serial Serial - Driver Camera - Câmera Motorista + Câmera voltada para o Motorista - PREVIEW PREVISUAL - Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) Pré-visualizar a câmera voltada para o motorista para garantir que monitor tem uma boa visibilidade (veículo precisa estar desligado) - Reset Calibration - Limpar Calibragem + Resetar Calibragem - RESET RESET - Are you sure you want to reset calibration? - Tem certeza que quer limpar calibragem? + Tem certeza que quer resetar a calibragem? - Review Training Guide - Revisar o Treinamento + Revisar Guia de Treinamento - REVIEW REVISAR - Review the rules, features, and limitations of openpilot - Revisar regras, features e limitações do openpilot + Revisar regras, aprimoramentos e limitações do openpilot - Are you sure you want to review the training guide? Tem certeza que quer rever o treinamento? - Regulatory Regulatório - VIEW VER - Change Language - Mudar Língua + Alterar Idioma - CHANGE - MUDAR + ALTERAR - Select a language - Selecione uma linguagem + Selecione o Idioma - Reboot Reiniciar - Power Off Desligar - 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. - o openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. o openpilot está continuamente calibrando, a redefinição raramente é necessária. + o openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. o openpilot está continuamente calibrando, resetar raramente é necessário. - Your device is pointed %1° %2 and %3° %4. Seu dispositivo está montado %1° %2 e %3° %4. - down baixo - up cima - left esquerda - right direita - Are you sure you want to reboot? Tem certeza que quer reiniciar? - Disengage to Reboot Desacione para Reiniciar - Are you sure you want to power off? Tem certeza que quer desligar? - Disengage to Power Off Desacione para Desligar + + Reset + Resetar + + + Review + Revisar + DriveStats - Drives Dirigidas - Hours Horas - ALL TIME TOTAL - PAST WEEK SEMANA PASSADA - KM KM - Miles Milhas @@ -294,20 +277,28 @@ DriverViewScene - camera starting - camera iniciando + câmera iniciando + + + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + + + + CHILL MODE ON + InputDialog - Cancel Cancelar - Need at least %n character(s)! Necessita no mínimo %n caractere! @@ -318,22 +309,18 @@ Installer - Installing... Instalando... - Receiving objects: Recebendo objetos: - Resolving deltas: Resolvendo deltas: - Updating files: Atualizando arquivos: @@ -341,27 +328,22 @@ MapETA - eta eta - min min - hr hr - km km - mi mi @@ -369,22 +351,18 @@ MapInstructions - km km - m m - mi milha - ft pés @@ -392,48 +370,40 @@ MapPanel - Current Destination Destino Atual - CLEAR LIMPAR - Recent Destinations Destinos Recentes - Try the Navigation Beta Experimente a Navegação Beta - Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai Obtenha instruções passo a passo exibidas e muito mais com -uma assinatura prime Increva-se agora：https://connect.comma.ai +uma assinatura prime Inscreva-se agora: https://connect.comma.ai - No home location set Sem local residência definido - No work location set - Sem local + Sem local de trabalho definido - no recent destinations sem destinos recentes @@ -441,12 +411,10 @@ trabalho definido MapWindow - Map Loading Carregando Mapa - Waiting for GPS Esperando por GPS @@ -454,12 +422,10 @@ trabalho definido MultiOptionDialog - Select Selecione - Cancel Cancelar @@ -467,72 +433,33 @@ trabalho definido Networking - Advanced Avançado - Enter password Insira a senha - - for "%1" para "%1" - Wrong password Senha incorreta - - NvgWindow - - - km/h - km/h - - - - mph - mph - - - - - MAX - LIMITE - - - - - SPEED - MAX - - - - - LIMIT - VELO - - OffroadHome - UPDATE ATUALIZAÇÃO - ALERTS ALERTAS - ALERT ALERTA @@ -540,55 +467,56 @@ trabalho definido PairingPopup - Pair your device to your comma account - Pareie seu dispositivo a sua conta comma + Pareie seu dispositivo à sua conta comma - Go to https://connect.comma.ai on your phone navegue até https://connect.comma.ai no seu telefone - Click "add new device" and scan the QR code on the right Clique "add new device" e escaneie o QR code a seguir - Bookmark connect.comma.ai to your home screen to use it like an app - Salve connect.comma.ai como sua página inicial para utilizar com um app + Salve connect.comma.ai como sua página inicial para utilizar como um app + + + + ParamControl + + Cancel + Cancelar + + + Enable + Ativar PrimeAdWidget - Upgrade Now Atualizar Agora - Become a comma prime member at connect.comma.ai Torne-se um membro comma prime em connect.comma.ai - PRIME FEATURES: - PRIME FEATURES: + BENEFÍCIOS PRIME: - Remote access Acesso remoto - 1 year of storage 1 ano de armazenamento - Developer perks Benefícios para desenvolvedor @@ -596,22 +524,18 @@ trabalho definido PrimeUserWidget - ✓ SUBSCRIBED ✓ INSCRITO - comma prime comma prime - CONNECT.COMMA.AI CONNECT.COMMA.AI - COMMA POINTS PONTOS COMMA @@ -619,27 +543,22 @@ trabalho definido QObject - Reboot Reiniciar - Exit Sair - dashcam dashcam - openpilot openpilot - %n minute(s) ago há %n minuto @@ -647,7 +566,6 @@ trabalho definido - %n hour(s) ago há %n hora @@ -655,7 +573,6 @@ trabalho definido - %n day(s) ago há %n dia @@ -666,89 +583,65 @@ trabalho definido Reset - Reset failed. Reboot to try again. Reset falhou. Reinicie para tentar novamente. - 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 - Reboot Reiniciar - Confirm 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. - - RichTextDialog - - - Ok - Ok - - SettingsWindow - × × - Device Dispositivo - - Network Rede - Toggles Ajustes - Software Software - Navigation Navegação @@ -756,105 +649,82 @@ trabalho definido Setup - WARNING: Low Voltage ALERTA: Baixa Voltagem - Power your device in a car with a harness or proceed at your own risk. Ligue seu dispositivo em um carro com um chicote ou prossiga por sua conta e risco. - Power off Desligar - - - Continue Continuar - Getting Started Começando - Before we get on the road, let’s finish installation and cover some details. Antes de pegarmos a estrada, vamos terminar a instalação e cobrir alguns detalhes. - Connect to Wi-Fi Conectar ao Wi-Fi - - Back Voltar - Continue without Wi-Fi Continuar sem Wi-Fi - Waiting for internet Esperando pela internet - Choose Software to Install Escolher Software para Instalar - Dashcam Dashcam - Custom Software Sofware Customizado - Enter URL Preencher URL - for Custom Software para o Software Customizado - Downloading... Baixando... - Download Failed Download Falhou - Ensure the entered URL is valid, and the device’s internet connection is good. Garanta que a URL inserida é valida, e uma boa conexão à internet. - Reboot device Reiniciar Dispositivo - Start over Inicializar @@ -862,17 +732,14 @@ trabalho definido SetupWidget - Finish Setup - Terminar Configuração + Concluir - Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. Pareie seu dispositivo com comma connect (connect.comma.ai) e reivindique sua oferta de comma prime. - Pair device Parear dispositivo @@ -880,106 +747,82 @@ trabalho definido Sidebar - - CONNECT CONEXÃO - OFFLINE - DESCONEC + OFFLINE - - ONLINE - CONECTADO + ONLINE - ERROR ERRO - - - TEMP TEMP - HIGH ALTA - GOOD BOA - OK OK - VEHICLE VEÍCULO - NO SEM - PANDA PANDA - GPS GPS - SEARCH PROCURA - -- -- - Wi-Fi Wi-Fi - ETH ETH - 2G 2G - 3G 3G - LTE LTE - 5G 5G @@ -987,138 +830,93 @@ trabalho definido SoftwarePanel - - Git Branch - Ramo Git + Updates are only downloaded while the car is off. + Atualizações baixadas durante o motor desligado. - - Git Commit - Commit Git + Current Version + Versao Atual - - OS Version - Versão do Sistema + Download + Download - - Version - Versão + Install Update + Instalar Atualização - - Last Update Check - Verificação da última atualização + INSTALL + INSTALAR - - The last time openpilot successfully checked for an update. The updater only runs while the car is off. - A última vez que o openpilot verificou com sucesso uma atualização. O atualizador só funciona com o carro desligado. + Target Branch + Alterar Branch - - Check for Update - Verifique atualizações + SELECT + SELECIONE - - CHECKING - VERIFICANDO + Select a branch + Selecione uma branch - - Switch Branch - Trocar Ramo - - - - ENTER - INSERIR - - - - - The new branch will be pulled the next time the updater runs. - O novo ramo será aplicado na próxima execução do atualizador. - - - - Enter branch name - Inserir o nome do ramo - - - UNINSTALL - DESINSTALAR + DESINSTAL - Uninstall %1 - Desintalando %1 + Desintalar o %1 - Are you sure you want to uninstall? Tem certeza que quer desinstalar? - - failed to fetch update - falha ao buscar atualização - - - - CHECK VERIFICAR + + Uninstall + Desinstalar + SshControl - SSH Keys Chave SSH - Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. Aviso: isso concede acesso SSH a todas as chaves públicas nas configurações do GitHub. Nunca insira um nome de usuário do GitHub que não seja o seu. Um funcionário da comma NUNCA pedirá que você adicione seu nome de usuário do GitHub. - - ADD ADICIONAR - Enter your GitHub username Insira seu nome de usuário do GitHub - LOADING CARREGANDO - REMOVE REMOVER - Username '%1' has no keys on GitHub Usuário "%1” não possui chaves no GitHub - Request timed out A solicitação expirou - Username '%1' doesn't exist on GitHub Usuário '%1' não existe no GitHub @@ -1126,7 +924,6 @@ trabalho definido SshToggle - Enable SSH Habilitar SSH @@ -1134,22 +931,18 @@ trabalho definido TermsPage - Terms & Conditions Termos & Condições - Decline Declinar - Scroll to accept - Role para aceitar + Role a tela para aceitar - Agree Concordo @@ -1157,125 +950,121 @@ trabalho definido TogglesPanel - Enable openpilot Ativar openpilot - Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. Use o sistema openpilot para controle de cruzeiro adaptativo e assistência ao motorista de manutenção de faixa. Sua atenção é necessária o tempo todo para usar esse recurso. A alteração desta configuração tem efeito quando o carro é desligado. - Enable Lane Departure Warnings Ativar Avisos de Saída de Faixa - Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). Receba alertas para voltar para a pista se o seu veículo sair da faixa e a seta não tiver sido acionada previamente quando em velocidades superiores a 50 km/h. - Use Metric System Usar Sistema Métrico - Display speed in km/h instead of mph. Exibir velocidade em km/h invés de mph. - Record and Upload Driver Camera Gravar e Upload Câmera Motorista - 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. - - Disengage On Accelerator Pedal + Experimental openpilot Longitudinal Control + Controle longitudinal experimental openpilot + + + Disengage on Accelerator Pedal Desacionar Com Pedal Do Acelerador - When enabled, pressing the accelerator pedal will disengage openpilot. Quando ativado, pressionar o pedal do acelerador desacionará o openpilot. - Show ETA in 24h Format Mostrar ETA em formato 24h - Use 24h format instead of am/pm Use o formato 24h em vez de am/pm - Show Map on Left Side of UI Exibir Mapa no Lado Esquerdo - Show map on left side when in split screen view. Exibir mapa do lado esquerdo quando a tela for dividida. - - openpilot Longitudinal Control - openpilot Controle Longitudinal + Experimental Mode + Modo Experimental - - openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! - openpilot desativará o radar do carro e assumirá o controle do acelerador e freios. Atenção: isso desativa AEB! + 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. + + + + 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: <br> <h4>🌮 End-to-End Longitudinal Control 🌮</h4> Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. <br> <h4>New Driving Visualization</h4> The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + + + + Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. + + + + Enable experimental longitudinal control to allow experimental mode. + Updater - Update Required Atualização Necessária - An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. Uma atualização do sistema operacional é necessária. Conecte seu dispositivo ao Wi-Fi para a experiência de atualização mais rápida. O tamanho do download é de aproximadamente 1GB. - Connect to Wi-Fi Conecte-se ao Wi-Fi - Install Instalar - Back Voltar - Loading... Carregando... - Reboot Reiniciar - Update failed Falha na atualização @@ -1283,25 +1072,24 @@ trabalho definido WifiUI - - Scanning for networks... Procurando redes... - CONNECTING... CONECTANDO... - FORGET ESQUECER - Forget Wi-Fi Network "%1"? Esquecer Rede Wi-Fi "%1"? + + Forget + Esquecer + diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts new file mode 100644 index 000000000..0de0ba5f9 --- /dev/null +++ b/selfdrive/ui/translations/main_th.ts @@ -0,0 +1,1056 @@ + + + + + AbstractAlert + + Close + ปิด + + + Snooze Update + เลื่อนการอัปเดต + + + Reboot and Update + รีบูตและอัปเดต + + + + AdvancedNetworking + + Back + ย้อนกลับ + + + Enable Tethering + ปล่อยฮอตสปอต + + + Tethering Password + รหัสผ่านฮอตสปอต + + + EDIT + แก้ไข + + + Enter new tethering password + ป้อนรหัสผ่านฮอตสปอตใหม่ + + + IP Address + หมายเลขไอพี + + + Enable Roaming + เปิดใช้งานโรมมิ่ง + + + APN Setting + ตั้งค่า APN + + + Enter APN + ป้อนค่า APN + + + leave blank for automatic configuration + เว้นว่างเพื่อตั้งค่าอัตโนมัติ + + + Cellular Metered + ลดการส่งข้อมูลผ่านเซลลูล่าร์ + + + Prevent large data uploads when on a metered connection + ปิดการอัพโหลดข้อมูลขนาดใหญ่เมื่อเชื่อมต่อผ่านเซลลูล่าร์ + + + + AnnotatedCameraWidget + + km/h + กม./ชม. + + + mph + ไมล์/ชม. + + + MAX + สูงสุด + + + SPEED + ความเร็ว + + + LIMIT + จำกัด + + + + ConfirmationDialog + + Ok + ตกลง + + + Cancel + ยกเลิก + + + + DeclinePage + + You must accept the Terms and Conditions in order to use openpilot. + คุณต้องยอมรับเงื่อนไขและข้อตกลง เพื่อใช้งาน openpilot + + + Back + ย้อนกลับ + + + Decline, uninstall %1 + ปฏิเสธ และถอนการติดตั้ง %1 + + + + DevicePanel + + Dongle ID + Dongle ID + + + N/A + ไม่มี + + + Serial + ซีเรียล + + + Driver Camera + กล้องฝั่งคนขับ + + + PREVIEW + แสดงภาพ + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + ดูภาพตัวอย่างกล้องที่หันเข้าหาคนขับเพื่อให้แน่ใจว่าการตรวจสอบคนขับมีทัศนวิสัยที่ดี (รถต้องดับเครื่องยนต์) + + + Reset Calibration + รีเซ็ตการคาลิเบรท + + + RESET + รีเซ็ต + + + Are you sure you want to reset calibration? + คุณแน่ใจหรือไม่ว่าต้องการรีเซ็ตการคาลิเบรท? + + + Review Training Guide + ทบทวนคู่มือการใช้งาน + + + REVIEW + ทบทวน + + + Review the rules, features, and limitations of openpilot + ตรวจสอบกฎ คุณสมบัติ และข้อจำกัดของ openpilot + + + Are you sure you want to review the training guide? + คุณแน่ใจหรือไม่ว่าต้องการทบทวนคู่มือการใช้งาน? + + + Regulatory + ระเบียบข้อบังคับ + + + VIEW + ดู + + + Change Language + เปลี่ยนภาษา + + + CHANGE + เปลี่ยน + + + Select a language + เลือกภาษา + + + Reboot + รีบูต + + + Power Off + ปิดเครื่อง + + + 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 ทำการคาลิเบรทอย่างต่อเนื่อง แทบจะไม่จำเป็นต้องทำการรีเซ็ตการคาลิเบรท + + + Your device is pointed %1° %2 and %3° %4. + อุปกรณ์ของคุณเอียงไปทาง %2 %1° และ %4 %3° + + + down + ด้านล่าง + + + up + ด้านบน + + + left + ด้านซ้าย + + + right + ด้านขวา + + + Are you sure you want to reboot? + คุณแน่ใจหรือไม่ว่าต้องการรีบูต? + + + Disengage to Reboot + ยกเลิกระบบช่วยขับเพื่อรีบูต + + + Are you sure you want to power off? + คุณแน่ใจหรือไม่ว่าต้องการปิดเครื่อง? + + + Disengage to Power Off + ยกเลิกระบบช่วยขับเพื่อปิดเครื่อง + + + + DriveStats + + Drives + การขับขี่ + + + Hours + ชั่วโมง + + + ALL TIME + ทั้งหมด + + + PAST WEEK + สัปดาห์ที่ผ่านมา + + + KM + กิโลเมตร + + + Miles + ไมล์ + + + + DriverViewScene + + camera starting + กำลังเปิดกล้อง + + + + InputDialog + + Cancel + ยกเลิก + + + Need at least %n character(s)! + + ต้องการอย่างน้อย %n ตัวอักษร! + + + + + Installer + + Installing... + กำลังติดตั้ง... + + + Receiving objects: + กำลังรับข้อมูล: + + + Resolving deltas: + การแก้ไขเดลต้า: + + + Updating files: + กำลังอัปเดตไฟล์: + + + + MapETA + + eta + eta + + + min + นาที + + + hr + ชม. + + + km + กม. + + + mi + ไมล์ + + + + MapInstructions + + km + กม. + + + m + ม. + + + mi + ไมล์ + + + ft + ฟุต + + + + MapPanel + + Current Destination + ปลายทางปัจจุบัน + + + CLEAR + ล้างข้อมูล + + + Recent Destinations + ปลายทางล่าสุด + + + Try the Navigation Beta + ลองใช้ระบบนำทาง (เบต้า) + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + รับการแสดงเส้นทางแบบเลี้ยวต่อเลี้ยว และอื่นๆ ด้วยการสมัครบริการ +comma prime สมัครเลย: https://connect.comma.ai + + + No home +location set + ยังไม่ได้กำหนด +ตำแหน่งของบ้าน + + + No work +location set + ยังไม่ได้กำหนด +ตำแหน่งของที่ทำงาน + + + no recent destinations + ไม่พบปลายทางล่าสุด + + + + MapWindow + + Map Loading + กำลังโหลดแผนที่ + + + Waiting for GPS + กำลังรอสัญญาณ GPS + + + + MultiOptionDialog + + Select + เลือก + + + Cancel + ยกเลิก + + + + Networking + + Advanced + ขั้นสูง + + + Enter password + ใส่รหัสผ่าน + + + for "%1" + สำหรับ "%1" + + + Wrong password + รหัสผ่านผิด + + + + OffroadHome + + UPDATE + อัปเดต + + + ALERTS + การแจ้งเตือน + + + ALERT + การแจ้งเตือน + + + + PairingPopup + + Pair your device to your comma account + จับคู่อุปกรณ์ของคุณกับบัญชี comma ของคุณ + + + Go to https://connect.comma.ai on your phone + ไปที่ https://connect.comma.ai ด้วยโทรศัพท์ของคุณ + + + Click "add new device" and scan the QR code on the right + กดที่ "add new device" และสแกนคิวอาร์โค้ดทางด้านขวา + + + Bookmark connect.comma.ai to your home screen to use it like an app + จดจำ connect.comma.ai โดยการเพิ่มไปยังหน้าจอโฮม เพื่อใช้งานเหมือนเป็นแอปพลิเคชัน + + + + PrimeAdWidget + + Upgrade Now + อัพเกรดเดี๋ยวนี้ + + + Become a comma prime member at connect.comma.ai + สมัครสมาชิก comma prime ได้ที่ connect.comma.ai + + + PRIME FEATURES: + คุณสมบัติของ PRIME: + + + Remote access + การเข้าถึงระยะไกล + + + 1 year of storage + จัดเก็บข้อมูลนาน 1 ปี + + + Developer perks + สิทธิพิเศษสำหรับนักพัฒนา + + + + PrimeUserWidget + + ✓ SUBSCRIBED + ✓ สมัครสำเร็จ + + + comma prime + comma prime + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + COMMA POINTS + คะแนน COMMA + + + + QObject + + Reboot + รีบูต + + + Exit + ปิด + + + dashcam + กล้องติดรถยนต์ + + + openpilot + openpilot + + + %n minute(s) ago + + %n นาทีที่แล้ว + + + + %n hour(s) ago + + %n ชั่วโมงที่แล้ว + + + + %n day(s) ago + + %n วันที่แล้ว + + + + + Reset + + Reset failed. Reboot to try again. + การรีเซ็ตล้มเหลว รีบูตเพื่อลองอีกครั้ง + + + 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 + ยกเลิก + + + Reboot + รีบูต + + + Confirm + ยืนยัน + + + Unable to mount data partition. Press confirm to reset your device. + ไม่สามารถเมานต์พาร์ติชั่นข้อมูล กดยืนยันเพื่อรีเซ็ตอุปกรณ์ของคุณ + + + + RichTextDialog + + Ok + ตกลง + + + + SettingsWindow + + × + × + + + Device + อุปกรณ์ + + + Network + เครือข่าย + + + Toggles + ตัวเลือก + + + Software + ซอฟต์แวร์ + + + Navigation + การนำทาง + + + + Setup + + WARNING: Low Voltage + คำเตือน: แรงดันแบตเตอรี่ต่ำ + + + Power your device in a car with a harness or proceed at your own risk. + โปรดต่ออุปกรณ์ของคุณเข้ากับสายควบคุมในรถยนต์ หรือดำเนินการด้วยความเสี่ยงของคุณเอง + + + Power off + ปิดเครื่อง + + + Continue + ดำเนินการต่อ + + + Getting Started + เริ่มกันเลย + + + Before we get on the road, let’s finish installation and cover some details. + ก่อนออกเดินทาง เรามาทำการติดตั้งซอฟต์แวร์ และตรวจสอบการตั้งค่า + + + Connect to Wi-Fi + เชื่อมต่อ Wi-Fi + + + Back + ย้อนกลับ + + + Continue without Wi-Fi + ดำเนินการต่อโดยไม่ใช้ Wi-Fi + + + Waiting for internet + กำลังรอสัญญาณอินเตอร์เน็ต + + + Choose Software to Install + เลือกซอฟต์แวร์ที่จะติดตั้ง + + + Dashcam + กล้องติดรถยนต์ + + + Custom Software + ซอฟต์แวร์ที่กำหนดเอง + + + Enter URL + ป้อน URL + + + for Custom Software + สำหรับซอฟต์แวร์ที่กำหนดเอง + + + Downloading... + กำลังดาวน์โหลด... + + + Download Failed + ดาวน์โหลดล้มเหลว + + + Ensure the entered URL is valid, and the device’s internet connection is good. + ตรวจสอบให้แน่ใจว่า URL ที่ป้อนนั้นถูกต้อง และอุปกรณ์เชื่อมต่ออินเทอร์เน็ตอยู่ + + + Reboot device + รีบูตอุปกรณ์ + + + Start over + เริ่มต้นใหม่ + + + + SetupWidget + + Finish Setup + ตั้งค่าเสร็จสิ้น + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + จับคู่อุปกรณ์ของคุณกับ comma connect (connect.comma.ai) และรับข้อเสนอ comma prime ของคุณ + + + Pair device + จับคู่อุปกรณ์ + + + + Sidebar + + CONNECT + เชื่อมต่อ + + + OFFLINE + ออฟไลน์ + + + ONLINE + ออนไลน์ + + + ERROR + เกิดข้อผิดพลาด + + + TEMP + อุณหภูมิ + + + HIGH + สูง + + + GOOD + ดี + + + OK + พอใช้ + + + VEHICLE + รถยนต์ + + + NO + ไม่พบ + + + PANDA + PANDA + + + GPS + จีพีเอส + + + SEARCH + ค้นหา + + + -- + -- + + + Wi-Fi + Wi-Fi + + + ETH + ETH + + + 2G + 2G + + + 3G + 3G + + + LTE + LTE + + + 5G + 5G + + + + SoftwarePanel + + Uninstall %1 + ถอนการติดตั้ง %1 + + + UNINSTALL + ถอนการติดตั้ง + + + Are you sure you want to uninstall? + คุณแน่ใจหรือไม่ว่าต้องการถอนการติดตั้ง? + + + CHECK + ตรวจสอบ + + + Updates are only downloaded while the car is off. + ตัวอัปเดตจะดำเนินการดาวน์โหลดเมื่อรถดับเครื่องยนต์อยู่เท่านั้น + + + Current Version + เวอร์ชั่นปัจจุบัน + + + Download + ดาวน์โหลด + + + Install Update + ติดตั้งตัวอัปเดต + + + INSTALL + ติดตั้ง + + + Target Branch + Branch ที่เลือก + + + SELECT + เลือก + + + Select a branch + เลือก Branch + + + + SshControl + + SSH Keys + คีย์ SSH + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + คำเตือน: สิ่งนี้ให้สิทธิ์ SSH เข้าถึงคีย์สาธารณะทั้งหมดใน GitHub ของคุณ อย่าป้อนชื่อผู้ใช้ GitHub อื่นนอกเหนือจากของคุณเอง พนักงาน comma จะไม่ขอให้คุณเพิ่มชื่อผู้ใช้ GitHub ของพวกเขา + + + ADD + เพิ่ม + + + Enter your GitHub username + ป้อนชื่อผู้ใช้ GitHub ของคุณ + + + LOADING + กำลังโหลด + + + REMOVE + ลบ + + + Username '%1' has no keys on GitHub + ชื่อผู้ใช้ '%1' ไม่มีคีย์บน GitHub + + + Request timed out + ตรวจสอบไม่สำเร็จ เนื่องจากใช้เวลามากเกินไป + + + Username '%1' doesn't exist on GitHub + ไม่พบชื่อผู้ใช้ '%1' บน GitHub + + + + SshToggle + + Enable SSH + เปิดใช้งาน SSH + + + + TermsPage + + Terms & Conditions + ข้อตกลงและเงื่อนไข + + + Decline + ปฏิเสธ + + + Scroll to accept + เลื่อนเพื่อตอบรับข้อตกลง + + + Agree + ยอมรับ + + + + TogglesPanel + + Enable openpilot + เปิดใช้งาน openpilot + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + ใช้ระบบ openpilot สำหรับระบบควบคุมความเร็วอัตโนมัติ และระบบช่วยควบคุมรถให้อยู่ในเลน คุณจำเป็นต้องให้ความสนใจตลอดเวลาที่ใช้คุณสมบัตินี้ การเปลี่ยนการตั้งค่านี้จะมีผลเมื่อคุณดับเครื่องยนต์ + + + Enable Lane Departure Warnings + เปิดใช้งานการเตือนการออกนอกเลน + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + รับการแจ้งเตือนให้เลี้ยวกลับเข้าเลนเมื่อรถของคุณตรวจพบการข้ามช่องจราจรโดยไม่เปิดสัญญาณไฟเลี้ยวในขณะขับขี่ที่ความเร็วเกิน 31 ไมล์ต่อชั่วโมง (50 กม./ชม) + + + Use Metric System + ใช้ระบบเมตริก + + + Display speed in km/h instead of mph. + แสดงความเร็วเป็น กม./ชม. แทน ไมล์/ชั่วโมง + + + Record and Upload Driver Camera + บันทึกและอัปโหลดภาพจากกล้องคนขับ + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + อัปโหลดข้อมูลจากกล้องที่หันหน้าไปทางคนขับ และช่วยปรับปรุงอัลกอริธึมการตรวจสอบผู้ขับขี่ + + + Disengage on Accelerator Pedal + ยกเลิกระบบช่วยขับเมื่อเหยียบคันเร่ง + + + When enabled, pressing the accelerator pedal will disengage openpilot. + เมื่อเปิดใช้งาน การกดแป้นคันเร่งจะเป็นการยกเลิกระบบช่วยขับโดย openpilot + + + Show ETA in 24h Format + แสดงเวลา ETA ในรูปแบบ 24 ชั่วโมง + + + Use 24h format instead of am/pm + ใช้รูปแบบเวลา 24 ชั่วโมง แทน am/pm + + + Show Map on Left Side of UI + แสดงแผนที่ที่ด้านซ้ายของหน้าจอ + + + Show map on left side when in split screen view. + แสดงแผนที่ด้านซ้ายของหน้าจอเมื่ออยู่ในโหมดแบ่งหน้าจอ + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + 🌮 ควบคุมเร่ง/เบรคแบบ End-to-end (อยู่ขั้นพัฒนา) 🌮 + + + Experimental openpilot Longitudinal Control + ทดลองใช้ระบบควบคุมการเร่ง/เบรคโดย openpilot + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + <b>คำเตือน: การควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ยังอยู่ในขั้นทดลอง และระบบเบรคฉุกเฉินอัตโนมัติ (AEB) จะถูกปิด</b> + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + ให้ openpilot ควบคุมการเร่ง/เบรคแบบ end-to-end โดย openpilot จะขับอย่างที่มนุษย์คิด ระบบยังอยู่ในขั้นทดลอง + + + openpilot longitudinal control is not currently available for this car. + ขณะนี้ยังไม่มีระบบควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ + + + Enable experimental longitudinal control to enable this. + เปิดใช้งานระบบควบคุมการเร่ง/เบรคขั้นทดลอง เพื่อเปิดใช้งานสิ่งนี้ + + + + Updater + + Update Required + จำเป็นต้องอัปเดต + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + จำเป็นต้องมีการอัปเดตระบบปฏิบัติการ เชื่อมต่ออุปกรณ์ของคุณกับ Wi-Fi เพื่อประสบการณ์การอัปเดตที่เร็วที่สุด ขนาดดาวน์โหลดประมาณ 1GB + + + Connect to Wi-Fi + เชื่อมต่อกับ Wi-Fi + + + Install + ติดตั้ง + + + Back + ย้อนกลับ + + + Loading... + กำลังโหลด... + + + Reboot + รีบูต + + + Update failed + การอัปเดตล้มเหลว + + + + WifiUI + + Scanning for networks... + กำลังสแกนหาเครือข่าย... + + + CONNECTING... + กำลังเชื่อมต่อ... + + + FORGET + เลิกใช้ + + + Forget Wi-Fi Network "%1"? + เลิกใช้เครือข่าย Wi-Fi "%1"? + + + diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index a26cddc0c..e77b3ef63 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -4,17 +4,14 @@ AbstractAlert - Close 关闭 - Snooze Update 暂停更新 - Reboot and Update 重启并更新 @@ -22,67 +19,84 @@ AdvancedNetworking - Back 返回 - Enable Tethering 启用WiFi热点 - Tethering Password WiFi热点密码 - - EDIT 编辑 - Enter new tethering password 输入新的WiFi热点密码 - IP Address IP地址 - Enable Roaming 启用数据漫游 - APN Setting APN设置 - Enter APN 输入APN - leave blank for automatic configuration 留空以自动配置 + + Cellular Metered + 按流量计费的手机移动网络 + + + Prevent large data uploads when on a metered connection + 当使用按流量计费的连接时，避免上传大流量数据 + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + 最高定速 + + + SPEED + SPEED + + + LIMIT + LIMIT + ConfirmationDialog - - Ok 好的 - Cancel 取消 @@ -90,17 +104,14 @@ DeclinePage - You must accept the Terms and Conditions in order to use openpilot. 您必须接受条款和条件以使用openpilot。 - Back 返回 - Decline, uninstall %1 拒绝并卸载%1 @@ -108,185 +119,157 @@ DevicePanel - Dongle ID 设备ID（Dongle ID） - N/A N/A - Serial 序列号 - Driver Camera 驾驶员摄像头 - PREVIEW 预览 - Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) - 打开并预览驾驶员摄像头，以确保驾驶员监控具有良好视野。仅熄火时可用。 + 打开并预览驾驶员摄像头，以确保驾驶员监控具有良好视野。（仅熄火时可用） - Reset Calibration 重置设备校准 - RESET 重置 - Are you sure you want to reset calibration? 您确定要重置设备校准吗？ - Review Training Guide 新手指南 - REVIEW 查看 - Review the rules, features, and limitations of openpilot 查看openpilot的使用规则，以及其功能和限制。 - Are you sure you want to review the training guide? 您确定要查看新手指南吗？ - Regulatory 监管信息 - VIEW 查看 - Change Language 切换语言 - CHANGE 切换 - Select a language 选择语言 - Reboot 重启 - Power Off 关机 - 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°和右4°之间，俯仰角在上5°和下8°之间。一般来说，openpilot会持续更新校准，很少需要重置。 - Your device is pointed %1° %2 and %3° %4. - 您的设备校准为%1° %2、%3° %4。 + 您的设备校准为%1° %2、%3° %4。 - down 朝下 - up 朝上 - left 朝左 - right 朝右 - Are you sure you want to reboot? 您确定要重新启动吗？ - Disengage to Reboot 取消openpilot以重新启动 - Are you sure you want to power off? 您确定要关机吗？ - Disengage to Power Off 取消openpilot以关机 + + Reset + 重置 + + + Review + 预览 + DriveStats - Drives 旅程数 - Hours 小时 - ALL TIME 全部 - PAST WEEK 过去一周 - KM 公里 - Miles 英里 @@ -294,20 +277,28 @@ DriverViewScene - camera starting 正在启动相机 + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + + + + CHILL MODE ON + + + InputDialog - Cancel 取消 - Need at least %n character(s)! 至少需要 %n 个字符！ @@ -317,22 +308,18 @@ Installer - Installing... 正在安装…… - Receiving objects: 正在接收： - Resolving deltas: 正在处理： - Updating files: 正在更新文件： @@ -340,27 +327,22 @@ MapETA - eta 埃塔 - min 分钟 - hr 小时 - km km - mi mi @@ -368,22 +350,18 @@ MapInstructions - km km - m m - mi mi - ft ft @@ -391,46 +369,38 @@ MapPanel - Current Destination 当前目的地 - CLEAR 清空 - Recent Destinations 最近目的地 - Try the Navigation Beta 试用导航测试版 - Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai - 订阅comma prime以获取导航。 + 订阅comma prime以获取导航。 立即注册：https://connect.comma.ai - No home location set 家：未设定 - No work location set 工作：未设定 - no recent destinations 无最近目的地 @@ -438,12 +408,10 @@ location set MapWindow - Map Loading 地图加载中 - Waiting for GPS 等待 GPS @@ -451,12 +419,10 @@ location set MultiOptionDialog - Select 选择 - Cancel 取消 @@ -464,72 +430,33 @@ location set Networking - Advanced 高级 - Enter password 输入密码 - - for "%1" 网络名称："%1" - Wrong password 密码错误 - - NvgWindow - - - km/h - km/h - - - - mph - mph - - - - - MAX - 最高定速 - - - - - SPEED - SPEED - - - - - LIMIT - LIMIT - - OffroadHome - UPDATE 更新 - ALERTS 警报 - ALERT 警报 @@ -537,55 +464,56 @@ location set PairingPopup - Pair your device to your comma account 将您的设备与comma账号配对 - Go to https://connect.comma.ai on your phone 在手机上访问 https://connect.comma.ai - Click "add new device" and scan the QR code on the right 点击“添加新设备”，扫描右侧二维码 - Bookmark connect.comma.ai to your home screen to use it like an app 将 connect.comma.ai 收藏到您的主屏幕，以便像应用程序一样使用它 + + ParamControl + + Cancel + 取消 + + + Enable + 启用 + + PrimeAdWidget - Upgrade Now 现在升级 - Become a comma prime member at connect.comma.ai 打开connect.comma.ai以注册comma prime会员 - PRIME FEATURES: comma prime特权： - Remote access 远程访问 - 1 year of storage 1年数据存储 - Developer perks 开发者福利 @@ -593,22 +521,18 @@ location set PrimeUserWidget - ✓ SUBSCRIBED ✓ 已订阅 - comma prime comma prime - CONNECT.COMMA.AI CONNECT.COMMA.AI - COMMA POINTS COMMA POINTS点数 @@ -616,41 +540,34 @@ location set QObject - Reboot 重启 - Exit 退出 - dashcam 行车记录仪 - openpilot openpilot - %n minute(s) ago %n 分钟前 - %n hour(s) ago %n 小时前 - %n day(s) ago %n 天前 @@ -660,89 +577,65 @@ location set Reset - Reset failed. Reboot to try again. 重置失败。 重新启动以重试。 - 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 取消 - Reboot 重启 - Confirm 确认 - Unable to mount data partition. Press confirm to reset your device. 无法挂载数据分区。 确认以重置您的设备。 - - RichTextDialog - - - Ok - 好的 - - SettingsWindow - × × - Device 设备 - - Network 网络 - Toggles 设定 - Software 软件 - Navigation 导航 @@ -750,105 +643,82 @@ location set Setup - WARNING: Low Voltage 警告：低电压 - Power your device in a car with a harness or proceed at your own risk. 请使用car harness线束为您的设备供电，或自行承担风险。 - Power off 关机 - - - Continue 继续 - Getting Started 开始设置 - Before we get on the road, let’s finish installation and cover some details. 开始旅程之前，让我们完成安装并介绍一些细节。 - Connect to Wi-Fi 连接到WiFi - - Back 返回 - Continue without Wi-Fi 不连接WiFi并继续 - Waiting for internet 等待网络连接 - Choose Software to Install 选择要安装的软件 - Dashcam Dashcam（行车记录仪） - Custom Software 自定义软件 - Enter URL 输入网址 - for Custom Software 以下载自定义软件 - Downloading... 正在下载…… - Download Failed 下载失败 - Ensure the entered URL is valid, and the device’s internet connection is good. 请确保互联网连接良好且输入的URL有效。 - Reboot device 重启设备 - Start over 重来 @@ -856,17 +726,14 @@ location set SetupWidget - Finish Setup 完成设置 - Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. 将您的设备与comma connect （connect.comma.ai）配对并领取您的comma prime优惠。 - Pair device 配对设备 @@ -874,106 +741,82 @@ location set Sidebar - - CONNECT CONNECT - OFFLINE 离线 - - ONLINE 在线 - ERROR 连接出错 - - - TEMP 设备温度 - HIGH 过热 - GOOD 良好 - OK 一般 - VEHICLE 车辆连接 - NO 无 - PANDA PANDA - GPS GPS - SEARCH 搜索中 - -- -- - Wi-Fi Wi-Fi - ETH 以太网 - 2G 2G - 3G 3G - LTE LTE - 5G 5G @@ -981,138 +824,93 @@ location set SoftwarePanel - - Git Branch - Git Branch + Updates are only downloaded while the car is off. + 车辆熄火时才能下载升级文件。 - - Git Commit - Git Commit + Current Version + 当前版本 - - OS Version - 系统版本 + Download + 下载 - - Version - 软件版本 + Install Update + 安装更新 - - Last Update Check - 上次检查更新 + INSTALL + 安装 - - The last time openpilot successfully checked for an update. The updater only runs while the car is off. - 上一次成功检查更新的时间。更新程序仅在汽车熄火时运行。 + Target Branch + 目标分支 - - Check for Update - 检查更新 + SELECT + 选择 - - CHECKING - 正在检查更新 + Select a branch + 选择分支 - - Switch Branch - 切换分支 - - - - ENTER - 输入 - - - - - The new branch will be pulled the next time the updater runs. - 分支将在更新服务下次启动时自动切换。 - - - - Enter branch name - 输入分支名称 - - - UNINSTALL 卸载 - Uninstall %1 卸载 %1 - Are you sure you want to uninstall? 您确定要卸载吗？ - - failed to fetch update - 获取更新失败 - - - - CHECK 查看 + + Uninstall + 卸载 + SshControl - SSH Keys SSH密钥 - Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. 警告：这将授予SSH访问权限给您GitHub设置中的所有公钥。切勿输入您自己以外的GitHub用户名。comma员工永远不会要求您添加他们的GitHub用户名。 - - ADD 添加 - Enter your GitHub username 输入您的GitHub用户名 - LOADING 正在加载 - REMOVE 删除 - Username '%1' has no keys on GitHub 用户名“%1”在GitHub上没有密钥 - Request timed out 请求超时 - Username '%1' doesn't exist on GitHub GitHub上不存在用户名“%1” @@ -1120,7 +918,6 @@ location set SshToggle - Enable SSH 启用SSH @@ -1128,22 +925,18 @@ location set TermsPage - Terms & Conditions 条款和条件 - Decline 拒绝 - Scroll to accept 滑动以接受 - Agree 同意 @@ -1151,125 +944,121 @@ location set TogglesPanel - Enable openpilot 启用openpilot - Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. 使用openpilot进行自适应巡航和车道保持辅助。使用此功能时您必须时刻保持注意力。该设置的更改在熄火时生效。 - Enable Lane Departure Warnings 启用车道偏离警告 - Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). 车速超过31mph（50km/h）时，若检测到车辆越过车道线且未打转向灯，系统将发出警告以提醒您返回车道。 - Use Metric System 使用公制单位 - Display speed in km/h instead of mph. 显示车速时，以km/h代替mph。 - Record and Upload Driver Camera 录制并上传驾驶员摄像头 - Upload data from the driver facing camera and help improve the driver monitoring algorithm. 上传驾驶员摄像头的数据，帮助改进驾驶员监控算法。 - - Disengage On Accelerator Pedal + Experimental openpilot Longitudinal Control + 试验性的openpilot纵向控制 + + + Disengage on Accelerator Pedal 踩油门时取消控制 - When enabled, pressing the accelerator pedal will disengage openpilot. 启用后，踩下油门踏板将取消openpilot。 - Show ETA in 24h Format 以24小时格式显示预计到达时间 - Use 24h format instead of am/pm 使用24小时制代替am/pm - Show Map on Left Side of UI 在介面左侧显示地图 - Show map on left side when in split screen view. 在分屏模式中，将地图置于屏幕左侧。 - - openpilot Longitudinal Control - openpilot纵向控制 + Experimental Mode + 测试模式 - - openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! - openpilot将禁用车辆的雷达并接管油门和刹车的控制。警告：AEB将被禁用！ + 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 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: <br> <h4>🌮 End-to-End Longitudinal Control 🌮</h4> Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. <br> <h4>New Driving Visualization</h4> The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + + + + Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. + + + + Enable experimental longitudinal control to allow experimental mode. + Updater - Update Required 需要更新 - An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. 操作系统需要更新。请将您的设备连接到WiFi以获取更快的更新体验。下载大小约为1GB。 - Connect to Wi-Fi 连接到WiFi - Install 安装 - Back 返回 - Loading... 正在加载…… - Reboot 重启 - Update failed 更新失败 @@ -1277,25 +1066,24 @@ location set WifiUI - - Scanning for networks... 正在扫描网络…… - CONNECTING... 正在连接…… - FORGET - 忘记 + 忽略 - Forget Wi-Fi Network "%1"? - 忘记WiFi网络 "%1"? + 忽略WiFi网络 "%1"? + + + Forget + 忽略 diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index f4681f85d..f667fdc97 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -4,17 +4,14 @@ AbstractAlert - Close 關閉 - Snooze Update 暫停更新 - Reboot and Update 重啟並更新 @@ -22,67 +19,84 @@ AdvancedNetworking - Back 回上頁 - Enable Tethering 啟用網路分享 - Tethering Password 網路分享密碼 - - EDIT 編輯 - Enter new tethering password 輸入新的網路分享密碼 - IP Address IP 地址 - Enable Roaming 啟用漫遊 - APN Setting APN 設置 - Enter APN 輸入 APN - leave blank for automatic configuration 留空白將自動配置 + + Cellular Metered + 行動網路 + + + Prevent large data uploads when on a metered connection + 防止使用行動網路上傳大量的數據 + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + 最高 + + + SPEED + 速度 + + + LIMIT + 速限 + ConfirmationDialog - - Ok 確定 - Cancel 取消 @@ -90,17 +104,14 @@ DeclinePage - You must accept the Terms and Conditions in order to use openpilot. 您必須先接受條款和條件才能使用 openpilot。 - Back 回上頁 - Decline, uninstall %1 拒絕並卸載 %1 @@ -108,185 +119,157 @@ DevicePanel - Dongle ID Dongle ID - N/A 無法使用 - Serial 序號 - Driver Camera 駕駛員攝像頭 - PREVIEW 預覽 - Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) 預覽駕駛員監控鏡頭畫面，以確保其具有良好視野。（僅在熄火時可用） - Reset Calibration 重置校準 - RESET 重置 - Are you sure you want to reset calibration? 您確定要重置校準嗎？ - Review Training Guide 觀看使用教學 - REVIEW 觀看 - Review the rules, features, and limitations of openpilot 觀看 openpilot 的使用規則、功能和限制 - Are you sure you want to review the training guide? 您確定要觀看使用教學嗎？ - Regulatory 法規/監管 - VIEW 觀看 - Change Language 更改語言 - CHANGE 更改 - Select a language 選擇語言 - Reboot 重新啟動 - Power Off 關機 - 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° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。 - Your device is pointed %1° %2 and %3° %4. 你的設備目前朝%2 %1° 以及朝%4 %3° 。 - down 下 - up 上 - left 左 - right 右 - Are you sure you want to reboot? 您確定要重新啟動嗎？ - Disengage to Reboot 請先取消控車才能重新啟動 - Are you sure you want to power off? 您確定您要關機嗎？ - Disengage to Power Off 請先取消控車才能關機 + + Reset + 重設 + + + Review + 回顧 + DriveStats - Drives 旅程 - Hours 小時 - ALL TIME 總共 - PAST WEEK 上周 - KM 公里 - Miles 英里 @@ -294,20 +277,28 @@ DriverViewScene - camera starting 開啟相機中 + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + + + + CHILL MODE ON + + + InputDialog - Cancel 取消 - Need at least %n character(s)! 需要至少 %n 個字元！ @@ -317,22 +308,18 @@ Installer - Installing... 安裝中… - Receiving objects: 接收對象： - Resolving deltas: 分析差異： - Updating files: 更新檔案： @@ -340,27 +327,22 @@ MapETA - eta 抵達 - min 分鐘 - hr 小時 - km km - mi mi @@ -368,22 +350,18 @@ MapInstructions - km km - m m - mi mi - ft ft @@ -391,48 +369,40 @@ MapPanel - Current Destination 當前目的地 - CLEAR 清除 - Recent Destinations 最近目的地 - Try the Navigation Beta 試用導航功能 - Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai 成為 comma 高級會員來使用導航功能 立即註冊：https://connect.comma.ai - No home location set 未設定 住家位置 - No work location set 未設定 工作位置 - no recent destinations 沒有最近的導航記錄 @@ -440,12 +410,10 @@ location set MapWindow - Map Loading 地圖加載中 - Waiting for GPS 等待 GPS @@ -453,12 +421,10 @@ location set MultiOptionDialog - Select 選擇 - Cancel 取消 @@ -466,72 +432,33 @@ location set Networking - Advanced 進階 - Enter password 輸入密碼 - - for "%1" 給 "%1" - Wrong password 密碼錯誤 - - NvgWindow - - - km/h - km/h - - - - mph - mph - - - - - MAX - 最高 - - - - - SPEED - 速度 - - - - - LIMIT - 速限 - - OffroadHome - UPDATE 更新 - ALERTS 提醒 - ALERT 提醒 @@ -539,55 +466,56 @@ location set PairingPopup - Pair your device to your comma account 將設備與您的 comma 帳號配對 - Go to https://connect.comma.ai on your phone 用手機連至 https://connect.comma.ai - Click "add new device" and scan the QR code on the right 點選 "add new device" 後掃描右邊的二維碼 - Bookmark connect.comma.ai to your home screen to use it like an app 將 connect.comma.ai 加入您的主屏幕，以便像手機 App 一樣使用它 + + ParamControl + + Cancel + 取消 + + + Enable + 啟用 + + PrimeAdWidget - Upgrade Now 馬上升級 - Become a comma prime member at connect.comma.ai 成為 connect.comma.ai 的高級會員 - PRIME FEATURES: 高級會員特點： - Remote access 遠程訪問 - 1 year of storage 一年的雲端行車記錄 - Developer perks 開發者福利 @@ -595,22 +523,18 @@ location set PrimeUserWidget - ✓ SUBSCRIBED ✓ 已訂閱 - comma prime comma 高級會員 - CONNECT.COMMA.AI CONNECT.COMMA.AI - COMMA POINTS COMMA 積分 @@ -618,41 +542,34 @@ location set QObject - Reboot 重新啟動 - Exit 離開 - dashcam 行車記錄器 - openpilot openpilot - %n minute(s) ago %n 分鐘前 - %n hour(s) ago %n 小時前 - %n day(s) ago %n 天前 @@ -662,89 +579,65 @@ location set Reset - Reset failed. Reboot to try again. 重置失敗。請重新啟動後再試。 - 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 取消 - Reboot 重新啟動 - Confirm 確認 - Unable to mount data partition. Press confirm to reset your device. 無法掛載數據分區。請按確認重置您的設備。 - - RichTextDialog - - - Ok - 確定 - - SettingsWindow - × × - Device 設備 - - Network 網路 - Toggles 設定 - Software 軟體 - Navigation 導航 @@ -752,105 +645,82 @@ location set Setup - WARNING: Low Voltage 警告：電壓過低 - Power your device in a car with a harness or proceed at your own risk. 請使用車上 harness 提供的電源，若繼續的話您需要自擔風險。 - Power off 關機 - - - Continue 繼續 - Getting Started 入門 - Before we get on the road, let’s finish installation and cover some details. 在我們上路之前，讓我們完成安裝並介紹一些細節。 - Connect to Wi-Fi 連接到無線網絡 - - Back 回上頁 - Continue without Wi-Fi 在沒有 Wi-Fi 的情況下繼續 - Waiting for internet 連接至網路中 - Choose Software to Install 選擇要安裝的軟體 - Dashcam 行車記錄器 - Custom Software 定制的軟體 - Enter URL 輸入網址 - for Custom Software 定制的軟體 - Downloading... 下載中… - Download Failed 下載失敗 - Ensure the entered URL is valid, and the device’s internet connection is good. 請確定您輸入的是有效的安裝網址，並且確定設備的網路連線狀態良好。 - Reboot device 重新啟動 - Start over 重新開始 @@ -858,17 +728,14 @@ location set SetupWidget - Finish Setup 完成設置 - Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. 將您的設備與 comma connect (connect.comma.ai) 配對並領取您的 comma 高級會員優惠。 - Pair device 配對設備 @@ -876,106 +743,82 @@ location set Sidebar - - CONNECT 雲端服務 - OFFLINE 已離線 - - ONLINE 已連線 - ERROR 錯誤 - - - TEMP 溫度 - HIGH 偏高 - GOOD 正常 - OK 一般 - VEHICLE 車輛通訊 - NO 未連線 - PANDA 車輛通訊 - GPS GPS - SEARCH 車輛通訊 - -- -- - Wi-Fi - ETH - 2G - 3G - LTE - 5G @@ -983,138 +826,93 @@ location set SoftwarePanel - - Git Branch - Git 分支 + Updates are only downloaded while the car is off. + 系統更新只會在熄火時下載。 - - Git Commit - Git 提交 + Current Version + 當前版本 - - OS Version - 系統版本 + Download + 下載 - - Version - 版本 + Install Update + 安裝更新 - - Last Update Check - 上次檢查時間 + INSTALL + 安裝 - - The last time openpilot successfully checked for an update. The updater only runs while the car is off. - 上次成功檢查更新的時間。更新系統只會在車子熄火時執行。 + Target Branch + 目標分支 - - Check for Update - 檢查更新 + SELECT + 選取 - - CHECKING - 檢查中 + Select a branch + 選取一個分支 - - Switch Branch - 切換分支 - - - - ENTER - 切換 - - - - - The new branch will be pulled the next time the updater runs. - 新的分支將會在下次檢查更新時切換過去。 - - - - Enter branch name - 輸入分支名稱 - - - UNINSTALL 卸載 - Uninstall %1 卸載 %1 - Are you sure you want to uninstall? 您確定您要卸載嗎？ - - failed to fetch update - 下載更新失敗 - - - - CHECK 檢查 + + Uninstall + 卸載 + SshControl - SSH Keys SSH 密鑰 - Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. 警告：這將授權給 GitHub 帳號中所有公鑰 SSH 訪問權限。切勿輸入非您自己的 GitHub 用戶名。comma 員工「永遠不會」要求您添加他們的 GitHub 用戶名。 - - ADD 新增 - Enter your GitHub username 請輸入您 GitHub 的用戶名 - LOADING 載入中 - REMOVE 移除 - Username '%1' has no keys on GitHub GitHub 用戶 '%1' 沒有設定任何密鑰 - Request timed out 請求超時 - Username '%1' doesn't exist on GitHub GitHub 用戶 '%1' 不存在 @@ -1122,7 +920,6 @@ location set SshToggle - Enable SSH 啟用 SSH 服務 @@ -1130,22 +927,18 @@ location set TermsPage - Terms & Conditions 條款和條件 - Decline 拒絕 - Scroll to accept 滑動至頁尾接受條款 - Agree 接受 @@ -1153,125 +946,121 @@ location set TogglesPanel - Enable openpilot 啟用 openpilot - Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. 使用 openpilot 的主動式巡航和車道保持功能，開啟後您需要持續集中注意力，設定變更在重新啟動車輛後生效。 - Enable Lane Departure Warnings 啟用車道偏離警告 - Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). 車速在時速 50 公里 (31 英里) 以上且未打方向燈的情況下，如果偵測到車輛駛出目前車道線時，發出車道偏離警告。 - Use Metric System 使用公制單位 - Display speed in km/h instead of mph. 啟用後，速度單位顯示將從 mp/h 改為 km/h。 - Record and Upload Driver Camera 記錄並上傳駕駛監控影像 - Upload data from the driver facing camera and help improve the driver monitoring algorithm. 上傳駕駛監控的錄像來協助我們提升駕駛監控的準確率。 - - Disengage On Accelerator Pedal + Experimental openpilot Longitudinal Control + 使用 openpilot 縱向控制（實驗） + + + Disengage on Accelerator Pedal 油門取消控車 - When enabled, pressing the accelerator pedal will disengage openpilot. 啟用後，踩踏油門將會取消 openpilot 控制。 - Show ETA in 24h Format 預計到達時間單位改用 24 小時制 - Use 24h format instead of am/pm 使用 24 小時制。(預設值為 12 小時制) - Show Map on Left Side of UI 將地圖顯示在畫面的左側 - Show map on left side when in split screen view. 進入分割畫面後，地圖將會顯示在畫面的左側。 - - openpilot Longitudinal Control - openpilot 縱向控制 + Experimental Mode + 實驗模式 - - openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! - openpilot 將會關閉雷達訊號並接管油門和剎車的控制。注意：這也會關閉自動緊急煞車 (AEB) 系統！ + 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 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: <br> <h4>🌮 End-to-End Longitudinal Control 🌮</h4> Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. <br> <h4>New Driving Visualization</h4> The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + + + + Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. + + + + Enable experimental longitudinal control to allow experimental mode. + Updater - Update Required 系統更新 - An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. 設備的操作系統需要更新。請將您的設備連接到 Wi-Fi 以獲得最快的更新體驗。下載大小約為 1GB。 - Connect to Wi-Fi 連接到無線網絡 - Install 安裝 - Back 回上頁 - Loading... 載入中… - Reboot 重新啟動 - Update failed 更新失敗 @@ -1279,25 +1068,24 @@ location set WifiUI - - Scanning for networks... 掃描無線網路中... - CONNECTING... 連線中... - FORGET 清除 - Forget Wi-Fi Network "%1"? 清除 Wi-Fi 網路 "%1"? + + Forget + 清除 + diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 317ef497a..2d4533afe 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -23,8 +23,8 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, const QRectF clip_region{-margin, -margin, s->fb_w + 2 * margin, s->fb_h + 2 * margin}; const vec3 pt = (vec3){{in_x, in_y, in_z}}; - const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); - const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); + const vec3 Ep = matvecmul3(s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib, pt); + const vec3 KEp = matvecmul3(s->scene.wide_cam ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); // Project. QPointF point = s->car_space_transform.map(QPointF{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2]}); @@ -35,7 +35,7 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, return false; } -static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { +int get_path_length_idx(const cereal::ModelDataV2::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 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line return max_idx; } -static 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::ModelDataV2::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 @@ static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_sta } } -static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, +void update_line_data(const UIState *s, const cereal::ModelDataV2::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(); @@ -63,6 +63,8 @@ static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTDa right_points.reserve(max_idx + 1); for (int i = 0; i <= max_idx; i++) { + // highly negative x positions are drawn above the frame and cause flickering, clip to zy plane of camera + if (line_x[i] < 0) continue; QPointF left, right; bool l = calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, &left); bool r = calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, &right); @@ -78,7 +80,7 @@ static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTDa *pvd = left_points + right_points; } -static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { +void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { UIScene &scene = s->scene; auto model_position = model.getPosition(); float max_distance = std::clamp(model_position.getX()[TRAJECTORY_SIZE - 1], @@ -121,28 +123,27 @@ static void update_state(UIState *s) { if (sm.updated("liveCalibration")) { auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + auto wfde_list = sm["liveCalibration"].getLiveCalibration().getWideFromDeviceEuler(); Eigen::Vector3d rpy; - rpy << rpy_list[0], rpy_list[1], rpy_list[2]; + Eigen::Vector3d wfde; + if (rpy_list.size() == 3) rpy << rpy_list[0], rpy_list[1], rpy_list[2]; + if (wfde_list.size() == 3) wfde << wfde_list[0], wfde_list[1], wfde_list[2]; Eigen::Matrix3d device_from_calib = euler2rot(rpy); + Eigen::Matrix3d wide_from_device = euler2rot(wfde); Eigen::Matrix3d view_from_device; view_from_device << 0,1,0, 0,0,1, 1,0,0; Eigen::Matrix3d view_from_calib = view_from_device * device_from_calib; + Eigen::Matrix3d view_from_wide_calib = view_from_device * wide_from_device * device_from_calib ; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j); + scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i,j); } } scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; - } - if (s->worldObjectsVisible()) { - if (sm.updated("modelV2")) { - update_model(s, sm["modelV2"].getModelV2()); - } - if (sm.updated("radarState") && sm.rcv_frame("modelV2") > s->scene.started_frame) { - update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); - } + scene.calibration_wide_valid = wfde_list.size() == 3; } if (sm.updated("pandaStates")) { auto pandaStates = sm["pandaStates"].getPandaStates(); @@ -162,31 +163,9 @@ static void update_state(UIState *s) { if (sm.updated("carParams")) { scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); } - if (!scene.started && sm.updated("sensorEvents")) { - for (auto sensor : sm["sensorEvents"].getSensorEvents()) { - if (sensor.which() == cereal::SensorEventData::ACCELERATION) { - auto accel = sensor.getAcceleration().getV(); - if (accel.totalSize().wordCount) { // TODO: sometimes empty lists are received. Figure out why - scene.accel_sensor = accel[2]; - } - } else if (sensor.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) { - auto gyro = sensor.getGyroUncalibrated().getV(); - if (gyro.totalSize().wordCount) { - scene.gyro_sensor = gyro[1]; - } - } - } - } if (sm.updated("wideRoadCameraState")) { - auto camera_state = sm["wideRoadCameraState"].getWideRoadCameraState(); - - float max_lines = 1618; - float max_gain = 10.0; - float max_ev = max_lines * max_gain / 6; - - float ev = camera_state.getGain() * float(camera_state.getIntegLines()); - - scene.light_sensor = std::clamp(1.0 - (ev / max_ev), 0.0, 1.0); + float scale = (sm["wideRoadCameraState"].getWideRoadCameraState().getSensor() == cereal::FrameData::ImageSensor::AR0321) ? 6.0f : 1.0f; + scene.light_sensor = std::max(100.0f - scale * sm["wideRoadCameraState"].getWideRoadCameraState().getExposureValPercent(), 0.0f); } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; } @@ -218,23 +197,31 @@ void UIState::updateStatus() { if (scene.started) { status = STATUS_DISENGAGED; scene.started_frame = sm->frame; - wide_camera = Params().getBool("WideCameraOnly"); + 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", "sensorEvents", "carState", "liveLocationKalman", + "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements", }); Params params; - wide_camera = params.getBool("WideCameraOnly"); + wide_cam_only = params.getBool("WideCameraOnly"); prime_type = std::atoi(params.get("PrimeType").c_str()); + language = QString::fromStdString(params.get("LanguageSetting")); // update timer timer = new QTimer(this); @@ -284,8 +271,7 @@ void Device::resetInteractiveTimout() { void Device::updateBrightness(const UIState &s) { float clipped_brightness = BACKLIGHT_OFFROAD; if (s.scene.started) { - // Scale to 0% to 100% - clipped_brightness = 100.0 * s.scene.light_sensor; + clipped_brightness = s.scene.light_sensor; // CIE 1931 - https://www.photonstophotos.net/GeneralTopics/Exposure/Psychometric_Lightness_and_Gamma.htm if (clipped_brightness <= 8) { @@ -311,24 +297,11 @@ void Device::updateBrightness(const UIState &s) { } } -bool Device::motionTriggered(const UIState &s) { - static float accel_prev = 0; - static float gyro_prev = 0; - - bool accel_trigger = abs(s.scene.accel_sensor - accel_prev) > 0.2; - bool gyro_trigger = abs(s.scene.gyro_sensor - gyro_prev) > 0.15; - - gyro_prev = s.scene.gyro_sensor; - accel_prev = (accel_prev * (accel_samples - 1) + s.scene.accel_sensor) / accel_samples; - - return (!awake && accel_trigger && gyro_trigger); -} - void Device::updateWakefulness(const UIState &s) { bool ignition_just_turned_off = !s.scene.ignition && ignition_on; ignition_on = s.scene.ignition; - if (ignition_just_turned_off || motionTriggered(s)) { + if (ignition_just_turned_off) { resetInteractiveTimout(); } else if (interactive_timeout > 0 && --interactive_timeout == 0) { emit interactiveTimout(); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 16f78cdef..d6f5c3e2e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -87,7 +87,10 @@ const QColor bg_colors [] = { typedef struct UIScene { bool calibration_valid = false; + bool calibration_wide_valid = false; + bool wide_cam = true; mat3 view_from_calib = DEFAULT_CALIBRATION; + mat3 view_from_wide_calib = DEFAULT_CALIBRATION; cereal::PandaState::PandaType pandaType; // modelV2 @@ -100,7 +103,7 @@ typedef struct UIScene { // lead QPointF lead_vertices[2]; - float light_sensor, accel_sensor, gyro_sensor; + float light_sensor; bool started, ignition, is_metric, map_on_left, longitudinal_control; uint64_t started_frame; } UIScene; @@ -126,14 +129,16 @@ public: UIScene scene = {}; bool awake; - int prime_type = 0; + int prime_type; + QString language; QTransform car_space_transform; - bool wide_camera; + bool wide_cam_only; signals: void uiUpdate(const UIState &s); void offroadTransition(bool offroad); + void primeTypeChanged(int prime_type); private slots: void update(); @@ -141,6 +146,7 @@ private slots: private: QTimer *timer; bool started_prev = false; + int prime_type_prev = -1; }; UIState *uiState(); @@ -154,9 +160,6 @@ public: Device(QObject *parent = 0); private: - // auto brightness - const float accel_samples = 5*UI_FREQ; - bool awake = false; int interactive_timeout = 0; bool ignition_on = false; @@ -179,3 +182,8 @@ 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); +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, + 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 new file mode 100755 index 000000000..e15d4c343 --- /dev/null +++ b/selfdrive/ui/update_translations.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +import argparse +import json +import os + +from common.basedir import BASEDIR + +UI_DIR = os.path.join(BASEDIR, "selfdrive", "ui") +TRANSLATIONS_DIR = os.path.join(UI_DIR, "translations") +LANGUAGES_FILE = os.path.join(TRANSLATIONS_DIR, "languages.json") + + +def update_translations(vanish=False, plural_only=None, translations_dir=TRANSLATIONS_DIR): + if plural_only is None: + plural_only = [] + + with open(LANGUAGES_FILE, "r") as f: + translation_files = json.load(f) + + 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}" + if vanish: + args += " -no-obsolete" + if file in plural_only: + args += " -pluralonly" + ret = os.system(args) + assert ret == 0 + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Update translation files for UI", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--vanish", action="store_true", help="Remove translations with source text no longer found") + parser.add_argument("--plural-only", type=str, nargs="*", default=["main_en"], help="Translation codes to only create plural translations for (ie. the base language)") + args = parser.parse_args() + + update_translations(args.vanish, args.plural_only) diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index d6b5cc67a..ec35c29b6 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -19,15 +19,15 @@ int main(int argc, char *argv[]) { { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_MAP, false)); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_ROAD, false)); + hlayout->addWidget(new CameraWidget("navd", VISION_STREAM_MAP, false)); + hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_ROAD, false)); } { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_DRIVER, false)); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_WIDE_ROAD, false)); + hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_DRIVER, false)); + hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_WIDE_ROAD, false)); } return a.exec(); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index e8905962b..9da2a05a1 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -1,28 +1,6 @@ #!/usr/bin/env python3 - -# Safe Update: A simple service that waits for network access and tries to -# update every 10 minutes. It's intended to make the OP update process more -# robust against Git repository corruption. This service DOES NOT try to fix -# an already-corrupt BASEDIR Git repo, only prevent it from happening. -# -# During normal operation, both onroad and offroad, the update process makes -# no changes to the BASEDIR install of OP. All update attempts are performed -# in a disposable staging area provided by OverlayFS. It assumes the deleter -# process provides enough disk space to carry out the process. -# -# If an update succeeds, a flag is set, and the update is swapped in at the -# next reboot. If an update is interrupted or otherwise fails, the OverlayFS -# upper layer and metadata can be discarded before trying again. -# -# The swap on boot is triggered by launch_chffrplus.sh -# gated on the existence of $FINALIZED/.overlay_consistent and also the -# existence and mtime of $BASEDIR/.overlay_init. -# -# Other than build byproducts, BASEDIR should not be modified while this -# service is running. Developers modifying code directly in BASEDIR should -# disable this service. - import os +import re import datetime import subprocess import psutil @@ -31,8 +9,9 @@ import signal import fcntl import time import threading +from collections import defaultdict from pathlib import Path -from typing import List, Tuple, Optional +from typing import List, Union, Optional from markdown_it import MarkdownIt from common.basedir import BASEDIR @@ -50,42 +29,33 @@ OVERLAY_METADATA = os.path.join(STAGING_ROOT, "metadata") OVERLAY_MERGED = os.path.join(STAGING_ROOT, "merged") FINALIZED = os.path.join(STAGING_ROOT, "finalized") +OVERLAY_INIT = Path(os.path.join(BASEDIR, ".overlay_init")) + DAYS_NO_CONNECTIVITY_MAX = 14 # do not allow to engage after this many days DAYS_NO_CONNECTIVITY_PROMPT = 10 # send an offroad prompt after this many days class WaitTimeHelper: - def __init__(self, proc): - self.proc = proc + def __init__(self): self.ready_event = threading.Event() - self.shutdown = False - signal.signal(signal.SIGTERM, self.graceful_shutdown) - signal.signal(signal.SIGINT, self.graceful_shutdown) + self.only_check_for_update = False signal.signal(signal.SIGHUP, self.update_now) - - def graceful_shutdown(self, signum: int, frame) -> None: - # umount -f doesn't appear effective in avoiding "device busy" on NEOS, - # so don't actually die until the next convenient opportunity in main(). - cloudlog.info("caught SIGINT/SIGTERM, dismounting overlay at next opportunity") - - # forward the signal to all our child processes - child_procs = self.proc.children(recursive=True) - for p in child_procs: - p.send_signal(signum) - - self.shutdown = True - self.ready_event.set() + signal.signal(signal.SIGUSR1, self.check_now) def update_now(self, signum: int, frame) -> None: - cloudlog.info("caught SIGHUP, running update check immediately") + cloudlog.info("caught SIGHUP, attempting to downloading update") + self.only_check_for_update = False + self.ready_event.set() + + def check_now(self, signum: int, frame) -> None: + cloudlog.info("caught SIGUSR1, checking for updates") + self.only_check_for_update = True self.ready_event.set() def sleep(self, t: float) -> None: self.ready_event.wait(timeout=t) -def run(cmd: List[str], cwd: Optional[str] = None, low_priority: bool = False): - if low_priority: - cmd = ["nice", "-n", "19"] + cmd +def run(cmd: List[str], cwd: Optional[str] = None) -> str: return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8') @@ -98,59 +68,19 @@ def set_consistent_flag(consistent: bool) -> None: consistent_file.unlink(missing_ok=True) os.sync() - -def set_params(new_version: bool, failed_count: int, exception: Optional[str]) -> None: - params = Params() - - params.put("UpdateFailedCount", str(failed_count)) - - last_update = datetime.datetime.utcnow() - if failed_count == 0: - t = last_update.isoformat() - params.put("LastUpdateTime", t.encode('utf8')) - else: +def parse_release_notes(basedir: str) -> bytes: + try: + with open(os.path.join(basedir, "RELEASES.md"), "rb") as f: + r = f.read().split(b'\n\n', 1)[0] # Slice latest release notes try: - t = params.get("LastUpdateTime", encoding='utf8') - last_update = datetime.datetime.fromisoformat(t) - except (TypeError, ValueError): - pass - - if exception is None: - params.delete("LastUpdateException") - else: - params.put("LastUpdateException", exception) - - # Write out release notes for new versions - if new_version: - try: - with open(os.path.join(FINALIZED, "RELEASES.md"), "rb") as f: - r = f.read().split(b'\n\n', 1)[0] # Slice latest release notes - try: - params.put("ReleaseNotes", MarkdownIt().render(r.decode("utf-8"))) - except Exception: - params.put("ReleaseNotes", r + b"\n") + return bytes(MarkdownIt().render(r.decode("utf-8")), encoding="utf-8") except Exception: - params.put("ReleaseNotes", "") - params.put_bool("UpdateAvailable", True) - - # Handle user prompt - for alert in ("Offroad_UpdateFailed", "Offroad_ConnectivityNeeded", "Offroad_ConnectivityNeededPrompt"): - set_offroad_alert(alert, False) - - now = datetime.datetime.utcnow() - dt = now - last_update - if failed_count > 15 and exception is not None: - if is_tested_branch(): - extra_text = "Ensure the software is correctly installed" - else: - extra_text = exception - set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text) - elif dt.days > DAYS_NO_CONNECTIVITY_MAX and failed_count > 1: - set_offroad_alert("Offroad_ConnectivityNeeded", True) - elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: - remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1) - set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.") - + return r + b"\n" + except FileNotFoundError: + pass + except Exception: + cloudlog.exception("failed to parse release notes") + return b"" def setup_git_options(cwd: str) -> None: # We sync FS object atimes (which NEOS doesn't use) and mtimes, but ctimes @@ -183,12 +113,10 @@ def dismount_overlay() -> None: def init_overlay() -> None: - overlay_init_file = Path(os.path.join(BASEDIR, ".overlay_init")) - # Re-create the overlay if BASEDIR/.git has changed since we created the overlay - if overlay_init_file.is_file(): + if OVERLAY_INIT.is_file() and os.path.ismount(OVERLAY_MERGED): git_dir_path = os.path.join(BASEDIR, ".git") - new_files = run(["find", git_dir_path, "-newer", str(overlay_init_file)]) + new_files = run(["find", git_dir_path, "-newer", str(OVERLAY_INIT)]) if not len(new_files.splitlines()): # A valid overlay already exists return @@ -219,7 +147,7 @@ def init_overlay() -> None: consistent_file = Path(os.path.join(BASEDIR, ".overlay_consistent")) if consistent_file.is_file(): consistent_file.unlink() - overlay_init_file.touch() + OVERLAY_INIT.touch() os.sync() overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}" @@ -228,12 +156,12 @@ def init_overlay() -> None: run(["sudo"] + mount_cmd) run(["sudo", "chmod", "755", os.path.join(OVERLAY_METADATA, "work")]) - git_diff = run(["git", "diff"], OVERLAY_MERGED, low_priority=True) + git_diff = run(["git", "diff"], OVERLAY_MERGED) params.put("GitDiff", git_diff) cloudlog.info(f"git diff output:\n{git_diff}") -def finalize_update(wait_helper: WaitTimeHelper) -> None: +def finalize_update() -> None: """Take the current OverlayFS merged view and finalize a copy outside of OverlayFS, ready to be swapped-in at BASEDIR. Copy using shutil.copytree""" @@ -247,24 +175,22 @@ def finalize_update(wait_helper: WaitTimeHelper) -> None: shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True) run(["git", "reset", "--hard"], FINALIZED) - run(["git", "submodule", "foreach", "--recursive", "git", "reset"], FINALIZED) + run(["git", "submodule", "foreach", "--recursive", "git", "reset", "--hard"], FINALIZED) - cloudlog.info("Starting git gc") + cloudlog.info("Starting git cleanup in finalized update") t = time.monotonic() try: run(["git", "gc"], FINALIZED) - cloudlog.event("Done git gc", duration=time.monotonic() - t) + run(["git", "lfs", "prune"], FINALIZED) + cloudlog.event("Done git cleanup", duration=time.monotonic() - t) except subprocess.CalledProcessError: - cloudlog.exception(f"Failed git gc, took {time.monotonic() - t:.3f} s") + cloudlog.exception(f"Failed git cleanup, took {time.monotonic() - t:.3f} s") - if wait_helper.shutdown: - cloudlog.info("got interrupted finalizing overlay") - else: - set_consistent_flag(True) - cloudlog.info("done finalizing overlay") + set_consistent_flag(True) + cloudlog.info("done finalizing overlay") -def handle_agnos_update(wait_helper: WaitTimeHelper) -> None: +def handle_agnos_update() -> None: from system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number cur_version = HARDWARE.get_os_version() @@ -287,65 +213,181 @@ def handle_agnos_update(wait_helper: WaitTimeHelper) -> None: set_offroad_alert("Offroad_NeosUpdate", False) -def check_git_fetch_result(fetch_txt: str) -> bool: - err_msg = "Failed to add the host to the list of known hosts (/data/data/com.termux/files/home/.ssh/known_hosts).\n" - return len(fetch_txt) > 0 and (fetch_txt != err_msg) +class Updater: + def __init__(self): + self.params = Params() + self.branches = defaultdict(lambda: '') + self._has_internet: bool = False -def check_for_update() -> Tuple[bool, bool]: - setup_git_options(OVERLAY_MERGED) - try: - git_fetch_output = run(["git", "fetch", "--dry-run"], OVERLAY_MERGED, low_priority=True) - return True, check_git_fetch_result(git_fetch_output) - except subprocess.CalledProcessError: - return False, False + @property + def has_internet(self) -> bool: + return self._has_internet + @property + def target_branch(self) -> str: + b: Union[str, None] = self.params.get("UpdaterTargetBranch", encoding='utf-8') + if b is None: + b = self.get_branch(BASEDIR) + self.params.put("UpdaterTargetBranch", b) + return b -def fetch_update(wait_helper: WaitTimeHelper) -> bool: - cloudlog.info("attempting git fetch inside staging overlay") + @property + def update_ready(self) -> bool: + consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent")) + if consistent_file.is_file(): + hash_mismatch = self.get_commit_hash(BASEDIR) != self.branches[self.target_branch] + branch_mismatch = self.get_branch(BASEDIR) != self.target_branch + on_target_branch = self.get_branch(FINALIZED) == self.target_branch + return ((hash_mismatch or branch_mismatch) and on_target_branch) + return False - setup_git_options(OVERLAY_MERGED) + @property + def update_available(self) -> bool: + if os.path.isdir(OVERLAY_MERGED): + hash_mismatch = self.get_commit_hash(OVERLAY_MERGED) != self.branches[self.target_branch] + branch_mismatch = self.get_branch(OVERLAY_MERGED) != self.target_branch + return hash_mismatch or branch_mismatch + return False - git_fetch_output = run(["git", "fetch"], OVERLAY_MERGED, low_priority=True) - cloudlog.info("git fetch success: %s", git_fetch_output) + def get_branch(self, path: str) -> str: + return run(["git", "rev-parse", "--abbrev-ref", "HEAD"], path).rstrip() - cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip() - upstream_hash = run(["git", "rev-parse", "@{u}"], OVERLAY_MERGED).rstrip() - new_version: bool = cur_hash != upstream_hash - git_fetch_result = check_git_fetch_result(git_fetch_output) + def get_commit_hash(self, path: str = OVERLAY_MERGED) -> str: + return run(["git", "rev-parse", "HEAD"], path).rstrip() - new_branch = Params().get("SwitchToBranch", encoding='utf8') - if new_branch is not None: - new_version = True + def set_params(self, failed_count: int, exception: Optional[str]) -> None: + self.params.put("UpdateFailedCount", str(failed_count)) - cloudlog.info(f"comparing {cur_hash} to {upstream_hash}") - if new_version or git_fetch_result: - cloudlog.info("Running update") + self.params.put_bool("UpdaterFetchAvailable", self.update_available) + self.params.put("UpdaterAvailableBranches", ','.join(self.branches.keys())) - if new_version: - cloudlog.info("git reset in progress") - cmds = [ - ["git", "reset", "--hard", "@{u}"], - ["git", "clean", "-xdf"], - ["git", "submodule", "init"], - ["git", "submodule", "update"], - ] - if new_branch is not None: - cloudlog.info(f"switching to branch {repr(new_branch)}") - cmds.insert(0, ["git", "checkout", "-f", new_branch]) - r = [run(cmd, OVERLAY_MERGED, low_priority=True) for cmd in cmds] - cloudlog.info("git reset success: %s", '\n'.join(r)) + last_update = datetime.datetime.utcnow() + if failed_count == 0: + t = last_update.isoformat() + self.params.put("LastUpdateTime", t.encode('utf8')) + else: + try: + t = self.params.get("LastUpdateTime", encoding='utf8') + last_update = datetime.datetime.fromisoformat(t) + except (TypeError, ValueError): + pass - if AGNOS: - handle_agnos_update(wait_helper) + if exception is None: + self.params.remove("LastUpdateException") + else: + self.params.put("LastUpdateException", exception) + + # Write out current and new version info + def get_description(basedir: str) -> str: + if not os.path.exists(basedir): + return "" + + version = "" + branch = "" + commit = "" + commit_date = "" + try: + branch = self.get_branch(basedir) + commit = self.get_commit_hash(basedir)[:7] + with open(os.path.join(basedir, "common", "version.h")) as f: + version = f.read().split('"')[1] + + commit_unix_ts = run(["git", "show", "-s", "--format=%ct", "HEAD"], basedir).rstrip() + dt = datetime.datetime.fromtimestamp(int(commit_unix_ts)) + commit_date = dt.strftime("%b %d") + except Exception: + cloudlog.exception("updater.get_description") + return f"{version} / {branch} / {commit} / {commit_date}" + self.params.put("UpdaterCurrentDescription", get_description(BASEDIR)) + self.params.put("UpdaterCurrentReleaseNotes", parse_release_notes(BASEDIR)) + self.params.put("UpdaterNewDescription", get_description(FINALIZED)) + self.params.put("UpdaterNewReleaseNotes", parse_release_notes(FINALIZED)) + self.params.put_bool("UpdateAvailable", self.update_ready) + + # Handle user prompt + for alert in ("Offroad_UpdateFailed", "Offroad_ConnectivityNeeded", "Offroad_ConnectivityNeededPrompt"): + set_offroad_alert(alert, False) + + now = datetime.datetime.utcnow() + dt = now - last_update + if failed_count > 15 and exception is not None and self.has_internet: + if is_tested_branch(): + extra_text = "Ensure the software is correctly installed. Uninstall and re-install if this error persists." + else: + extra_text = exception + set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text) + elif dt.days > DAYS_NO_CONNECTIVITY_MAX and failed_count > 1: + set_offroad_alert("Offroad_ConnectivityNeeded", True) + elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: + remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1) + set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.") + + def check_for_update(self) -> None: + cloudlog.info("checking for updates") + + excluded_branches = ('release2', 'release2-staging', 'dashcam', 'dashcam-staging') + + try: + run(["git", "ls-remote", "origin", "HEAD"], OVERLAY_MERGED) + self._has_internet = True + except subprocess.CalledProcessError: + self._has_internet = False + + setup_git_options(OVERLAY_MERGED) + output = run(["git", "ls-remote", "--heads"], OVERLAY_MERGED) + + self.branches = defaultdict(lambda: None) + for line in output.split('\n'): + ls_remotes_re = r'(?P\b[0-9a-f]{5,40}\b)(\s+)(refs\/heads\/)(?P.*$)' + x = re.fullmatch(ls_remotes_re, line.strip()) + if x is not None and x.group('branch_name') not in excluded_branches: + self.branches[x.group('branch_name')] = x.group('commit_sha') + + cur_branch = self.get_branch(OVERLAY_MERGED) + cur_commit = self.get_commit_hash(OVERLAY_MERGED) + new_branch = self.target_branch + new_commit = self.branches[new_branch] + if (cur_branch, cur_commit) != (new_branch, new_commit): + cloudlog.info(f"update available, {cur_branch} ({str(cur_commit)[:7]}) -> {new_branch} ({str(new_commit)[:7]})") + else: + cloudlog.info(f"up to date on {cur_branch} ({str(cur_commit)[:7]})") + + def fetch_update(self) -> None: + cloudlog.info("attempting git fetch inside staging overlay") + + self.params.put("UpdaterState", "downloading...") + + # TODO: cleanly interrupt this and invalidate old update + set_consistent_flag(False) + self.params.put_bool("UpdateAvailable", False) + + setup_git_options(OVERLAY_MERGED) + + branch = self.target_branch + git_fetch_output = run(["git", "fetch", "origin", branch], OVERLAY_MERGED) + cloudlog.info("git fetch success: %s", git_fetch_output) + + cloudlog.info("git reset in progress") + cmds = [ + ["git", "checkout", "--force", "--no-recurse-submodules", "-B", branch, "FETCH_HEAD"], + ["git", "reset", "--hard"], + ["git", "clean", "-xdff"], + ["git", "submodule", "sync"], + ["git", "submodule", "update", "--init", "--recursive"], + ["git", "submodule", "foreach", "--recursive", "git", "reset", "--hard"], + ] + r = [run(cmd, OVERLAY_MERGED) for cmd in cmds] + cloudlog.info("git reset success: %s", '\n'.join(r)) + + # TODO: show agnos download progress + if AGNOS: + handle_agnos_update() # Create the finalized, ready-to-swap update - finalize_update(wait_helper) - cloudlog.info("openpilot update successful!") - else: - cloudlog.info("nothing new from git at this time") - - return new_version + self.params.put("UpdaterState", "finalizing update...") + finalize_update() + cloudlog.info("finalize success!") def main() -> None: @@ -374,33 +416,37 @@ def main() -> None: t = datetime.datetime.utcnow().isoformat() params.put("InstallDate", t.encode('utf8')) - overlay_init = Path(os.path.join(BASEDIR, ".overlay_init")) - overlay_init.unlink(missing_ok=True) - + updater = Updater() update_failed_count = 0 # TODO: Load from param? - wait_helper = WaitTimeHelper(proc) + + # no fetch on the first time + wait_helper = WaitTimeHelper() + wait_helper.only_check_for_update = True # Run the update loop - while not wait_helper.shutdown: + while True: wait_helper.ready_event.clear() # Attempt an update exception = None - new_version = False - update_failed_count += 1 try: + # TODO: reuse overlay from previous updated instance if it looks clean init_overlay() - # TODO: still needed? skip this and just fetch? - # Lightweight internt check - internet_ok, update_available = check_for_update() - if internet_ok and not update_available: - update_failed_count = 0 + # ensure we have some params written soon after startup + updater.set_params(update_failed_count, exception) + update_failed_count += 1 - # Fetch update - if internet_ok: - new_version = fetch_update(wait_helper) - update_failed_count = 0 + # check for update + params.put("UpdaterState", "checking...") + updater.check_for_update() + + # download update + if wait_helper.only_check_for_update: + cloudlog.info("skipping fetch this cycle") + else: + updater.fetch_update() + update_failed_count = 0 except subprocess.CalledProcessError as e: cloudlog.event( "update process failed", @@ -409,22 +455,21 @@ def main() -> None: returncode=e.returncode ) exception = f"command failed: {e.cmd}\n{e.output}" - overlay_init.unlink(missing_ok=True) + OVERLAY_INIT.unlink(missing_ok=True) except Exception as e: cloudlog.exception("uncaught updated exception, shouldn't happen") exception = str(e) - overlay_init.unlink(missing_ok=True) + OVERLAY_INIT.unlink(missing_ok=True) - if not wait_helper.shutdown: - try: - set_params(new_version, update_failed_count, exception) - except Exception: - cloudlog.exception("uncaught updated exception while setting params, shouldn't happen") + try: + params.put("UpdaterState", "idle") + updater.set_params(update_failed_count, exception) + except Exception: + cloudlog.exception("uncaught updated exception while setting params, shouldn't happen") # infrequent attempts if we successfully updated recently - wait_helper.sleep(5*60 if update_failed_count > 0 else 90*60) - - dismount_overlay() + wait_helper.only_check_for_update = False + wait_helper.sleep(5*60 if update_failed_count > 0 else 1.5*60*60) if __name__ == "__main__": diff --git a/system/camerad/SConscript b/system/camerad/SConscript index 25e366210..ddc763b53 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -1,21 +1,18 @@ -Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM') +Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') -libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon] +libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] -cameras = [] -if arch == "larch64": - libs += ['atomic'] - cameras = ['cameras/camera_qcom2.cc'] +cenv = env.Clone() +cenv['CPPPATH'].append('include/') - env.Program('camerad', [ - 'main.cc', - 'cameras/camera_common.cc', - 'imgproc/utils.cc', - cameras, - ], LIBS=libs) +camera_obj = cenv.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc']) +cenv.Program('camerad', [ + 'main.cc', + camera_obj, + ], LIBS=libs) -if GetOption("test"): - env.Program('test/ae_gray_test', [ +if GetOption("test") and arch == "x86_64": + cenv.Program('test/ae_gray_test', [ 'test/ae_gray_test.cc', - 'cameras/camera_common.cc', + camera_obj, ], LIBS=libs) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 1d4ecd526..30e2810ec 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -18,11 +18,9 @@ #include "system/hardware/hw.h" #include "msm_media_info.h" +#include "system/camerad/cameras/camera_qcom2.h" #ifdef QCOM2 #include "CL/cl_ext_qcom.h" -#include "system/camerad/cameras/camera_qcom2.h" -#else -#include "system/camerad/test/camera_test.h" #endif ExitHandler do_exit; @@ -36,10 +34,10 @@ public: "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d " - "-DCAM_NUM=%d%s", + "-DIS_OX=%d -DCAM_NUM=%d%s", ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset, - s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); + s->camera_id==CAMERA_ID_OX03C10 ? 1 : 0, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); const char *cl_file = "cameras/real_debayer.cl"; cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); @@ -67,11 +65,9 @@ private: void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType init_yuv_type) { vipc_server = v; this->yuv_type = init_yuv_type; - - const CameraInfo *ci = &s->ci; - camera_state = s; frame_buf_count = frame_cnt; + const CameraInfo *ci = &s->ci; // RAW frame const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; camera_bufs = std::make_unique(frame_buf_count); @@ -120,30 +116,24 @@ bool CameraBuf::acquire() { if (camera_bufs_metadata[cur_buf_idx].frame_id == -1) { LOGE("no frame data? wtf"); - release(); return false; } cur_frame_data = camera_bufs_metadata[cur_buf_idx]; cur_yuv_buf = vipc_server->get_buffer(yuv_type); - cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl; - cl_event event; - - double start_time = millis_since_boot(); - cur_camera_buf = &camera_bufs[cur_buf_idx]; - debayer->queue(q, camrabuf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event); - + double start_time = millis_since_boot(); + cl_event event; + debayer->queue(q, camera_bufs[cur_buf_idx].buf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event); clWaitForEvents(1, &event); CL_CHECK(clReleaseEvent(event)); - cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0; VisionIpcBufExtra extra = { - cur_frame_data.frame_id, - cur_frame_data.timestamp_sof, - cur_frame_data.timestamp_eof, + cur_frame_data.frame_id, + cur_frame_data.timestamp_sof, + cur_frame_data.timestamp_eof, }; cur_yuv_buf->set_frame_id(cur_frame_data.frame_id); vipc_server->send(cur_yuv_buf, &extra); @@ -151,17 +141,13 @@ bool CameraBuf::acquire() { return true; } -void CameraBuf::release() { - // Empty -} - void CameraBuf::queue(size_t buf_idx) { safe_queue.push(buf_idx); } // common functions -void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data) { +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c) { framed.setFrameId(frame_data.frame_id); framed.setTimestampEof(frame_data.timestamp_eof); framed.setTimestampSof(frame_data.timestamp_sof); @@ -175,6 +161,16 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr 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]; + const float perc = util::map_val(ev, c->min_ev, c->max_ev, 0.0f, 100.0f); + framed.setExposureValPercent(perc); + + if (c->camera_id == CAMERA_ID_AR0231) { + framed.setSensor(cereal::FrameData::ImageSensor::AR0321); + } else if (c->camera_id == CAMERA_ID_OX03C10) { + framed.setSensor(cereal::FrameData::ImageSensor::OX03C10); + } } kj::Array get_raw_frame_image(const CameraBuf *b) { @@ -326,7 +322,6 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre // this takes 10ms??? publish_thumbnail(cameras->pm, &(cs->buf)); } - cs->buf.release(); ++cnt; } return NULL; @@ -345,15 +340,17 @@ void camerad_thread() { cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); #endif - MultiCameraState cameras = {}; - VisionIpcServer vipc_server("camerad", device_id, context); + { + MultiCameraState cameras = {}; + VisionIpcServer vipc_server("camerad", device_id, context); - cameras_init(&vipc_server, &cameras, device_id, context); - cameras_open(&cameras); + cameras_open(&cameras); + cameras_init(&vipc_server, &cameras, device_id, context); - vipc_server.start_listener(); + vipc_server.start_listener(); - cameras_run(&cameras); + cameras_run(&cameras); + } CL_CHECK(clReleaseContext(context)); } diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 2a5605255..088b9f793 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -23,7 +23,7 @@ #define CAMERA_ID_LGC920 6 #define CAMERA_ID_LGC615 7 #define CAMERA_ID_AR0231 8 -#define CAMERA_ID_IMX390 9 +#define CAMERA_ID_OX03C10 9 #define CAMERA_ID_MAX 10 const int YUV_BUFFER_COUNT = 40; @@ -40,6 +40,7 @@ const bool env_disable_wide_road = getenv("DISABLE_WIDE_ROAD") != NULL; const bool env_disable_driver = getenv("DISABLE_DRIVER") != NULL; const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL; const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL; +const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != NULL; typedef struct CameraInfo { uint32_t frame_width, frame_height; @@ -74,27 +75,21 @@ typedef struct FrameMetadata { } FrameMetadata; struct MultiCameraState; -struct CameraState; +class CameraState; class Debayer; class CameraBuf { private: VisionIpcServer *vipc_server; - CameraState *camera_state; Debayer *debayer = nullptr; - VisionStreamType yuv_type; - int cur_buf_idx; - SafeQueue safe_queue; - int frame_buf_count; public: cl_command_queue q; FrameMetadata cur_frame_data; - VisionBuf *cur_rgb_buf; VisionBuf *cur_yuv_buf; VisionBuf *cur_camera_buf; std::unique_ptr camera_bufs; @@ -107,13 +102,12 @@ public: ~CameraBuf(); void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType yuv_type); bool acquire(); - void release(); void queue(size_t buf_idx); }; typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); -void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data); +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c); kj::Array get_raw_frame_image(const CameraBuf *b); float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); @@ -122,7 +116,6 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i void cameras_open(MultiCameraState *s); void cameras_run(MultiCameraState *s); void cameras_close(MultiCameraState *s); -void camera_autoexposure(CameraState *s, float grey_frac); void camerad_thread(); int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 31af284d5..2ee06e372 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -32,7 +32,8 @@ const size_t FRAME_HEIGHT = 1208; const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) const size_t AR0231_REGISTERS_HEIGHT = 2; -const size_t AR0231_STATS_HEIGHT = 2; +// TODO: this extra height is universal and doesn't apply per camera +const size_t AR0231_STATS_HEIGHT = 2+8; const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py @@ -47,153 +48,66 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .frame_offset = AR0231_REGISTERS_HEIGHT, .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, }, - [CAMERA_ID_IMX390] = { + [CAMERA_ID_OX03C10] = { .frame_width = FRAME_WIDTH, .frame_height = FRAME_HEIGHT, - .frame_stride = FRAME_STRIDE, + .frame_stride = FRAME_STRIDE, // (0xa80*12//8) + .extra_height = 16, // top 2 + bot 14 + .frame_offset = 2, }, }; -const float DC_GAIN = 2.5; -const float sensor_analog_gains[] = { +const float DC_GAIN_AR0231 = 2.5; +const float DC_GAIN_OX03C10 = 7.32; + +const float DC_GAIN_ON_GREY_AR0231= 0.2; +const float DC_GAIN_OFF_GREY_AR0231 = 0.3; +const float DC_GAIN_ON_GREY_OX03C10= 0.25; +const float DC_GAIN_OFF_GREY_OX03C10 = 0.35; + +const int DC_GAIN_MIN_WEIGHT_AR0231 = 0; +const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; +const int DC_GAIN_MIN_WEIGHT_OX03C10 = 16; +const int DC_GAIN_MAX_WEIGHT_OX03C10 = 32; + +const float TARGET_GREY_FACTOR_AR0231 = 1.0; +const float TARGET_GREY_FACTOR_OX03C10 = 0.02; + +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 3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0, // 4, 5, 6, 7 5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0, // 8, 9, 10, 11 7.0/2.0, 8.0/2.0, 8.0/1.0}; // 12, 13, 14, 15 = bypass -const int ANALOG_GAIN_MIN_IDX = 0x1; // 0.25x -const int ANALOG_GAIN_REC_IDX = 0x6; // 0.8x -const int ANALOG_GAIN_MAX_IDX = 0xD; // 4.0x +const float sensor_analog_gains_OX03C10[] = { + 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, + 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, + 3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, + 5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, + 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; -const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss -const int EXPOSURE_TIME_MAX = 0x0855; // with HDR, slowest ss, 40ms +const uint32_t ox03c10_analog_gains_reg[] = { + 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, 0x190, 0x1B0, + 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300, + 0x320, 0x360, 0x3A0, 0x3E0, 0x400, 0x440, 0x480, 0x4C0, 0x500, 0x540, 0x580, + 0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00, + 0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80}; -// ************** low level camera helpers **************** -int do_cam_control(int fd, int op_code, void *handle, int size) { - struct cam_control camcontrol = {0}; - camcontrol.op_code = op_code; - camcontrol.handle = (uint64_t)handle; - if (size == 0) { - camcontrol.size = 8; - camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; - } else { - camcontrol.size = size; - camcontrol.handle_type = CAM_HANDLE_USER_POINTER; - } +const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x +const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x +const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x - int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); - if (ret == -1) { - LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); - } - return ret; -} +const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; +const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x11; // 2.5x +const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; -std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1) { - struct cam_acquire_dev_cmd cmd = { - .session_handle = session_handle, - .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? num_resources : 0), - .resource_hdl = (uint64_t)data, - }; - int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); - return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; -}; +const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss +const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms -int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { - struct cam_config_dev_cmd cmd = { - .session_handle = session_handle, - .dev_handle = dev_handle, - .packet_handle = packet_handle, - }; - return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); -} - -int device_control(int fd, int op_code, int session_handle, int dev_handle) { - // start stop and release are all the same - struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; - return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); -} - -void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, - int mmu_hdl = 0, int mmu_hdl2 = 0) { - struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; - mem_mgr_alloc_cmd.len = len; - mem_mgr_alloc_cmd.align = align; - mem_mgr_alloc_cmd.flags = flags; - mem_mgr_alloc_cmd.num_hdl = 0; - if (mmu_hdl != 0) { - mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; - mem_mgr_alloc_cmd.num_hdl++; - } - if (mmu_hdl2 != 0) { - mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; - mem_mgr_alloc_cmd.num_hdl++; - } - - do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); - *handle = mem_mgr_alloc_cmd.out.buf_handle; - - void *ptr = NULL; - if (mem_mgr_alloc_cmd.out.fd > 0) { - ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); - assert(ptr != MAP_FAILED); - } - - // LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); - - return ptr; -} - -void release(int video0_fd, uint32_t handle) { - int ret; - struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; - mem_mgr_release_cmd.buf_handle = handle; - - ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); - assert(ret == 0); -} - -void release_fd(int video0_fd, uint32_t handle) { - // handle to fd - close(handle>>16); - release(video0_fd, handle); -} - -void *MemoryManager::alloc(int size, uint32_t *handle) { - lock.lock(); - void *ptr; - if (!cached_allocations[size].empty()) { - ptr = cached_allocations[size].front(); - cached_allocations[size].pop(); - *handle = handle_lookup[ptr]; - } else { - ptr = alloc_w_mmu_hdl(video0_fd, size, handle); - handle_lookup[ptr] = *handle; - size_lookup[ptr] = size; - } - lock.unlock(); - return ptr; -} - -void MemoryManager::free(void *ptr) { - lock.lock(); - cached_allocations[size_lookup[ptr]].push(ptr); - lock.unlock(); -} - -MemoryManager::~MemoryManager() { - for (auto& x : cached_allocations) { - while (!x.second.empty()) { - void *ptr = x.second.front(); - x.second.pop(); - LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); - munmap(ptr, size_lookup[ptr]); - release_fd(video0_fd, handle_lookup[ptr]); - handle_lookup.erase(ptr); - size_lookup.erase(ptr); - } - } -} +const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x +const int EXPOSURE_TIME_MAX_OX03C10 = 2016; +const uint32_t VS_TIME_MIN_OX03C10 = 1; +const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 int CameraState::clear_req_queue() { struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; @@ -213,8 +127,8 @@ void CameraState::sensors_start() { LOGD("starting sensor %d", camera_num); if (camera_id == CAMERA_ID_AR0231) { sensors_i2c(start_reg_array_ar0231, std::size(start_reg_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_IMX390) { - sensors_i2c(start_reg_array_imx390, std::size(start_reg_array_imx390), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + } else if (camera_id == CAMERA_ID_OX03C10) { + sensors_i2c(start_reg_array_ox03c10, std::size(start_reg_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); } else { assert(false); } @@ -300,15 +214,15 @@ int CameraState::sensors_init() { switch (camera_num) { case 0: // port 0 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; // 6C = 0x36*2 break; case 1: // port 1 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x36; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x20; break; case 2: // port 2 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; break; } @@ -324,9 +238,9 @@ int CameraState::sensors_init() { if (camera_id == CAMERA_ID_AR0231) { probe->reg_addr = 0x3000; probe->expected_data = 0x354; - } else if (camera_id == CAMERA_ID_IMX390) { - probe->reg_addr = 0x330; - probe->expected_data = 0x1538; + } else if (camera_id == CAMERA_ID_OX03C10) { + probe->reg_addr = 0x300a; + probe->expected_data = 0x5803; } else { assert(false); } @@ -393,8 +307,8 @@ int CameraState::sensors_init() { power->power_settings[1].power_seq_type = 1; power->power_settings[2].power_seq_type = 3; - LOGD("probing the sensor"); int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); + LOGD("probing the sensor: %d", ret); mm.free(i2c_info); mm.free(power_settings); @@ -606,12 +520,71 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id_, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type, bool enabled_) { - multi_cam_state = multi_cam_state_; - camera_id = camera_id_; - camera_num = camera_num_; - enabled = enabled_; +void CameraState::camera_set_parameters() { + if (camera_id == CAMERA_ID_AR0231) { + dc_gain_factor = DC_GAIN_AR0231; + dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231; + dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; + dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231; + dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231; + exposure_time_min = EXPOSURE_TIME_MIN_AR0231; + exposure_time_max = EXPOSURE_TIME_MAX_AR0231; + analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231; + analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231; + analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231; + for (int i=0; i<=analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; + } + min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; + target_grey_factor = TARGET_GREY_FACTOR_AR0231; + } else if (camera_id == CAMERA_ID_OX03C10) { + dc_gain_factor = DC_GAIN_OX03C10; + dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10; + dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; + dc_gain_on_grey = DC_GAIN_ON_GREY_OX03C10; + dc_gain_off_grey = DC_GAIN_OFF_GREY_OX03C10; + exposure_time_min = EXPOSURE_TIME_MIN_OX03C10; + exposure_time_max = EXPOSURE_TIME_MAX_OX03C10; + analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10; + analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10; + analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10; + for (int i=0; i<=analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; + } + min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; + target_grey_factor = TARGET_GREY_FACTOR_OX03C10; + } else { + assert(false); + } + + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_fraction = 0.3; + + dc_gain_enabled = false; + dc_gain_weight = dc_gain_min_weight; + gain_idx = analog_gain_rec_idx; + exposure_time = 5; + cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight) * sensor_analog_gains[gain_idx] * exposure_time; +} + +void CameraState::camera_map_bufs(MultiCameraState *s) { + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + // configure ISP to put the image in place + struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; + mem_mgr_map_cmd.mmu_hdls[0] = s->device_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; + mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; + int ret = do_cam_control(s->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + } + enqueue_req_multi(1, FRAME_BUF_COUNT, 0); +} + +void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int camera_id_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { if (!enabled) return; + camera_id = camera_id_; LOGD("camera init %d", camera_num); assert(camera_id < std::size(cameras_supported)); @@ -621,19 +594,18 @@ void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServe request_id_last = 0; skipped = true; - min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX]; - max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN; - target_grey_fraction = 0.3; - - dc_gain_enabled = false; - gain_idx = ANALOG_GAIN_REC_IDX; - exposure_time = 5; - cur_ev[0] = cur_ev[1] = cur_ev[2] = (dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[gain_idx] * exposure_time; + camera_set_parameters(); buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, yuv_type); + camera_map_bufs(s); } -void CameraState::camera_open() { +void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num_, bool enabled_) { + multi_cam_state = multi_cam_state_; + camera_num = camera_num_; + enabled = enabled_; + if (!enabled) return; + int ret; sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num); assert(sensor_fd >= 0); @@ -644,10 +616,12 @@ void CameraState::camera_open() { // probe the sensor LOGD("-- Probing sensor %d", camera_num); + camera_id = CAMERA_ID_AR0231; ret = sensors_init(); if (ret != 0) { - LOGD("AR0231 init failed, trying IMX390"); - camera_id = CAMERA_ID_IMX390; + // TODO: use build flag instead? + LOGD("AR0231 init failed, trying OX03C10"); + camera_id = CAMERA_ID_OX03C10; ret = sensors_init(); } LOGD("-- Probing sensor %d done with %d", camera_num, ret); @@ -671,13 +645,18 @@ void CameraState::camera_open() { LOGD("acquire sensor dev"); LOG("-- Configuring sensor"); + uint32_t dt; if (camera_id == CAMERA_ID_AR0231) { sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_IMX390) { - sensors_i2c(init_array_imx390, std::size(init_array_imx390), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + } else if (camera_id == CAMERA_ID_OX03C10) { + sensors_i2c(init_array_ox03c10, std::size(init_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + // one is 0x2a, two are 0x2b + dt = 0x2c; } else { assert(false); } + printf("dt is %x\n", dt); // NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c // If you don't do this, the strobe GPIO is an output (even in reset it seems!) @@ -691,7 +670,7 @@ void CameraState::camera_open() { .lane_cfg = 0x3210, .vc = 0x0, - .dt = 0x12, // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + .dt = dt, .format = CAM_FORMAT_MIPI_RAW_12, .test_pattern = 0x2, // 0x3? @@ -805,29 +784,15 @@ void CameraState::camera_open() { ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); LOGD("start isp: %d", ret); - for (int i = 0; i < FRAME_BUF_COUNT; i++) { - // configure ISP to put the image in place - struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; - mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; - mem_mgr_map_cmd.num_hdl = 1; - mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; - mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; - ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); - LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); - buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; - } - // TODO: this is unneeded, should we be doing the start i2c in a different way? //ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle); //LOGD("start sensor: %d", ret); - - enqueue_req_multi(1, FRAME_BUF_COUNT, 0); } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_DRIVER, !env_disable_driver); - s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_ROAD, !env_disable_road); - s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD, !env_disable_wide_road); + s->driver_cam.camera_init(s, v, s->driver_cam.camera_id, 20, device_id, ctx, VISION_STREAM_DRIVER); + s->road_cam.camera_init(s, v, s->road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_ROAD); + s->wide_road_cam.camera_init(s, v, s->wide_road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } @@ -853,8 +818,8 @@ void cameras_open(MultiCameraState *s) { // query icp for MMU handles LOG("-- Query ICP for MMU handles"); - static struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0}; - static struct cam_query_cap_cmd query_cap_cmd = {0}; + struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0}; + struct cam_query_cap_cmd query_cap_cmd = {0}; query_cap_cmd.handle_type = 1; query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd; query_cap_cmd.size = sizeof(isp_query_cap_cmd); @@ -867,17 +832,17 @@ void cameras_open(MultiCameraState *s) { // subscribe LOG("-- Subscribing"); - static struct v4l2_event_subscription sub = {0}; + struct v4l2_event_subscription sub = {0}; sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT; sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS; ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); LOGD("req mgr subscribe: %d", ret); - s->driver_cam.camera_open(); + s->driver_cam.camera_open(s, 2, !env_disable_driver); LOGD("driver camera opened"); - s->road_cam.camera_open(); + s->road_cam.camera_open(s, 1, !env_disable_road); LOGD("road camera opened"); - s->wide_road_cam.camera_open(); + s->wide_road_cam.camera_open(s, 0, !env_disable_wide_road); LOGD("wide road camera opened"); } @@ -896,7 +861,7 @@ void CameraState::camera_close() { LOGD("stop csiphy: %d", ret); // link control stop LOG("-- Stop link control"); - static struct cam_req_mgr_link_control req_mgr_link_control = {0}; + struct cam_req_mgr_link_control req_mgr_link_control = {0}; req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE; req_mgr_link_control.session_hdl = session_handle; req_mgr_link_control.num_links = 1; @@ -906,7 +871,7 @@ void CameraState::camera_close() { // unlink LOG("-- Unlink"); - static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; + struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; req_mgr_unlink_info.session_hdl = session_handle; req_mgr_unlink_info.link_hdl = link_handle; ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); @@ -949,7 +914,7 @@ std::map> CameraState::ar0231_build_register_lut(u // // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional - // for contigous ranges. See page 27-29 of the AR0231 Developer guide for more information. + // for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information. int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; auto get_next_idx = [](int cur_idx) { @@ -1044,7 +1009,7 @@ void CameraState::handle_camera_event(void *evdat) { meta_data.frame_id = main_id - idx_offset; meta_data.timestamp_sof = timestamp; exp_lock.lock(); - meta_data.gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac; + meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); meta_data.high_conversion_gain = dc_gain_enabled; meta_data.integ_lines = exposure_time; meta_data.measured_grey_fraction = measured_grey_fraction; @@ -1077,12 +1042,12 @@ void CameraState::set_camera_exposure(float grey_frac) { // It takes 3 frames for the commanded exposure settings to take effect. The first frame is already started by the time // we reach this function, the other 2 are due to the register buffering in the sensor. // Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action. - // TODO: Lower latency to 2 frames, by using the histogram outputed by the sensor we can do AE before the debayering is complete + // TODO: Lower latency to 2 frames, by using the histogram outputted by the sensor we can do AE before the debayering is complete const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3]; // Scale target grey between 0.1 and 0.4 depending on lighting conditions - float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev_) / log2(6000.0), 0.1, 0.4); + float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey; float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev); @@ -1096,41 +1061,62 @@ void CameraState::set_camera_exposure(float grey_frac) { // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes bool enable_dc_gain = dc_gain_enabled; - if (!enable_dc_gain && target_grey < 0.2) { + if (!enable_dc_gain && target_grey < dc_gain_on_grey) { enable_dc_gain = true; - } else if (enable_dc_gain && target_grey > 0.3) { + dc_gain_weight = dc_gain_min_weight; + } else if (enable_dc_gain && target_grey > dc_gain_off_grey) { enable_dc_gain = false; + dc_gain_weight = dc_gain_max_weight; } - // Simple brute force optimizer to choose sensor parameters - // to reach desired EV - for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, gain_idx + 1); g++) { - float gain = sensor_analog_gains[g] * (enable_dc_gain ? DC_GAIN : 1); + if (enable_dc_gain && dc_gain_weight < dc_gain_max_weight) {dc_gain_weight += 1;} + if (!enable_dc_gain && dc_gain_weight > dc_gain_min_weight) {dc_gain_weight -= 1;} - // Compute optimal time for given gain - int t = std::clamp(int(std::round(desired_ev / gain)), EXPOSURE_TIME_MIN, EXPOSURE_TIME_MAX); + std::string gain_bytes, time_bytes; + if (env_ctrl_exp_from_params) { + gain_bytes = Params().get("CameraDebugExpGain"); + time_bytes = Params().get("CameraDebugExpTime"); + } - // Only go below recomended gain when absolutely necessary to not overexpose - if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < gain_idx) { - continue; - } + if (gain_bytes.size() > 0 && time_bytes.size() > 0) { + // Override gain and exposure time + gain_idx = std::stoi(gain_bytes); + exposure_time = std::stoi(time_bytes); - // Compute error to desired ev - float score = std::abs(desired_ev - (t * gain)) * 10; + new_g = gain_idx; + new_t = exposure_time; + enable_dc_gain = false; + } else { + // Simple brute force optimizer to choose sensor parameters + // to reach desired EV + for (int g = std::max((int)analog_gain_min_idx, gain_idx - 1); g <= std::min((int)analog_gain_max_idx, gain_idx + 1); g++) { + float gain = sensor_analog_gains[g] * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); - // Going below recomended gain needs lower penalty to not overexpose - float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1; - score += std::abs(g - (int)ANALOG_GAIN_REC_IDX) * m; + // Compute optimal time for given gain + int t = std::clamp(int(std::round(desired_ev / gain)), exposure_time_min, exposure_time_max); - // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev); + // Only go below recommended gain when absolutely necessary to not overexpose + if (g < analog_gain_rec_idx && t > 20 && g < gain_idx) { + continue; + } - // Small penalty on changing gain - score += std::abs(g - gain_idx) * (score + 1.0) / 10.0; + // Compute error to desired ev + float score = std::abs(desired_ev - (t * gain)) * 10; - if (score < best_ev_score) { - new_t = t; - new_g = g; - best_ev_score = score; + // Going below recommended gain needs lower penalty to not overexpose + float m = g > analog_gain_rec_idx ? 5.0 : 0.1; + score += std::abs(g - (int)analog_gain_rec_idx) * m; + + // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev); + + // Small penalty on changing gain + score += std::abs(g - gain_idx) * (score + 1.0) / 10.0; + + if (score < best_ev_score) { + new_t = t; + new_g = g; + best_ev_score = score; + } } } @@ -1144,7 +1130,7 @@ void CameraState::set_camera_exposure(float grey_frac) { exposure_time = new_t; dc_gain_enabled = enable_dc_gain; - float gain = analog_gain_frac * (dc_gain_enabled ? DC_GAIN : 1.0); + float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain; exp_lock.unlock(); @@ -1160,31 +1146,36 @@ void CameraState::set_camera_exposure(float grey_frac) { if (camera_id == CAMERA_ID_AR0231) { uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g; struct i2c_random_wr_payload exp_reg_array[] = { - {0x3366, analog_gain_reg}, - {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, - {0x3012, (uint16_t)exposure_time}, - }; + {0x3366, analog_gain_reg}, + {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, + {0x3012, (uint16_t)exposure_time}, + }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_IMX390) { - // if gain is sub 1, we have to use exposure to mimic sub 1 gains - uint32_t real_exposure_time = (gain < 1.0) ? (exposure_time*gain) : exposure_time; - // invert real_exposure_time, max exposure is 2 - real_exposure_time = (exposure_time >= 0x7cf) ? 2 : (0x7cf - exposure_time); - uint32_t real_gain = int((10*log10(fmax(1.0, gain)))/0.3); - //printf("%d expose: %d gain: %f = %d\n", camera_num, exposure_time, gain, real_gain); + } else if (camera_id == CAMERA_ID_OX03C10) { + // t_HCG + t_LCG + t_VS on LPD, t_SPD on SPD + uint32_t hcg_time = std::max((dc_gain_weight * exposure_time / dc_gain_max_weight), 0); + uint32_t lcg_time = std::max(((dc_gain_max_weight - dc_gain_weight) * exposure_time / dc_gain_max_weight), 0); + // uint32_t spd_time = std::max(hcg_time / 16, (uint32_t)exposure_time_min); + uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 128, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); + uint32_t spd_time = vs_time; + + uint32_t real_gain = ox03c10_analog_gains_reg[new_g]; + uint32_t min_gain = ox03c10_analog_gains_reg[0]; struct i2c_random_wr_payload exp_reg_array[] = { - {0x000c, real_exposure_time&0xFF}, {0x000d, real_exposure_time>>8}, - {0x0010, real_exposure_time&0xFF}, {0x0011, real_exposure_time>>8}, - {0x0018, real_gain&0xFF}, {0x0019, real_gain>>8}, + {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}, + + {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, + {0x3588, min_gain>>8}, {0x3589, min_gain&0xFF}, + {0x3548, min_gain>>8}, {0x3549, min_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); } } -void camera_autoexposure(CameraState *s, float grey_frac) { - s->set_camera_exposure(grey_frac); -} - static float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { // See AR0231 Developer Guide - page 36 float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); @@ -1211,20 +1202,13 @@ static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal framed.setTemperaturesC({temp_0, temp_1}); } -static void driver_cam_auto_exposure(CameraState *c) { - struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; - const CameraBuf *b = &c->buf; - static ExpRect rect = {96, 1832, 2, 242, 1148, 4}; - camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip)); -} - static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { - driver_cam_auto_exposure(c); + c->set_camera_exposure(set_exposure_target(&c->buf, 96, 1832, 2, 242, 1148, 4)); MessageBuilder msg; auto framed = msg.initEvent().initDriverCameraState(); framed.setFrameType(cereal::FrameData::FrameType::FRONT); - fill_frame_data(framed, c->buf.cur_frame_data); + fill_frame_data(framed, c->buf.cur_frame_data, c); if (c->camera_id == CAMERA_ID_AR0231) { ar0231_process_registers(s, c, framed); @@ -1237,7 +1221,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { MessageBuilder msg; auto framed = c == &s->road_cam ? msg.initEvent().initRoadCameraState() : msg.initEvent().initWideRoadCameraState(); - fill_frame_data(framed, b->cur_frame_data); + fill_frame_data(framed, b->cur_frame_data, c); if (env_log_raw_frames && c == &s->road_cam && cnt % 100 == 5) { // no overlap with qlog decimation framed.setImage(get_raw_frame_image(b)); } @@ -1255,7 +1239,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); const int skip = 2; - camera_autoexposure(c, set_exposure_target(b, x, x + w, skip, y, y + h, skip)); + c->set_camera_exposure(set_exposure_target(b, x, x + w, skip, y, y + h, skip)); } void cameras_run(MultiCameraState *s) { @@ -1320,4 +1304,3 @@ void cameras_run(MultiCameraState *s) { cameras_close(s); } - diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 03d3d1a82..1e25e605c 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -7,23 +7,12 @@ #include #include "system/camerad/cameras/camera_common.h" +#include "system/camerad/cameras/camera_util.h" +#include "common/params.h" #include "common/util.h" #define FRAME_BUF_COUNT 4 - -class MemoryManager { - public: - void init(int _video0_fd) { video0_fd = _video0_fd; } - void *alloc(int len, uint32_t *handle); - void free(void *ptr); - ~MemoryManager(); - private: - std::mutex lock; - std::map handle_lookup; - std::map size_lookup; - std::map > cached_allocations; - int video0_fd; -}; +#define ANALOG_GAIN_MAX_CNT 55 class CameraState { public: @@ -35,14 +24,30 @@ public: int exposure_time; bool dc_gain_enabled; + int dc_gain_weight; + int gain_idx; float analog_gain_frac; + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_min_weight; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + float cur_ev[3]; float min_ev, max_ev; float measured_grey_fraction; float target_grey_fraction; - int gain_idx; + float target_grey_factor; unique_fd sensor_fd; unique_fd csiphy_fd; @@ -54,8 +59,10 @@ public: void sensors_start(); - void camera_open(); - void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type, bool enabled); + void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); + void camera_set_parameters(); + void camera_map_bufs(MultiCameraState *s); + void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); void camera_close(); std::map ar0231_parse_registers(uint8_t *data, std::initializer_list addrs); diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc new file mode 100644 index 000000000..6d139590e --- /dev/null +++ b/system/camerad/cameras/camera_util.cc @@ -0,0 +1,135 @@ +#include "system/camerad/cameras/camera_util.h" + +#include + +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +// ************** low level camera helpers **************** +int do_cam_control(int fd, int op_code, void *handle, int size) { + struct cam_control camcontrol = {0}; + camcontrol.op_code = op_code; + camcontrol.handle = (uint64_t)handle; + if (size == 0) { + camcontrol.size = 8; + camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; + } else { + camcontrol.size = size; + camcontrol.handle_type = CAM_HANDLE_USER_POINTER; + } + + int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); + if (ret == -1) { + LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); + } + return ret; +} + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { + struct cam_acquire_dev_cmd cmd = { + .session_handle = session_handle, + .handle_type = CAM_HANDLE_USER_POINTER, + .num_resources = (uint32_t)(data ? num_resources : 0), + .resource_hdl = (uint64_t)data, + }; + int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); + return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; +}; + +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { + struct cam_config_dev_cmd cmd = { + .session_handle = session_handle, + .dev_handle = dev_handle, + .packet_handle = packet_handle, + }; + return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); +} + +int device_control(int fd, int op_code, int session_handle, int dev_handle) { + // start stop and release are all the same + struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; + return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); +} + +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) { + struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; + mem_mgr_alloc_cmd.len = len; + mem_mgr_alloc_cmd.align = align; + mem_mgr_alloc_cmd.flags = flags; + mem_mgr_alloc_cmd.num_hdl = 0; + if (mmu_hdl != 0) { + mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; + mem_mgr_alloc_cmd.num_hdl++; + } + if (mmu_hdl2 != 0) { + mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; + mem_mgr_alloc_cmd.num_hdl++; + } + + do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); + *handle = mem_mgr_alloc_cmd.out.buf_handle; + + void *ptr = NULL; + if (mem_mgr_alloc_cmd.out.fd > 0) { + ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); + assert(ptr != MAP_FAILED); + } + + // LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); + + return ptr; +} + +void release(int video0_fd, uint32_t handle) { + int ret; + struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; + mem_mgr_release_cmd.buf_handle = handle; + + ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); + assert(ret == 0); +} + +void release_fd(int video0_fd, uint32_t handle) { + // handle to fd + close(handle>>16); + release(video0_fd, handle); +} + +void *MemoryManager::alloc(int size, uint32_t *handle) { + lock.lock(); + void *ptr; + if (!cached_allocations[size].empty()) { + ptr = cached_allocations[size].front(); + cached_allocations[size].pop(); + *handle = handle_lookup[ptr]; + } else { + ptr = alloc_w_mmu_hdl(video0_fd, size, handle); + handle_lookup[ptr] = *handle; + size_lookup[ptr] = size; + } + lock.unlock(); + return ptr; +} + +void MemoryManager::free(void *ptr) { + lock.lock(); + cached_allocations[size_lookup[ptr]].push(ptr); + lock.unlock(); +} + +MemoryManager::~MemoryManager() { + for (auto& x : cached_allocations) { + while (!x.second.empty()) { + void *ptr = x.second.front(); + x.second.pop(); + LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); + munmap(ptr, size_lookup[ptr]); + release_fd(video0_fd, handle_lookup[ptr]); + handle_lookup.erase(ptr); + size_lookup.erase(ptr); + } + } +} diff --git a/system/camerad/cameras/camera_util.h b/system/camerad/cameras/camera_util.h new file mode 100644 index 000000000..e408f6c0e --- /dev/null +++ b/system/camerad/cameras/camera_util.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include + +#include + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1); +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle); +int device_control(int fd, int op_code, int session_handle, int dev_handle); +int do_cam_control(int fd, int op_code, void *handle, int size); +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, + int mmu_hdl = 0, int mmu_hdl2 = 0); +void release(int video0_fd, uint32_t handle); + +class MemoryManager { + public: + void init(int _video0_fd) { video0_fd = _video0_fd; } + void *alloc(int len, uint32_t *handle); + void free(void *ptr); + ~MemoryManager(); + private: + std::mutex lock; + std::map handle_lookup; + std::map size_lookup; + std::map > cached_allocations; + int video0_fd; +}; diff --git a/system/camerad/cameras/real_debayer.cl b/system/camerad/cameras/real_debayer.cl index fc9f41049..cff5ae455 100644 --- a/system/camerad/cameras/real_debayer.cl +++ b/system/camerad/cameras/real_debayer.cl @@ -8,9 +8,15 @@ float3 color_correct(float3 rgb) { // color correction + #if IS_OX + float3 x = rgb.x * (float3)(1.5664815 , -0.29808738, -0.03973474); + x += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248); + x += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722); + #else float3 x = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673); x += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455); x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782); + #endif // tone mapping params const float gamma_k = 0.75; @@ -36,14 +42,43 @@ float get_vignetting_s(float r) { } } +constant float ox03c10_lut[] = { + 0.0000e+00, 5.9488e-08, 1.1898e-07, 1.7846e-07, 2.3795e-07, 2.9744e-07, 3.5693e-07, 4.1642e-07, 4.7591e-07, 5.3539e-07, 5.9488e-07, 6.5437e-07, 7.1386e-07, 7.7335e-07, 8.3284e-07, 8.9232e-07, 9.5181e-07, 1.0113e-06, 1.0708e-06, 1.1303e-06, 1.1898e-06, 1.2493e-06, 1.3087e-06, 1.3682e-06, 1.4277e-06, 1.4872e-06, 1.5467e-06, 1.6062e-06, 1.6657e-06, 1.7252e-06, 1.7846e-06, 1.8441e-06, 1.9036e-06, 1.9631e-06, 2.0226e-06, 2.0821e-06, 2.1416e-06, 2.2011e-06, 2.2606e-06, 2.3200e-06, 2.3795e-06, 2.4390e-06, 2.4985e-06, 2.5580e-06, 2.6175e-06, 2.6770e-06, 2.7365e-06, 2.7959e-06, 2.8554e-06, 2.9149e-06, 2.9744e-06, 3.0339e-06, 3.0934e-06, 3.1529e-06, 3.2124e-06, 3.2719e-06, 3.3313e-06, 3.3908e-06, 3.4503e-06, 3.5098e-06, 3.5693e-06, 3.6288e-06, 3.6883e-06, 3.7478e-06, 3.8072e-06, 3.8667e-06, 3.9262e-06, 3.9857e-06, 4.0452e-06, 4.1047e-06, 4.1642e-06, 4.2237e-06, 4.2832e-06, 4.3426e-06, 4.4021e-06, 4.4616e-06, 4.5211e-06, 4.5806e-06, 4.6401e-06, 4.6996e-06, 4.7591e-06, 4.8185e-06, 4.8780e-06, 4.9375e-06, 4.9970e-06, 5.0565e-06, 5.1160e-06, 5.1755e-06, 5.2350e-06, 5.2945e-06, 5.3539e-06, 5.4134e-06, 5.4729e-06, 5.5324e-06, 5.5919e-06, 5.6514e-06, 5.7109e-06, 5.7704e-06, 5.8298e-06, 5.8893e-06, 5.9488e-06, 6.0083e-06, 6.0678e-06, 6.1273e-06, 6.1868e-06, 6.2463e-06, 6.3058e-06, 6.3652e-06, 6.4247e-06, 6.4842e-06, 6.5437e-06, 6.6032e-06, 6.6627e-06, 6.7222e-06, 6.7817e-06, 6.8411e-06, 6.9006e-06, 6.9601e-06, 7.0196e-06, 7.0791e-06, 7.1386e-06, 7.1981e-06, 7.2576e-06, 7.3171e-06, 7.3765e-06, 7.4360e-06, 7.4955e-06, 7.5550e-06, 7.6145e-06, 7.6740e-06, 7.7335e-06, 7.7930e-06, 7.8524e-06, 7.9119e-06, 7.9714e-06, 8.0309e-06, 8.0904e-06, 8.1499e-06, 8.2094e-06, 8.2689e-06, 8.3284e-06, 8.3878e-06, 8.4473e-06, 8.5068e-06, 8.5663e-06, 8.6258e-06, 8.6853e-06, 8.7448e-06, 8.8043e-06, 8.8637e-06, 8.9232e-06, 8.9827e-06, 9.0422e-06, 9.1017e-06, 9.1612e-06, 9.2207e-06, 9.2802e-06, 9.3397e-06, 9.3991e-06, 9.4586e-06, 9.5181e-06, 9.5776e-06, 9.6371e-06, 9.6966e-06, 9.7561e-06, 9.8156e-06, 9.8750e-06, 9.9345e-06, 9.9940e-06, 1.0054e-05, 1.0113e-05, 1.0172e-05, 1.0232e-05, 1.0291e-05, 1.0351e-05, 1.0410e-05, 1.0470e-05, 1.0529e-05, 1.0589e-05, 1.0648e-05, 1.0708e-05, 1.0767e-05, 1.0827e-05, 1.0886e-05, 1.0946e-05, 1.1005e-05, 1.1065e-05, 1.1124e-05, 1.1184e-05, 1.1243e-05, 1.1303e-05, 1.1362e-05, 1.1422e-05, 1.1481e-05, 1.1541e-05, 1.1600e-05, 1.1660e-05, 1.1719e-05, 1.1779e-05, 1.1838e-05, 1.1898e-05, 1.1957e-05, 1.2017e-05, 1.2076e-05, 1.2136e-05, 1.2195e-05, 1.2255e-05, 1.2314e-05, 1.2374e-05, 1.2433e-05, 1.2493e-05, 1.2552e-05, 1.2612e-05, 1.2671e-05, 1.2730e-05, 1.2790e-05, 1.2849e-05, 1.2909e-05, 1.2968e-05, 1.3028e-05, 1.3087e-05, 1.3147e-05, 1.3206e-05, 1.3266e-05, 1.3325e-05, 1.3385e-05, 1.3444e-05, 1.3504e-05, 1.3563e-05, 1.3623e-05, 1.3682e-05, 1.3742e-05, 1.3801e-05, 1.3861e-05, 1.3920e-05, 1.3980e-05, 1.4039e-05, 1.4099e-05, 1.4158e-05, 1.4218e-05, 1.4277e-05, 1.4337e-05, 1.4396e-05, 1.4456e-05, 1.4515e-05, 1.4575e-05, 1.4634e-05, 1.4694e-05, 1.4753e-05, 1.4813e-05, 1.4872e-05, 1.4932e-05, 1.4991e-05, 1.5051e-05, 1.5110e-05, 1.5169e-05, + 1.5229e-05, 1.5288e-05, 1.5348e-05, 1.5407e-05, 1.5467e-05, 1.5526e-05, 1.5586e-05, 1.5645e-05, 1.5705e-05, 1.5764e-05, 1.5824e-05, 1.5883e-05, 1.5943e-05, 1.6002e-05, 1.6062e-05, 1.6121e-05, 1.6181e-05, 1.6240e-05, 1.6300e-05, 1.6359e-05, 1.6419e-05, 1.6478e-05, 1.6538e-05, 1.6597e-05, 1.6657e-05, 1.6716e-05, 1.6776e-05, 1.6835e-05, 1.6895e-05, 1.6954e-05, 1.7014e-05, 1.7073e-05, 1.7133e-05, 1.7192e-05, 1.7252e-05, 1.7311e-05, 1.7371e-05, 1.7430e-05, 1.7490e-05, 1.7549e-05, 1.7609e-05, 1.7668e-05, 1.7727e-05, 1.7787e-05, 1.7846e-05, 1.7906e-05, 1.7965e-05, 1.8025e-05, 1.8084e-05, 1.8144e-05, 1.8203e-05, 1.8263e-05, 1.8322e-05, 1.8382e-05, 1.8441e-05, 1.8501e-05, 1.8560e-05, 1.8620e-05, 1.8679e-05, 1.8739e-05, 1.8798e-05, 1.8858e-05, 1.8917e-05, 1.8977e-05, 1.9036e-05, 1.9096e-05, 1.9155e-05, 1.9215e-05, 1.9274e-05, 1.9334e-05, 1.9393e-05, 1.9453e-05, 1.9512e-05, 1.9572e-05, 1.9631e-05, 1.9691e-05, 1.9750e-05, 1.9810e-05, 1.9869e-05, 1.9929e-05, 1.9988e-05, 2.0048e-05, 2.0107e-05, 2.0167e-05, 2.0226e-05, 2.0285e-05, 2.0345e-05, 2.0404e-05, 2.0464e-05, 2.0523e-05, 2.0583e-05, 2.0642e-05, 2.0702e-05, 2.0761e-05, 2.0821e-05, 2.0880e-05, 2.0940e-05, 2.0999e-05, 2.1059e-05, 2.1118e-05, 2.1178e-05, 2.1237e-05, 2.1297e-05, 2.1356e-05, 2.1416e-05, 2.1475e-05, 2.1535e-05, 2.1594e-05, 2.1654e-05, 2.1713e-05, 2.1773e-05, 2.1832e-05, 2.1892e-05, 2.1951e-05, 2.2011e-05, 2.2070e-05, 2.2130e-05, 2.2189e-05, 2.2249e-05, 2.2308e-05, 2.2368e-05, 2.2427e-05, 2.2487e-05, 2.2546e-05, 2.2606e-05, 2.2665e-05, 2.2725e-05, 2.2784e-05, 2.2843e-05, 2.2903e-05, 2.2962e-05, 2.3022e-05, 2.3081e-05, 2.3141e-05, 2.3200e-05, 2.3260e-05, 2.3319e-05, 2.3379e-05, 2.3438e-05, 2.3498e-05, 2.3557e-05, 2.3617e-05, 2.3676e-05, 2.3736e-05, 2.3795e-05, 2.3855e-05, 2.3914e-05, 2.3974e-05, 2.4033e-05, 2.4093e-05, 2.4152e-05, 2.4212e-05, 2.4271e-05, 2.4331e-05, 2.4390e-05, 2.4450e-05, 2.4509e-05, 2.4569e-05, 2.4628e-05, 2.4688e-05, 2.4747e-05, 2.4807e-05, 2.4866e-05, 2.4926e-05, 2.4985e-05, 2.5045e-05, 2.5104e-05, 2.5164e-05, 2.5223e-05, 2.5282e-05, 2.5342e-05, 2.5401e-05, 2.5461e-05, 2.5520e-05, 2.5580e-05, 2.5639e-05, 2.5699e-05, 2.5758e-05, 2.5818e-05, 2.5877e-05, 2.5937e-05, 2.5996e-05, 2.6056e-05, 2.6115e-05, 2.6175e-05, 2.6234e-05, 2.6294e-05, 2.6353e-05, 2.6413e-05, 2.6472e-05, 2.6532e-05, 2.6591e-05, 2.6651e-05, 2.6710e-05, 2.6770e-05, 2.6829e-05, 2.6889e-05, 2.6948e-05, 2.7008e-05, 2.7067e-05, 2.7127e-05, 2.7186e-05, 2.7246e-05, 2.7305e-05, 2.7365e-05, 2.7424e-05, 2.7484e-05, 2.7543e-05, 2.7603e-05, 2.7662e-05, 2.7722e-05, 2.7781e-05, 2.7840e-05, 2.7900e-05, 2.7959e-05, 2.8019e-05, 2.8078e-05, 2.8138e-05, 2.8197e-05, 2.8257e-05, 2.8316e-05, 2.8376e-05, 2.8435e-05, 2.8495e-05, 2.8554e-05, 2.8614e-05, 2.8673e-05, 2.8733e-05, 2.8792e-05, 2.8852e-05, 2.8911e-05, 2.8971e-05, 2.9030e-05, 2.9090e-05, 2.9149e-05, 2.9209e-05, 2.9268e-05, 2.9328e-05, 2.9387e-05, 2.9447e-05, 2.9506e-05, 2.9566e-05, 2.9625e-05, 2.9685e-05, 2.9744e-05, 2.9804e-05, 2.9863e-05, 2.9923e-05, 2.9982e-05, 3.0042e-05, 3.0101e-05, 3.0161e-05, 3.0220e-05, 3.0280e-05, 3.0339e-05, 3.0398e-05, + 3.0458e-05, 3.0577e-05, 3.0697e-05, 3.0816e-05, 3.0936e-05, 3.1055e-05, 3.1175e-05, 3.1294e-05, 3.1414e-05, 3.1533e-05, 3.1652e-05, 3.1772e-05, 3.1891e-05, 3.2011e-05, 3.2130e-05, 3.2250e-05, 3.2369e-05, 3.2489e-05, 3.2608e-05, 3.2727e-05, 3.2847e-05, 3.2966e-05, 3.3086e-05, 3.3205e-05, 3.3325e-05, 3.3444e-05, 3.3563e-05, 3.3683e-05, 3.3802e-05, 3.3922e-05, 3.4041e-05, 3.4161e-05, 3.4280e-05, 3.4400e-05, 3.4519e-05, 3.4638e-05, 3.4758e-05, 3.4877e-05, 3.4997e-05, 3.5116e-05, 3.5236e-05, 3.5355e-05, 3.5475e-05, 3.5594e-05, 3.5713e-05, 3.5833e-05, 3.5952e-05, 3.6072e-05, 3.6191e-05, 3.6311e-05, 3.6430e-05, 3.6550e-05, 3.6669e-05, 3.6788e-05, 3.6908e-05, 3.7027e-05, 3.7147e-05, 3.7266e-05, 3.7386e-05, 3.7505e-05, 3.7625e-05, 3.7744e-05, 3.7863e-05, 3.7983e-05, 3.8102e-05, 3.8222e-05, 3.8341e-05, 3.8461e-05, 3.8580e-05, 3.8700e-05, 3.8819e-05, 3.8938e-05, 3.9058e-05, 3.9177e-05, 3.9297e-05, 3.9416e-05, 3.9536e-05, 3.9655e-05, 3.9775e-05, 3.9894e-05, 4.0013e-05, 4.0133e-05, 4.0252e-05, 4.0372e-05, 4.0491e-05, 4.0611e-05, 4.0730e-05, 4.0850e-05, 4.0969e-05, 4.1088e-05, 4.1208e-05, 4.1327e-05, 4.1447e-05, 4.1566e-05, 4.1686e-05, 4.1805e-05, 4.1925e-05, 4.2044e-05, 4.2163e-05, 4.2283e-05, 4.2402e-05, 4.2522e-05, 4.2641e-05, 4.2761e-05, 4.2880e-05, 4.2999e-05, 4.3119e-05, 4.3238e-05, 4.3358e-05, 4.3477e-05, 4.3597e-05, 4.3716e-05, 4.3836e-05, 4.3955e-05, 4.4074e-05, 4.4194e-05, 4.4313e-05, 4.4433e-05, 4.4552e-05, 4.4672e-05, 4.4791e-05, 4.4911e-05, 4.5030e-05, 4.5149e-05, 4.5269e-05, 4.5388e-05, 4.5508e-05, 4.5627e-05, 4.5747e-05, 4.5866e-05, 4.5986e-05, 4.6105e-05, 4.6224e-05, 4.6344e-05, 4.6463e-05, 4.6583e-05, 4.6702e-05, 4.6822e-05, 4.6941e-05, 4.7061e-05, 4.7180e-05, 4.7299e-05, 4.7419e-05, 4.7538e-05, 4.7658e-05, 4.7777e-05, 4.7897e-05, 4.8016e-05, 4.8136e-05, 4.8255e-05, 4.8374e-05, 4.8494e-05, 4.8613e-05, 4.8733e-05, 4.8852e-05, 4.8972e-05, 4.9091e-05, 4.9211e-05, 4.9330e-05, 4.9449e-05, 4.9569e-05, 4.9688e-05, 4.9808e-05, 4.9927e-05, 5.0047e-05, 5.0166e-05, 5.0286e-05, 5.0405e-05, 5.0524e-05, 5.0644e-05, 5.0763e-05, 5.0883e-05, 5.1002e-05, 5.1122e-05, 5.1241e-05, 5.1361e-05, 5.1480e-05, 5.1599e-05, 5.1719e-05, 5.1838e-05, 5.1958e-05, 5.2077e-05, 5.2197e-05, 5.2316e-05, 5.2435e-05, 5.2555e-05, 5.2674e-05, 5.2794e-05, 5.2913e-05, 5.3033e-05, 5.3152e-05, 5.3272e-05, 5.3391e-05, 5.3510e-05, 5.3630e-05, 5.3749e-05, 5.3869e-05, 5.3988e-05, 5.4108e-05, 5.4227e-05, 5.4347e-05, 5.4466e-05, 5.4585e-05, 5.4705e-05, 5.4824e-05, 5.4944e-05, 5.5063e-05, 5.5183e-05, 5.5302e-05, 5.5422e-05, 5.5541e-05, 5.5660e-05, 5.5780e-05, 5.5899e-05, 5.6019e-05, 5.6138e-05, 5.6258e-05, 5.6377e-05, 5.6497e-05, 5.6616e-05, 5.6735e-05, 5.6855e-05, 5.6974e-05, 5.7094e-05, 5.7213e-05, 5.7333e-05, 5.7452e-05, 5.7572e-05, 5.7691e-05, 5.7810e-05, 5.7930e-05, 5.8049e-05, 5.8169e-05, 5.8288e-05, 5.8408e-05, 5.8527e-05, 5.8647e-05, 5.8766e-05, 5.8885e-05, 5.9005e-05, 5.9124e-05, 5.9244e-05, 5.9363e-05, 5.9483e-05, 5.9602e-05, 5.9722e-05, 5.9841e-05, 5.9960e-05, 6.0080e-05, 6.0199e-05, 6.0319e-05, 6.0438e-05, 6.0558e-05, 6.0677e-05, 6.0797e-05, 6.0916e-05, + 6.1154e-05, 6.1392e-05, 6.1631e-05, 6.1869e-05, 6.2107e-05, 6.2345e-05, 6.2583e-05, 6.2821e-05, 6.3060e-05, 6.3298e-05, 6.3536e-05, 6.3774e-05, 6.4012e-05, 6.4251e-05, 6.4489e-05, 6.4727e-05, 6.4965e-05, 6.5203e-05, 6.5441e-05, 6.5680e-05, 6.5918e-05, 6.6156e-05, 6.6394e-05, 6.6632e-05, 6.6871e-05, 6.7109e-05, 6.7347e-05, 6.7585e-05, 6.7823e-05, 6.8062e-05, 6.8300e-05, 6.8538e-05, 6.8776e-05, 6.9014e-05, 6.9252e-05, 6.9491e-05, 6.9729e-05, 6.9967e-05, 7.0205e-05, 7.0443e-05, 7.0682e-05, 7.0920e-05, 7.1158e-05, 7.1396e-05, 7.1634e-05, 7.1872e-05, 7.2111e-05, 7.2349e-05, 7.2587e-05, 7.2825e-05, 7.3063e-05, 7.3302e-05, 7.3540e-05, 7.3778e-05, 7.4016e-05, 7.4254e-05, 7.4493e-05, 7.4731e-05, 7.4969e-05, 7.5207e-05, 7.5445e-05, 7.5683e-05, 7.5922e-05, 7.6160e-05, 7.6398e-05, 7.6636e-05, 7.6874e-05, 7.7113e-05, 7.7351e-05, 7.7589e-05, 7.7827e-05, 7.8065e-05, 7.8304e-05, 7.8542e-05, 7.8780e-05, 7.9018e-05, 7.9256e-05, 7.9494e-05, 7.9733e-05, 7.9971e-05, 8.0209e-05, 8.0447e-05, 8.0685e-05, 8.0924e-05, 8.1162e-05, 8.1400e-05, 8.1638e-05, 8.1876e-05, 8.2114e-05, 8.2353e-05, 8.2591e-05, 8.2829e-05, 8.3067e-05, 8.3305e-05, 8.3544e-05, 8.3782e-05, 8.4020e-05, 8.4258e-05, 8.4496e-05, 8.4735e-05, 8.4973e-05, 8.5211e-05, 8.5449e-05, 8.5687e-05, 8.5925e-05, 8.6164e-05, 8.6402e-05, 8.6640e-05, 8.6878e-05, 8.7116e-05, 8.7355e-05, 8.7593e-05, 8.7831e-05, 8.8069e-05, 8.8307e-05, 8.8545e-05, 8.8784e-05, 8.9022e-05, 8.9260e-05, 8.9498e-05, 8.9736e-05, 8.9975e-05, 9.0213e-05, 9.0451e-05, 9.0689e-05, 9.0927e-05, 9.1166e-05, 9.1404e-05, 9.1642e-05, 9.1880e-05, 9.2118e-05, 9.2356e-05, 9.2595e-05, 9.2833e-05, 9.3071e-05, 9.3309e-05, 9.3547e-05, 9.3786e-05, 9.4024e-05, 9.4262e-05, 9.4500e-05, 9.4738e-05, 9.4977e-05, 9.5215e-05, 9.5453e-05, 9.5691e-05, 9.5929e-05, 9.6167e-05, 9.6406e-05, 9.6644e-05, 9.6882e-05, 9.7120e-05, 9.7358e-05, 9.7597e-05, 9.7835e-05, 9.8073e-05, 9.8311e-05, 9.8549e-05, 9.8787e-05, 9.9026e-05, 9.9264e-05, 9.9502e-05, 9.9740e-05, 9.9978e-05, 1.0022e-04, 1.0045e-04, 1.0069e-04, 1.0093e-04, 1.0117e-04, 1.0141e-04, 1.0165e-04, 1.0188e-04, 1.0212e-04, 1.0236e-04, 1.0260e-04, 1.0284e-04, 1.0307e-04, 1.0331e-04, 1.0355e-04, 1.0379e-04, 1.0403e-04, 1.0427e-04, 1.0450e-04, 1.0474e-04, 1.0498e-04, 1.0522e-04, 1.0546e-04, 1.0569e-04, 1.0593e-04, 1.0617e-04, 1.0641e-04, 1.0665e-04, 1.0689e-04, 1.0712e-04, 1.0736e-04, 1.0760e-04, 1.0784e-04, 1.0808e-04, 1.0831e-04, 1.0855e-04, 1.0879e-04, 1.0903e-04, 1.0927e-04, 1.0951e-04, 1.0974e-04, 1.0998e-04, 1.1022e-04, 1.1046e-04, 1.1070e-04, 1.1093e-04, 1.1117e-04, 1.1141e-04, 1.1165e-04, 1.1189e-04, 1.1213e-04, 1.1236e-04, 1.1260e-04, 1.1284e-04, 1.1308e-04, 1.1332e-04, 1.1355e-04, 1.1379e-04, 1.1403e-04, 1.1427e-04, 1.1451e-04, 1.1475e-04, 1.1498e-04, 1.1522e-04, 1.1546e-04, 1.1570e-04, 1.1594e-04, 1.1618e-04, 1.1641e-04, 1.1665e-04, 1.1689e-04, 1.1713e-04, 1.1737e-04, 1.1760e-04, 1.1784e-04, 1.1808e-04, 1.1832e-04, 1.1856e-04, 1.1880e-04, 1.1903e-04, 1.1927e-04, 1.1951e-04, 1.1975e-04, 1.1999e-04, 1.2022e-04, 1.2046e-04, 1.2070e-04, 1.2094e-04, 1.2118e-04, 1.2142e-04, 1.2165e-04, 1.2189e-04, + 1.2213e-04, 1.2237e-04, 1.2261e-04, 1.2284e-04, 1.2308e-04, 1.2332e-04, 1.2356e-04, 1.2380e-04, 1.2404e-04, 1.2427e-04, 1.2451e-04, 1.2475e-04, 1.2499e-04, 1.2523e-04, 1.2546e-04, 1.2570e-04, 1.2594e-04, 1.2618e-04, 1.2642e-04, 1.2666e-04, 1.2689e-04, 1.2713e-04, 1.2737e-04, 1.2761e-04, 1.2785e-04, 1.2808e-04, 1.2832e-04, 1.2856e-04, 1.2880e-04, 1.2904e-04, 1.2928e-04, 1.2951e-04, 1.2975e-04, 1.2999e-04, 1.3023e-04, 1.3047e-04, 1.3070e-04, 1.3094e-04, 1.3118e-04, 1.3142e-04, 1.3166e-04, 1.3190e-04, 1.3213e-04, 1.3237e-04, 1.3261e-04, 1.3285e-04, 1.3309e-04, 1.3332e-04, 1.3356e-04, 1.3380e-04, 1.3404e-04, 1.3428e-04, 1.3452e-04, 1.3475e-04, 1.3499e-04, 1.3523e-04, 1.3547e-04, 1.3571e-04, 1.3594e-04, 1.3618e-04, 1.3642e-04, 1.3666e-04, 1.3690e-04, 1.3714e-04, 1.3737e-04, 1.3761e-04, 1.3785e-04, 1.3809e-04, 1.3833e-04, 1.3856e-04, 1.3880e-04, 1.3904e-04, 1.3928e-04, 1.3952e-04, 1.3976e-04, 1.3999e-04, 1.4023e-04, 1.4047e-04, 1.4071e-04, 1.4095e-04, 1.4118e-04, 1.4142e-04, 1.4166e-04, 1.4190e-04, 1.4214e-04, 1.4238e-04, 1.4261e-04, 1.4285e-04, 1.4309e-04, 1.4333e-04, 1.4357e-04, 1.4380e-04, 1.4404e-04, 1.4428e-04, 1.4452e-04, 1.4476e-04, 1.4500e-04, 1.4523e-04, 1.4547e-04, 1.4571e-04, 1.4595e-04, 1.4619e-04, 1.4642e-04, 1.4666e-04, 1.4690e-04, 1.4714e-04, 1.4738e-04, 1.4762e-04, 1.4785e-04, 1.4809e-04, 1.4833e-04, 1.4857e-04, 1.4881e-04, 1.4904e-04, 1.4928e-04, 1.4952e-04, 1.4976e-04, 1.5000e-04, 1.5024e-04, 1.5047e-04, 1.5071e-04, 1.5095e-04, 1.5119e-04, 1.5143e-04, 1.5166e-04, 1.5190e-04, 1.5214e-04, 1.5238e-04, 1.5262e-04, 1.5286e-04, 1.5309e-04, 1.5333e-04, 1.5357e-04, 1.5381e-04, 1.5405e-04, 1.5428e-04, 1.5452e-04, 1.5476e-04, 1.5500e-04, 1.5524e-04, 1.5548e-04, 1.5571e-04, 1.5595e-04, 1.5619e-04, 1.5643e-04, 1.5667e-04, 1.5690e-04, 1.5714e-04, 1.5738e-04, 1.5762e-04, 1.5786e-04, 1.5810e-04, 1.5833e-04, 1.5857e-04, 1.5881e-04, 1.5905e-04, 1.5929e-04, 1.5952e-04, 1.5976e-04, 1.6000e-04, 1.6024e-04, 1.6048e-04, 1.6072e-04, 1.6095e-04, 1.6119e-04, 1.6143e-04, 1.6167e-04, 1.6191e-04, 1.6214e-04, 1.6238e-04, 1.6262e-04, 1.6286e-04, 1.6310e-04, 1.6334e-04, 1.6357e-04, 1.6381e-04, 1.6405e-04, 1.6429e-04, 1.6453e-04, 1.6476e-04, 1.6500e-04, 1.6524e-04, 1.6548e-04, 1.6572e-04, 1.6596e-04, 1.6619e-04, 1.6643e-04, 1.6667e-04, 1.6691e-04, 1.6715e-04, 1.6738e-04, 1.6762e-04, 1.6786e-04, 1.6810e-04, 1.6834e-04, 1.6858e-04, 1.6881e-04, 1.6905e-04, 1.6929e-04, 1.6953e-04, 1.6977e-04, 1.7001e-04, 1.7024e-04, 1.7048e-04, 1.7072e-04, 1.7096e-04, 1.7120e-04, 1.7143e-04, 1.7167e-04, 1.7191e-04, 1.7215e-04, 1.7239e-04, 1.7263e-04, 1.7286e-04, 1.7310e-04, 1.7334e-04, 1.7358e-04, 1.7382e-04, 1.7405e-04, 1.7429e-04, 1.7453e-04, 1.7477e-04, 1.7501e-04, 1.7525e-04, 1.7548e-04, 1.7572e-04, 1.7596e-04, 1.7620e-04, 1.7644e-04, 1.7667e-04, 1.7691e-04, 1.7715e-04, 1.7739e-04, 1.7763e-04, 1.7787e-04, 1.7810e-04, 1.7834e-04, 1.7858e-04, 1.7882e-04, 1.7906e-04, 1.7929e-04, 1.7953e-04, 1.7977e-04, 1.8001e-04, 1.8025e-04, 1.8049e-04, 1.8072e-04, 1.8096e-04, 1.8120e-04, 1.8144e-04, 1.8168e-04, 1.8191e-04, 1.8215e-04, 1.8239e-04, 1.8263e-04, 1.8287e-04, + 1.8311e-04, 1.8334e-04, 1.8358e-04, 1.8382e-04, 1.8406e-04, 1.8430e-04, 1.8453e-04, 1.8477e-04, 1.8501e-04, 1.8525e-04, 1.8549e-04, 1.8573e-04, 1.8596e-04, 1.8620e-04, 1.8644e-04, 1.8668e-04, 1.8692e-04, 1.8715e-04, 1.8739e-04, 1.8763e-04, 1.8787e-04, 1.8811e-04, 1.8835e-04, 1.8858e-04, 1.8882e-04, 1.8906e-04, 1.8930e-04, 1.8954e-04, 1.8977e-04, 1.9001e-04, 1.9025e-04, 1.9049e-04, 1.9073e-04, 1.9097e-04, 1.9120e-04, 1.9144e-04, 1.9168e-04, 1.9192e-04, 1.9216e-04, 1.9239e-04, 1.9263e-04, 1.9287e-04, 1.9311e-04, 1.9335e-04, 1.9359e-04, 1.9382e-04, 1.9406e-04, 1.9430e-04, 1.9454e-04, 1.9478e-04, 1.9501e-04, 1.9525e-04, 1.9549e-04, 1.9573e-04, 1.9597e-04, 1.9621e-04, 1.9644e-04, 1.9668e-04, 1.9692e-04, 1.9716e-04, 1.9740e-04, 1.9763e-04, 1.9787e-04, 1.9811e-04, 1.9835e-04, 1.9859e-04, 1.9883e-04, 1.9906e-04, 1.9930e-04, 1.9954e-04, 1.9978e-04, 2.0002e-04, 2.0025e-04, 2.0049e-04, 2.0073e-04, 2.0097e-04, 2.0121e-04, 2.0145e-04, 2.0168e-04, 2.0192e-04, 2.0216e-04, 2.0240e-04, 2.0264e-04, 2.0287e-04, 2.0311e-04, 2.0335e-04, 2.0359e-04, 2.0383e-04, 2.0407e-04, 2.0430e-04, 2.0454e-04, 2.0478e-04, 2.0502e-04, 2.0526e-04, 2.0549e-04, 2.0573e-04, 2.0597e-04, 2.0621e-04, 2.0645e-04, 2.0669e-04, 2.0692e-04, 2.0716e-04, 2.0740e-04, 2.0764e-04, 2.0788e-04, 2.0811e-04, 2.0835e-04, 2.0859e-04, 2.0883e-04, 2.0907e-04, 2.0931e-04, 2.0954e-04, 2.0978e-04, 2.1002e-04, 2.1026e-04, 2.1050e-04, 2.1073e-04, 2.1097e-04, 2.1121e-04, 2.1145e-04, 2.1169e-04, 2.1193e-04, 2.1216e-04, 2.1240e-04, 2.1264e-04, 2.1288e-04, 2.1312e-04, 2.1335e-04, 2.1359e-04, 2.1383e-04, 2.1407e-04, 2.1431e-04, 2.1455e-04, 2.1478e-04, 2.1502e-04, 2.1526e-04, 2.1550e-04, 2.1574e-04, 2.1597e-04, 2.1621e-04, 2.1645e-04, 2.1669e-04, 2.1693e-04, 2.1717e-04, 2.1740e-04, 2.1764e-04, 2.1788e-04, 2.1812e-04, 2.1836e-04, 2.1859e-04, 2.1883e-04, 2.1907e-04, 2.1931e-04, 2.1955e-04, 2.1979e-04, 2.2002e-04, 2.2026e-04, 2.2050e-04, 2.2074e-04, 2.2098e-04, 2.2121e-04, 2.2145e-04, 2.2169e-04, 2.2193e-04, 2.2217e-04, 2.2241e-04, 2.2264e-04, 2.2288e-04, 2.2312e-04, 2.2336e-04, 2.2360e-04, 2.2383e-04, 2.2407e-04, 2.2431e-04, 2.2455e-04, 2.2479e-04, 2.2503e-04, 2.2526e-04, 2.2550e-04, 2.2574e-04, 2.2598e-04, 2.2622e-04, 2.2646e-04, 2.2669e-04, 2.2693e-04, 2.2717e-04, 2.2741e-04, 2.2765e-04, 2.2788e-04, 2.2812e-04, 2.2836e-04, 2.2860e-04, 2.2884e-04, 2.2908e-04, 2.2931e-04, 2.2955e-04, 2.2979e-04, 2.3003e-04, 2.3027e-04, 2.3050e-04, 2.3074e-04, 2.3098e-04, 2.3122e-04, 2.3146e-04, 2.3170e-04, 2.3193e-04, 2.3217e-04, 2.3241e-04, 2.3265e-04, 2.3289e-04, 2.3312e-04, 2.3336e-04, 2.3360e-04, 2.3384e-04, 2.3408e-04, 2.3432e-04, 2.3455e-04, 2.3479e-04, 2.3503e-04, 2.3527e-04, 2.3551e-04, 2.3574e-04, 2.3598e-04, 2.3622e-04, 2.3646e-04, 2.3670e-04, 2.3694e-04, 2.3717e-04, 2.3741e-04, 2.3765e-04, 2.3789e-04, 2.3813e-04, 2.3836e-04, 2.3860e-04, 2.3884e-04, 2.3908e-04, 2.3932e-04, 2.3956e-04, 2.3979e-04, 2.4003e-04, 2.4027e-04, 2.4051e-04, 2.4075e-04, 2.4098e-04, 2.4122e-04, 2.4146e-04, 2.4170e-04, 2.4194e-04, 2.4218e-04, 2.4241e-04, 2.4265e-04, 2.4289e-04, 2.4313e-04, 2.4337e-04, 2.4360e-04, 2.4384e-04, + 2.4480e-04, 2.4575e-04, 2.4670e-04, 2.4766e-04, 2.4861e-04, 2.4956e-04, 2.5052e-04, 2.5147e-04, 2.5242e-04, 2.5337e-04, 2.5433e-04, 2.5528e-04, 2.5623e-04, 2.5719e-04, 2.5814e-04, 2.5909e-04, 2.6005e-04, 2.6100e-04, 2.6195e-04, 2.6291e-04, 2.6386e-04, 2.6481e-04, 2.6577e-04, 2.6672e-04, 2.6767e-04, 2.6863e-04, 2.6958e-04, 2.7053e-04, 2.7149e-04, 2.7244e-04, 2.7339e-04, 2.7435e-04, 2.7530e-04, 2.7625e-04, 2.7720e-04, 2.7816e-04, 2.7911e-04, 2.8006e-04, 2.8102e-04, 2.8197e-04, 2.8292e-04, 2.8388e-04, 2.8483e-04, 2.8578e-04, 2.8674e-04, 2.8769e-04, 2.8864e-04, 2.8960e-04, 2.9055e-04, 2.9150e-04, 2.9246e-04, 2.9341e-04, 2.9436e-04, 2.9532e-04, 2.9627e-04, 2.9722e-04, 2.9818e-04, 2.9913e-04, 3.0008e-04, 3.0104e-04, 3.0199e-04, 3.0294e-04, 3.0389e-04, 3.0485e-04, 3.0580e-04, 3.0675e-04, 3.0771e-04, 3.0866e-04, 3.0961e-04, 3.1057e-04, 3.1152e-04, 3.1247e-04, 3.1343e-04, 3.1438e-04, 3.1533e-04, 3.1629e-04, 3.1724e-04, 3.1819e-04, 3.1915e-04, 3.2010e-04, 3.2105e-04, 3.2201e-04, 3.2296e-04, 3.2391e-04, 3.2487e-04, 3.2582e-04, 3.2677e-04, 3.2772e-04, 3.2868e-04, 3.2963e-04, 3.3058e-04, 3.3154e-04, 3.3249e-04, 3.3344e-04, 3.3440e-04, 3.3535e-04, 3.3630e-04, 3.3726e-04, 3.3821e-04, 3.3916e-04, 3.4012e-04, 3.4107e-04, 3.4202e-04, 3.4298e-04, 3.4393e-04, 3.4488e-04, 3.4584e-04, 3.4679e-04, 3.4774e-04, 3.4870e-04, 3.4965e-04, 3.5060e-04, 3.5156e-04, 3.5251e-04, 3.5346e-04, 3.5441e-04, 3.5537e-04, 3.5632e-04, 3.5727e-04, 3.5823e-04, 3.5918e-04, 3.6013e-04, 3.6109e-04, 3.6204e-04, 3.6299e-04, 3.6395e-04, 3.6490e-04, 3.6585e-04, 3.6681e-04, 3.6776e-04, 3.6871e-04, 3.6967e-04, 3.7062e-04, 3.7157e-04, 3.7253e-04, 3.7348e-04, 3.7443e-04, 3.7539e-04, 3.7634e-04, 3.7729e-04, 3.7825e-04, 3.7920e-04, 3.8015e-04, 3.8110e-04, 3.8206e-04, 3.8301e-04, 3.8396e-04, 3.8492e-04, 3.8587e-04, 3.8682e-04, 3.8778e-04, 3.8873e-04, 3.8968e-04, 3.9064e-04, 3.9159e-04, 3.9254e-04, 3.9350e-04, 3.9445e-04, 3.9540e-04, 3.9636e-04, 3.9731e-04, 3.9826e-04, 3.9922e-04, 4.0017e-04, 4.0112e-04, 4.0208e-04, 4.0303e-04, 4.0398e-04, 4.0493e-04, 4.0589e-04, 4.0684e-04, 4.0779e-04, 4.0875e-04, 4.0970e-04, 4.1065e-04, 4.1161e-04, 4.1256e-04, 4.1351e-04, 4.1447e-04, 4.1542e-04, 4.1637e-04, 4.1733e-04, 4.1828e-04, 4.1923e-04, 4.2019e-04, 4.2114e-04, 4.2209e-04, 4.2305e-04, 4.2400e-04, 4.2495e-04, 4.2591e-04, 4.2686e-04, 4.2781e-04, 4.2877e-04, 4.2972e-04, 4.3067e-04, 4.3162e-04, 4.3258e-04, 4.3353e-04, 4.3448e-04, 4.3544e-04, 4.3639e-04, 4.3734e-04, 4.3830e-04, 4.3925e-04, 4.4020e-04, 4.4116e-04, 4.4211e-04, 4.4306e-04, 4.4402e-04, 4.4497e-04, 4.4592e-04, 4.4688e-04, 4.4783e-04, 4.4878e-04, 4.4974e-04, 4.5069e-04, 4.5164e-04, 4.5260e-04, 4.5355e-04, 4.5450e-04, 4.5545e-04, 4.5641e-04, 4.5736e-04, 4.5831e-04, 4.5927e-04, 4.6022e-04, 4.6117e-04, 4.6213e-04, 4.6308e-04, 4.6403e-04, 4.6499e-04, 4.6594e-04, 4.6689e-04, 4.6785e-04, 4.6880e-04, 4.6975e-04, 4.7071e-04, 4.7166e-04, 4.7261e-04, 4.7357e-04, 4.7452e-04, 4.7547e-04, 4.7643e-04, 4.7738e-04, 4.7833e-04, 4.7929e-04, 4.8024e-04, 4.8119e-04, 4.8214e-04, 4.8310e-04, 4.8405e-04, 4.8500e-04, 4.8596e-04, 4.8691e-04, 4.8786e-04, + 4.8977e-04, 4.9168e-04, 4.9358e-04, 4.9549e-04, 4.9740e-04, 4.9931e-04, 5.0121e-04, 5.0312e-04, 5.0503e-04, 5.0693e-04, 5.0884e-04, 5.1075e-04, 5.1265e-04, 5.1456e-04, 5.1647e-04, 5.1837e-04, 5.2028e-04, 5.2219e-04, 5.2409e-04, 5.2600e-04, 5.2791e-04, 5.2982e-04, 5.3172e-04, 5.3363e-04, 5.3554e-04, 5.3744e-04, 5.3935e-04, 5.4126e-04, 5.4316e-04, 5.4507e-04, 5.4698e-04, 5.4888e-04, 5.5079e-04, 5.5270e-04, 5.5460e-04, 5.5651e-04, 5.5842e-04, 5.6033e-04, 5.6223e-04, 5.6414e-04, 5.6605e-04, 5.6795e-04, 5.6986e-04, 5.7177e-04, 5.7367e-04, 5.7558e-04, 5.7749e-04, 5.7939e-04, 5.8130e-04, 5.8321e-04, 5.8512e-04, 5.8702e-04, 5.8893e-04, 5.9084e-04, 5.9274e-04, 5.9465e-04, 5.9656e-04, 5.9846e-04, 6.0037e-04, 6.0228e-04, 6.0418e-04, 6.0609e-04, 6.0800e-04, 6.0990e-04, 6.1181e-04, 6.1372e-04, 6.1563e-04, 6.1753e-04, 6.1944e-04, 6.2135e-04, 6.2325e-04, 6.2516e-04, 6.2707e-04, 6.2897e-04, 6.3088e-04, 6.3279e-04, 6.3469e-04, 6.3660e-04, 6.3851e-04, 6.4041e-04, 6.4232e-04, 6.4423e-04, 6.4614e-04, 6.4804e-04, 6.4995e-04, 6.5186e-04, 6.5376e-04, 6.5567e-04, 6.5758e-04, 6.5948e-04, 6.6139e-04, 6.6330e-04, 6.6520e-04, 6.6711e-04, 6.6902e-04, 6.7092e-04, 6.7283e-04, 6.7474e-04, 6.7665e-04, 6.7855e-04, 6.8046e-04, 6.8237e-04, 6.8427e-04, 6.8618e-04, 6.8809e-04, 6.8999e-04, 6.9190e-04, 6.9381e-04, 6.9571e-04, 6.9762e-04, 6.9953e-04, 7.0143e-04, 7.0334e-04, 7.0525e-04, 7.0716e-04, 7.0906e-04, 7.1097e-04, 7.1288e-04, 7.1478e-04, 7.1669e-04, 7.1860e-04, 7.2050e-04, 7.2241e-04, 7.2432e-04, 7.2622e-04, 7.2813e-04, 7.3004e-04, 7.3195e-04, 7.3385e-04, 7.3576e-04, 7.3767e-04, 7.3957e-04, 7.4148e-04, 7.4339e-04, 7.4529e-04, 7.4720e-04, 7.4911e-04, 7.5101e-04, 7.5292e-04, 7.5483e-04, 7.5673e-04, 7.5864e-04, 7.6055e-04, 7.6246e-04, 7.6436e-04, 7.6627e-04, 7.6818e-04, 7.7008e-04, 7.7199e-04, 7.7390e-04, 7.7580e-04, 7.7771e-04, 7.7962e-04, 7.8152e-04, 7.8343e-04, 7.8534e-04, 7.8724e-04, 7.8915e-04, 7.9106e-04, 7.9297e-04, 7.9487e-04, 7.9678e-04, 7.9869e-04, 8.0059e-04, 8.0250e-04, 8.0441e-04, 8.0631e-04, 8.0822e-04, 8.1013e-04, 8.1203e-04, 8.1394e-04, 8.1585e-04, 8.1775e-04, 8.1966e-04, 8.2157e-04, 8.2348e-04, 8.2538e-04, 8.2729e-04, 8.2920e-04, 8.3110e-04, 8.3301e-04, 8.3492e-04, 8.3682e-04, 8.3873e-04, 8.4064e-04, 8.4254e-04, 8.4445e-04, 8.4636e-04, 8.4826e-04, 8.5017e-04, 8.5208e-04, 8.5399e-04, 8.5589e-04, 8.5780e-04, 8.5971e-04, 8.6161e-04, 8.6352e-04, 8.6543e-04, 8.6733e-04, 8.6924e-04, 8.7115e-04, 8.7305e-04, 8.7496e-04, 8.7687e-04, 8.7878e-04, 8.8068e-04, 8.8259e-04, 8.8450e-04, 8.8640e-04, 8.8831e-04, 8.9022e-04, 8.9212e-04, 8.9403e-04, 8.9594e-04, 8.9784e-04, 8.9975e-04, 9.0166e-04, 9.0356e-04, 9.0547e-04, 9.0738e-04, 9.0929e-04, 9.1119e-04, 9.1310e-04, 9.1501e-04, 9.1691e-04, 9.1882e-04, 9.2073e-04, 9.2263e-04, 9.2454e-04, 9.2645e-04, 9.2835e-04, 9.3026e-04, 9.3217e-04, 9.3407e-04, 9.3598e-04, 9.3789e-04, 9.3980e-04, 9.4170e-04, 9.4361e-04, 9.4552e-04, 9.4742e-04, 9.4933e-04, 9.5124e-04, 9.5314e-04, 9.5505e-04, 9.5696e-04, 9.5886e-04, 9.6077e-04, 9.6268e-04, 9.6458e-04, 9.6649e-04, 9.6840e-04, 9.7031e-04, 9.7221e-04, 9.7412e-04, 9.7603e-04, + 9.7984e-04, 9.8365e-04, 9.8747e-04, 9.9128e-04, 9.9510e-04, 9.9891e-04, 1.0027e-03, 1.0065e-03, 1.0104e-03, 1.0142e-03, 1.0180e-03, 1.0218e-03, 1.0256e-03, 1.0294e-03, 1.0332e-03, 1.0371e-03, 1.0409e-03, 1.0447e-03, 1.0485e-03, 1.0523e-03, 1.0561e-03, 1.0599e-03, 1.0638e-03, 1.0676e-03, 1.0714e-03, 1.0752e-03, 1.0790e-03, 1.0828e-03, 1.0866e-03, 1.0905e-03, 1.0943e-03, 1.0981e-03, 1.1019e-03, 1.1057e-03, 1.1095e-03, 1.1133e-03, 1.1172e-03, 1.1210e-03, 1.1248e-03, 1.1286e-03, 1.1324e-03, 1.1362e-03, 1.1400e-03, 1.1439e-03, 1.1477e-03, 1.1515e-03, 1.1553e-03, 1.1591e-03, 1.1629e-03, 1.1667e-03, 1.1706e-03, 1.1744e-03, 1.1782e-03, 1.1820e-03, 1.1858e-03, 1.1896e-03, 1.1934e-03, 1.1973e-03, 1.2011e-03, 1.2049e-03, 1.2087e-03, 1.2125e-03, 1.2163e-03, 1.2201e-03, 1.2240e-03, 1.2278e-03, 1.2316e-03, 1.2354e-03, 1.2392e-03, 1.2430e-03, 1.2468e-03, 1.2507e-03, 1.2545e-03, 1.2583e-03, 1.2621e-03, 1.2659e-03, 1.2697e-03, 1.2735e-03, 1.2774e-03, 1.2812e-03, 1.2850e-03, 1.2888e-03, 1.2926e-03, 1.2964e-03, 1.3002e-03, 1.3040e-03, 1.3079e-03, 1.3117e-03, 1.3155e-03, 1.3193e-03, 1.3231e-03, 1.3269e-03, 1.3307e-03, 1.3346e-03, 1.3384e-03, 1.3422e-03, 1.3460e-03, 1.3498e-03, 1.3536e-03, 1.3574e-03, 1.3613e-03, 1.3651e-03, 1.3689e-03, 1.3727e-03, 1.3765e-03, 1.3803e-03, 1.3841e-03, 1.3880e-03, 1.3918e-03, 1.3956e-03, 1.3994e-03, 1.4032e-03, 1.4070e-03, 1.4108e-03, 1.4147e-03, 1.4185e-03, 1.4223e-03, 1.4261e-03, 1.4299e-03, 1.4337e-03, 1.4375e-03, 1.4414e-03, 1.4452e-03, 1.4490e-03, 1.4528e-03, 1.4566e-03, 1.4604e-03, 1.4642e-03, 1.4681e-03, 1.4719e-03, 1.4757e-03, 1.4795e-03, 1.4833e-03, 1.4871e-03, 1.4909e-03, 1.4948e-03, 1.4986e-03, 1.5024e-03, 1.5062e-03, 1.5100e-03, 1.5138e-03, 1.5176e-03, 1.5215e-03, 1.5253e-03, 1.5291e-03, 1.5329e-03, 1.5367e-03, 1.5405e-03, 1.5443e-03, 1.5482e-03, 1.5520e-03, 1.5558e-03, 1.5596e-03, 1.5634e-03, 1.5672e-03, 1.5710e-03, 1.5749e-03, 1.5787e-03, 1.5825e-03, 1.5863e-03, 1.5901e-03, 1.5939e-03, 1.5977e-03, 1.6016e-03, 1.6054e-03, 1.6092e-03, 1.6130e-03, 1.6168e-03, 1.6206e-03, 1.6244e-03, 1.6283e-03, 1.6321e-03, 1.6359e-03, 1.6397e-03, 1.6435e-03, 1.6473e-03, 1.6511e-03, 1.6550e-03, 1.6588e-03, 1.6626e-03, 1.6664e-03, 1.6702e-03, 1.6740e-03, 1.6778e-03, 1.6817e-03, 1.6855e-03, 1.6893e-03, 1.6931e-03, 1.6969e-03, 1.7007e-03, 1.7045e-03, 1.7084e-03, 1.7122e-03, 1.7160e-03, 1.7198e-03, 1.7236e-03, 1.7274e-03, 1.7312e-03, 1.7351e-03, 1.7389e-03, 1.7427e-03, 1.7465e-03, 1.7503e-03, 1.7541e-03, 1.7579e-03, 1.7618e-03, 1.7656e-03, 1.7694e-03, 1.7732e-03, 1.7770e-03, 1.7808e-03, 1.7846e-03, 1.7885e-03, 1.7923e-03, 1.7961e-03, 1.7999e-03, 1.8037e-03, 1.8075e-03, 1.8113e-03, 1.8152e-03, 1.8190e-03, 1.8228e-03, 1.8266e-03, 1.8304e-03, 1.8342e-03, 1.8380e-03, 1.8419e-03, 1.8457e-03, 1.8495e-03, 1.8533e-03, 1.8571e-03, 1.8609e-03, 1.8647e-03, 1.8686e-03, 1.8724e-03, 1.8762e-03, 1.8800e-03, 1.8838e-03, 1.8876e-03, 1.8914e-03, 1.8953e-03, 1.8991e-03, 1.9029e-03, 1.9067e-03, 1.9105e-03, 1.9143e-03, 1.9181e-03, 1.9220e-03, 1.9258e-03, 1.9296e-03, 1.9334e-03, 1.9372e-03, 1.9410e-03, 1.9448e-03, 1.9487e-03, 1.9525e-03, + 1.9601e-03, 1.9677e-03, 1.9754e-03, 1.9830e-03, 1.9906e-03, 1.9982e-03, 2.0059e-03, 2.0135e-03, 2.0211e-03, 2.0288e-03, 2.0364e-03, 2.0440e-03, 2.0516e-03, 2.0593e-03, 2.0669e-03, 2.0745e-03, 2.0822e-03, 2.0898e-03, 2.0974e-03, 2.1050e-03, 2.1127e-03, 2.1203e-03, 2.1279e-03, 2.1356e-03, 2.1432e-03, 2.1508e-03, 2.1585e-03, 2.1661e-03, 2.1737e-03, 2.1813e-03, 2.1890e-03, 2.1966e-03, 2.2042e-03, 2.2119e-03, 2.2195e-03, 2.2271e-03, 2.2347e-03, 2.2424e-03, 2.2500e-03, 2.2576e-03, 2.2653e-03, 2.2729e-03, 2.2805e-03, 2.2881e-03, 2.2958e-03, 2.3034e-03, 2.3110e-03, 2.3187e-03, 2.3263e-03, 2.3339e-03, 2.3415e-03, 2.3492e-03, 2.3568e-03, 2.3644e-03, 2.3721e-03, 2.3797e-03, 2.3873e-03, 2.3949e-03, 2.4026e-03, 2.4102e-03, 2.4178e-03, 2.4255e-03, 2.4331e-03, 2.4407e-03, 2.4483e-03, 2.4560e-03, 2.4636e-03, 2.4712e-03, 2.4789e-03, 2.4865e-03, 2.4941e-03, 2.5018e-03, 2.5094e-03, 2.5170e-03, 2.5246e-03, 2.5323e-03, 2.5399e-03, 2.5475e-03, 2.5552e-03, 2.5628e-03, 2.5704e-03, 2.5780e-03, 2.5857e-03, 2.5933e-03, 2.6009e-03, 2.6086e-03, 2.6162e-03, 2.6238e-03, 2.6314e-03, 2.6391e-03, 2.6467e-03, 2.6543e-03, 2.6620e-03, 2.6696e-03, 2.6772e-03, 2.6848e-03, 2.6925e-03, 2.7001e-03, 2.7077e-03, 2.7154e-03, 2.7230e-03, 2.7306e-03, 2.7382e-03, 2.7459e-03, 2.7535e-03, 2.7611e-03, 2.7688e-03, 2.7764e-03, 2.7840e-03, 2.7917e-03, 2.7993e-03, 2.8069e-03, 2.8145e-03, 2.8222e-03, 2.8298e-03, 2.8374e-03, 2.8451e-03, 2.8527e-03, 2.8603e-03, 2.8679e-03, 2.8756e-03, 2.8832e-03, 2.8908e-03, 2.8985e-03, 2.9061e-03, 2.9137e-03, 2.9213e-03, 2.9290e-03, 2.9366e-03, 2.9442e-03, 2.9519e-03, 2.9595e-03, 2.9671e-03, 2.9747e-03, 2.9824e-03, 2.9900e-03, 2.9976e-03, 3.0053e-03, 3.0129e-03, 3.0205e-03, 3.0281e-03, 3.0358e-03, 3.0434e-03, 3.0510e-03, 3.0587e-03, 3.0663e-03, 3.0739e-03, 3.0816e-03, 3.0892e-03, 3.0968e-03, 3.1044e-03, 3.1121e-03, 3.1197e-03, 3.1273e-03, 3.1350e-03, 3.1426e-03, 3.1502e-03, 3.1578e-03, 3.1655e-03, 3.1731e-03, 3.1807e-03, 3.1884e-03, 3.1960e-03, 3.2036e-03, 3.2112e-03, 3.2189e-03, 3.2265e-03, 3.2341e-03, 3.2418e-03, 3.2494e-03, 3.2570e-03, 3.2646e-03, 3.2723e-03, 3.2799e-03, 3.2875e-03, 3.2952e-03, 3.3028e-03, 3.3104e-03, 3.3180e-03, 3.3257e-03, 3.3333e-03, 3.3409e-03, 3.3486e-03, 3.3562e-03, 3.3638e-03, 3.3715e-03, 3.3791e-03, 3.3867e-03, 3.3943e-03, 3.4020e-03, 3.4096e-03, 3.4172e-03, 3.4249e-03, 3.4325e-03, 3.4401e-03, 3.4477e-03, 3.4554e-03, 3.4630e-03, 3.4706e-03, 3.4783e-03, 3.4859e-03, 3.4935e-03, 3.5011e-03, 3.5088e-03, 3.5164e-03, 3.5240e-03, 3.5317e-03, 3.5393e-03, 3.5469e-03, 3.5545e-03, 3.5622e-03, 3.5698e-03, 3.5774e-03, 3.5851e-03, 3.5927e-03, 3.6003e-03, 3.6079e-03, 3.6156e-03, 3.6232e-03, 3.6308e-03, 3.6385e-03, 3.6461e-03, 3.6537e-03, 3.6613e-03, 3.6690e-03, 3.6766e-03, 3.6842e-03, 3.6919e-03, 3.6995e-03, 3.7071e-03, 3.7148e-03, 3.7224e-03, 3.7300e-03, 3.7376e-03, 3.7453e-03, 3.7529e-03, 3.7605e-03, 3.7682e-03, 3.7758e-03, 3.7834e-03, 3.7910e-03, 3.7987e-03, 3.8063e-03, 3.8139e-03, 3.8216e-03, 3.8292e-03, 3.8368e-03, 3.8444e-03, 3.8521e-03, 3.8597e-03, 3.8673e-03, 3.8750e-03, 3.8826e-03, 3.8902e-03, 3.8978e-03, 3.9055e-03, + 3.9207e-03, 3.9360e-03, 3.9513e-03, 3.9665e-03, 3.9818e-03, 3.9970e-03, 4.0123e-03, 4.0275e-03, 4.0428e-03, 4.0581e-03, 4.0733e-03, 4.0886e-03, 4.1038e-03, 4.1191e-03, 4.1343e-03, 4.1496e-03, 4.1649e-03, 4.1801e-03, 4.1954e-03, 4.2106e-03, 4.2259e-03, 4.2412e-03, 4.2564e-03, 4.2717e-03, 4.2869e-03, 4.3022e-03, 4.3174e-03, 4.3327e-03, 4.3480e-03, 4.3632e-03, 4.3785e-03, 4.3937e-03, 4.4090e-03, 4.4243e-03, 4.4395e-03, 4.4548e-03, 4.4700e-03, 4.4853e-03, 4.5005e-03, 4.5158e-03, 4.5311e-03, 4.5463e-03, 4.5616e-03, 4.5768e-03, 4.5921e-03, 4.6074e-03, 4.6226e-03, 4.6379e-03, 4.6531e-03, 4.6684e-03, 4.6836e-03, 4.6989e-03, 4.7142e-03, 4.7294e-03, 4.7447e-03, 4.7599e-03, 4.7752e-03, 4.7905e-03, 4.8057e-03, 4.8210e-03, 4.8362e-03, 4.8515e-03, 4.8667e-03, 4.8820e-03, 4.8973e-03, 4.9125e-03, 4.9278e-03, 4.9430e-03, 4.9583e-03, 4.9736e-03, 4.9888e-03, 5.0041e-03, 5.0193e-03, 5.0346e-03, 5.0498e-03, 5.0651e-03, 5.0804e-03, 5.0956e-03, 5.1109e-03, 5.1261e-03, 5.1414e-03, 5.1567e-03, 5.1719e-03, 5.1872e-03, 5.2024e-03, 5.2177e-03, 5.2329e-03, 5.2482e-03, 5.2635e-03, 5.2787e-03, 5.2940e-03, 5.3092e-03, 5.3245e-03, 5.3398e-03, 5.3550e-03, 5.3703e-03, 5.3855e-03, 5.4008e-03, 5.4160e-03, 5.4313e-03, 5.4466e-03, 5.4618e-03, 5.4771e-03, 5.4923e-03, 5.5076e-03, 5.5229e-03, 5.5381e-03, 5.5534e-03, 5.5686e-03, 5.5839e-03, 5.5991e-03, 5.6144e-03, 5.6297e-03, 5.6449e-03, 5.6602e-03, 5.6754e-03, 5.6907e-03, 5.7060e-03, 5.7212e-03, 5.7365e-03, 5.7517e-03, 5.7670e-03, 5.7822e-03, 5.7975e-03, 5.8128e-03, 5.8280e-03, 5.8433e-03, 5.8585e-03, 5.8738e-03, 5.8891e-03, 5.9043e-03, 5.9196e-03, 5.9348e-03, 5.9501e-03, 5.9653e-03, 5.9806e-03, 5.9959e-03, 6.0111e-03, 6.0264e-03, 6.0416e-03, 6.0569e-03, 6.0722e-03, 6.0874e-03, 6.1027e-03, 6.1179e-03, 6.1332e-03, 6.1484e-03, 6.1637e-03, 6.1790e-03, 6.1942e-03, 6.2095e-03, 6.2247e-03, 6.2400e-03, 6.2553e-03, 6.2705e-03, 6.2858e-03, 6.3010e-03, 6.3163e-03, 6.3315e-03, 6.3468e-03, 6.3621e-03, 6.3773e-03, 6.3926e-03, 6.4078e-03, 6.4231e-03, 6.4384e-03, 6.4536e-03, 6.4689e-03, 6.4841e-03, 6.4994e-03, 6.5146e-03, 6.5299e-03, 6.5452e-03, 6.5604e-03, 6.5757e-03, 6.5909e-03, 6.6062e-03, 6.6215e-03, 6.6367e-03, 6.6520e-03, 6.6672e-03, 6.6825e-03, 6.6977e-03, 6.7130e-03, 6.7283e-03, 6.7435e-03, 6.7588e-03, 6.7740e-03, 6.7893e-03, 6.8046e-03, 6.8198e-03, 6.8351e-03, 6.8503e-03, 6.8656e-03, 6.8808e-03, 6.8961e-03, 6.9114e-03, 6.9266e-03, 6.9419e-03, 6.9571e-03, 6.9724e-03, 6.9877e-03, 7.0029e-03, 7.0182e-03, 7.0334e-03, 7.0487e-03, 7.0639e-03, 7.0792e-03, 7.0945e-03, 7.1097e-03, 7.1250e-03, 7.1402e-03, 7.1555e-03, 7.1708e-03, 7.1860e-03, 7.2013e-03, 7.2165e-03, 7.2318e-03, 7.2470e-03, 7.2623e-03, 7.2776e-03, 7.2928e-03, 7.3081e-03, 7.3233e-03, 7.3386e-03, 7.3539e-03, 7.3691e-03, 7.3844e-03, 7.3996e-03, 7.4149e-03, 7.4301e-03, 7.4454e-03, 7.4607e-03, 7.4759e-03, 7.4912e-03, 7.5064e-03, 7.5217e-03, 7.5370e-03, 7.5522e-03, 7.5675e-03, 7.5827e-03, 7.5980e-03, 7.6132e-03, 7.6285e-03, 7.6438e-03, 7.6590e-03, 7.6743e-03, 7.6895e-03, 7.7048e-03, 7.7201e-03, 7.7353e-03, 7.7506e-03, 7.7658e-03, 7.7811e-03, 7.7963e-03, 7.8116e-03, + 7.8421e-03, 7.8726e-03, 7.9032e-03, 7.9337e-03, 7.9642e-03, 7.9947e-03, 8.0252e-03, 8.0557e-03, 8.0863e-03, 8.1168e-03, 8.1473e-03, 8.1778e-03, 8.2083e-03, 8.2388e-03, 8.2694e-03, 8.2999e-03, 8.3304e-03, 8.3609e-03, 8.3914e-03, 8.4219e-03, 8.4525e-03, 8.4830e-03, 8.5135e-03, 8.5440e-03, 8.5745e-03, 8.6051e-03, 8.6356e-03, 8.6661e-03, 8.6966e-03, 8.7271e-03, 8.7576e-03, 8.7882e-03, 8.8187e-03, 8.8492e-03, 8.8797e-03, 8.9102e-03, 8.9407e-03, 8.9713e-03, 9.0018e-03, 9.0323e-03, 9.0628e-03, 9.0933e-03, 9.1238e-03, 9.1544e-03, 9.1849e-03, 9.2154e-03, 9.2459e-03, 9.2764e-03, 9.3069e-03, 9.3375e-03, 9.3680e-03, 9.3985e-03, 9.4290e-03, 9.4595e-03, 9.4900e-03, 9.5206e-03, 9.5511e-03, 9.5816e-03, 9.6121e-03, 9.6426e-03, 9.6731e-03, 9.7037e-03, 9.7342e-03, 9.7647e-03, 9.7952e-03, 9.8257e-03, 9.8563e-03, 9.8868e-03, 9.9173e-03, 9.9478e-03, 9.9783e-03, 1.0009e-02, 1.0039e-02, 1.0070e-02, 1.0100e-02, 1.0131e-02, 1.0161e-02, 1.0192e-02, 1.0222e-02, 1.0253e-02, 1.0283e-02, 1.0314e-02, 1.0345e-02, 1.0375e-02, 1.0406e-02, 1.0436e-02, 1.0467e-02, 1.0497e-02, 1.0528e-02, 1.0558e-02, 1.0589e-02, 1.0619e-02, 1.0650e-02, 1.0680e-02, 1.0711e-02, 1.0741e-02, 1.0772e-02, 1.0802e-02, 1.0833e-02, 1.0863e-02, 1.0894e-02, 1.0924e-02, 1.0955e-02, 1.0985e-02, 1.1016e-02, 1.1046e-02, 1.1077e-02, 1.1107e-02, 1.1138e-02, 1.1168e-02, 1.1199e-02, 1.1230e-02, 1.1260e-02, 1.1291e-02, 1.1321e-02, 1.1352e-02, 1.1382e-02, 1.1413e-02, 1.1443e-02, 1.1474e-02, 1.1504e-02, 1.1535e-02, 1.1565e-02, 1.1596e-02, 1.1626e-02, 1.1657e-02, 1.1687e-02, 1.1718e-02, 1.1748e-02, 1.1779e-02, 1.1809e-02, 1.1840e-02, 1.1870e-02, 1.1901e-02, 1.1931e-02, 1.1962e-02, 1.1992e-02, 1.2023e-02, 1.2053e-02, 1.2084e-02, 1.2115e-02, 1.2145e-02, 1.2176e-02, 1.2206e-02, 1.2237e-02, 1.2267e-02, 1.2298e-02, 1.2328e-02, 1.2359e-02, 1.2389e-02, 1.2420e-02, 1.2450e-02, 1.2481e-02, 1.2511e-02, 1.2542e-02, 1.2572e-02, 1.2603e-02, 1.2633e-02, 1.2664e-02, 1.2694e-02, 1.2725e-02, 1.2755e-02, 1.2786e-02, 1.2816e-02, 1.2847e-02, 1.2877e-02, 1.2908e-02, 1.2938e-02, 1.2969e-02, 1.3000e-02, 1.3030e-02, 1.3061e-02, 1.3091e-02, 1.3122e-02, 1.3152e-02, 1.3183e-02, 1.3213e-02, 1.3244e-02, 1.3274e-02, 1.3305e-02, 1.3335e-02, 1.3366e-02, 1.3396e-02, 1.3427e-02, 1.3457e-02, 1.3488e-02, 1.3518e-02, 1.3549e-02, 1.3579e-02, 1.3610e-02, 1.3640e-02, 1.3671e-02, 1.3701e-02, 1.3732e-02, 1.3762e-02, 1.3793e-02, 1.3823e-02, 1.3854e-02, 1.3885e-02, 1.3915e-02, 1.3946e-02, 1.3976e-02, 1.4007e-02, 1.4037e-02, 1.4068e-02, 1.4098e-02, 1.4129e-02, 1.4159e-02, 1.4190e-02, 1.4220e-02, 1.4251e-02, 1.4281e-02, 1.4312e-02, 1.4342e-02, 1.4373e-02, 1.4403e-02, 1.4434e-02, 1.4464e-02, 1.4495e-02, 1.4525e-02, 1.4556e-02, 1.4586e-02, 1.4617e-02, 1.4647e-02, 1.4678e-02, 1.4708e-02, 1.4739e-02, 1.4770e-02, 1.4800e-02, 1.4831e-02, 1.4861e-02, 1.4892e-02, 1.4922e-02, 1.4953e-02, 1.4983e-02, 1.5014e-02, 1.5044e-02, 1.5075e-02, 1.5105e-02, 1.5136e-02, 1.5166e-02, 1.5197e-02, 1.5227e-02, 1.5258e-02, 1.5288e-02, 1.5319e-02, 1.5349e-02, 1.5380e-02, 1.5410e-02, 1.5441e-02, 1.5471e-02, 1.5502e-02, 1.5532e-02, 1.5563e-02, 1.5593e-02, 1.5624e-02, + 1.5746e-02, 1.5868e-02, 1.5990e-02, 1.6112e-02, 1.6234e-02, 1.6356e-02, 1.6478e-02, 1.6601e-02, 1.6723e-02, 1.6845e-02, 1.6967e-02, 1.7089e-02, 1.7211e-02, 1.7333e-02, 1.7455e-02, 1.7577e-02, 1.7699e-02, 1.7821e-02, 1.7943e-02, 1.8065e-02, 1.8187e-02, 1.8310e-02, 1.8432e-02, 1.8554e-02, 1.8676e-02, 1.8798e-02, 1.8920e-02, 1.9042e-02, 1.9164e-02, 1.9286e-02, 1.9408e-02, 1.9530e-02, 1.9652e-02, 1.9774e-02, 1.9896e-02, 2.0018e-02, 2.0141e-02, 2.0263e-02, 2.0385e-02, 2.0507e-02, 2.0629e-02, 2.0751e-02, 2.0873e-02, 2.0995e-02, 2.1117e-02, 2.1239e-02, 2.1361e-02, 2.1483e-02, 2.1605e-02, 2.1727e-02, 2.1850e-02, 2.1972e-02, 2.2094e-02, 2.2216e-02, 2.2338e-02, 2.2460e-02, 2.2582e-02, 2.2704e-02, 2.2826e-02, 2.2948e-02, 2.3070e-02, 2.3192e-02, 2.3314e-02, 2.3436e-02, 2.3558e-02, 2.3681e-02, 2.3803e-02, 2.3925e-02, 2.4047e-02, 2.4169e-02, 2.4291e-02, 2.4413e-02, 2.4535e-02, 2.4657e-02, 2.4779e-02, 2.4901e-02, 2.5023e-02, 2.5145e-02, 2.5267e-02, 2.5390e-02, 2.5512e-02, 2.5634e-02, 2.5756e-02, 2.5878e-02, 2.6000e-02, 2.6122e-02, 2.6244e-02, 2.6366e-02, 2.6488e-02, 2.6610e-02, 2.6732e-02, 2.6854e-02, 2.6976e-02, 2.7099e-02, 2.7221e-02, 2.7343e-02, 2.7465e-02, 2.7587e-02, 2.7709e-02, 2.7831e-02, 2.7953e-02, 2.8075e-02, 2.8197e-02, 2.8319e-02, 2.8441e-02, 2.8563e-02, 2.8685e-02, 2.8807e-02, 2.8930e-02, 2.9052e-02, 2.9174e-02, 2.9296e-02, 2.9418e-02, 2.9540e-02, 2.9662e-02, 2.9784e-02, 2.9906e-02, 3.0028e-02, 3.0150e-02, 3.0272e-02, 3.0394e-02, 3.0516e-02, 3.0639e-02, 3.0761e-02, 3.0883e-02, 3.1005e-02, 3.1127e-02, 3.1249e-02, 3.1493e-02, 3.1737e-02, 3.1981e-02, 3.2225e-02, 3.2470e-02, 3.2714e-02, 3.2958e-02, 3.3202e-02, 3.3446e-02, 3.3690e-02, 3.3934e-02, 3.4179e-02, 3.4423e-02, 3.4667e-02, 3.4911e-02, 3.5155e-02, 3.5399e-02, 3.5643e-02, 3.5888e-02, 3.6132e-02, 3.6376e-02, 3.6620e-02, 3.6864e-02, 3.7108e-02, 3.7352e-02, 3.7596e-02, 3.7841e-02, 3.8085e-02, 3.8329e-02, 3.8573e-02, 3.8817e-02, 3.9061e-02, 3.9305e-02, 3.9550e-02, 3.9794e-02, 4.0038e-02, 4.0282e-02, 4.0526e-02, 4.0770e-02, 4.1014e-02, 4.1259e-02, 4.1503e-02, 4.1747e-02, 4.1991e-02, 4.2235e-02, 4.2479e-02, 4.2723e-02, 4.2968e-02, 4.3212e-02, 4.3456e-02, 4.3700e-02, 4.3944e-02, 4.4188e-02, 4.4432e-02, 4.4677e-02, 4.4921e-02, 4.5165e-02, 4.5409e-02, 4.5653e-02, 4.5897e-02, 4.6141e-02, 4.6386e-02, 4.6630e-02, 4.6874e-02, 4.7118e-02, 4.7362e-02, 4.7606e-02, 4.7850e-02, 4.8095e-02, 4.8339e-02, 4.8583e-02, 4.8827e-02, 4.9071e-02, 4.9315e-02, 4.9559e-02, 4.9803e-02, 5.0048e-02, 5.0292e-02, 5.0536e-02, 5.0780e-02, 5.1024e-02, 5.1268e-02, 5.1512e-02, 5.1757e-02, 5.2001e-02, 5.2245e-02, 5.2489e-02, 5.2733e-02, 5.2977e-02, 5.3221e-02, 5.3466e-02, 5.3710e-02, 5.3954e-02, 5.4198e-02, 5.4442e-02, 5.4686e-02, 5.4930e-02, 5.5175e-02, 5.5419e-02, 5.5663e-02, 5.5907e-02, 5.6151e-02, 5.6395e-02, 5.6639e-02, 5.6884e-02, 5.7128e-02, 5.7372e-02, 5.7616e-02, 5.7860e-02, 5.8104e-02, 5.8348e-02, 5.8593e-02, 5.8837e-02, 5.9081e-02, 5.9325e-02, 5.9569e-02, 5.9813e-02, 6.0057e-02, 6.0301e-02, 6.0546e-02, 6.0790e-02, 6.1034e-02, 6.1278e-02, 6.1522e-02, 6.1766e-02, 6.2010e-02, 6.2255e-02, 6.2499e-02, + 6.2743e-02, 6.2987e-02, 6.3231e-02, 6.3475e-02, 6.3719e-02, 6.3964e-02, 6.4208e-02, 6.4452e-02, 6.4696e-02, 6.4940e-02, 6.5184e-02, 6.5428e-02, 6.5673e-02, 6.5917e-02, 6.6161e-02, 6.6405e-02, 6.6649e-02, 6.6893e-02, 6.7137e-02, 6.7382e-02, 6.7626e-02, 6.7870e-02, 6.8114e-02, 6.8358e-02, 6.8602e-02, 6.8846e-02, 6.9091e-02, 6.9335e-02, 6.9579e-02, 6.9823e-02, 7.0067e-02, 7.0311e-02, 7.0555e-02, 7.0799e-02, 7.1044e-02, 7.1288e-02, 7.1532e-02, 7.1776e-02, 7.2020e-02, 7.2264e-02, 7.2508e-02, 7.2753e-02, 7.2997e-02, 7.3241e-02, 7.3485e-02, 7.3729e-02, 7.3973e-02, 7.4217e-02, 7.4462e-02, 7.4706e-02, 7.4950e-02, 7.5194e-02, 7.5438e-02, 7.5682e-02, 7.5926e-02, 7.6171e-02, 7.6415e-02, 7.6659e-02, 7.6903e-02, 7.7147e-02, 7.7391e-02, 7.7635e-02, 7.7880e-02, 7.8124e-02, 7.8368e-02, 7.8612e-02, 7.8856e-02, 7.9100e-02, 7.9344e-02, 7.9589e-02, 7.9833e-02, 8.0077e-02, 8.0321e-02, 8.0565e-02, 8.0809e-02, 8.1053e-02, 8.1298e-02, 8.1542e-02, 8.1786e-02, 8.2030e-02, 8.2274e-02, 8.2518e-02, 8.2762e-02, 8.3006e-02, 8.3251e-02, 8.3495e-02, 8.3739e-02, 8.3983e-02, 8.4227e-02, 8.4471e-02, 8.4715e-02, 8.4960e-02, 8.5204e-02, 8.5448e-02, 8.5692e-02, 8.5936e-02, 8.6180e-02, 8.6424e-02, 8.6669e-02, 8.6913e-02, 8.7157e-02, 8.7401e-02, 8.7645e-02, 8.7889e-02, 8.8133e-02, 8.8378e-02, 8.8622e-02, 8.8866e-02, 8.9110e-02, 8.9354e-02, 8.9598e-02, 8.9842e-02, 9.0087e-02, 9.0331e-02, 9.0575e-02, 9.0819e-02, 9.1063e-02, 9.1307e-02, 9.1551e-02, 9.1796e-02, 9.2040e-02, 9.2284e-02, 9.2528e-02, 9.2772e-02, 9.3016e-02, 9.3260e-02, 9.3504e-02, 9.3749e-02, 9.4237e-02, 9.4725e-02, 9.5213e-02, 9.5702e-02, 9.6190e-02, 9.6678e-02, 9.7167e-02, 9.7655e-02, 9.8143e-02, 9.8631e-02, 9.9120e-02, 9.9608e-02, 1.0010e-01, 1.0058e-01, 1.0107e-01, 1.0156e-01, 1.0205e-01, 1.0254e-01, 1.0303e-01, 1.0351e-01, 1.0400e-01, 1.0449e-01, 1.0498e-01, 1.0547e-01, 1.0596e-01, 1.0644e-01, 1.0693e-01, 1.0742e-01, 1.0791e-01, 1.0840e-01, 1.0889e-01, 1.0937e-01, 1.0986e-01, 1.1035e-01, 1.1084e-01, 1.1133e-01, 1.1182e-01, 1.1230e-01, 1.1279e-01, 1.1328e-01, 1.1377e-01, 1.1426e-01, 1.1474e-01, 1.1523e-01, 1.1572e-01, 1.1621e-01, 1.1670e-01, 1.1719e-01, 1.1767e-01, 1.1816e-01, 1.1865e-01, 1.1914e-01, 1.1963e-01, 1.2012e-01, 1.2060e-01, 1.2109e-01, 1.2158e-01, 1.2207e-01, 1.2256e-01, 1.2305e-01, 1.2353e-01, 1.2402e-01, 1.2451e-01, 1.2500e-01, 1.2549e-01, 1.2598e-01, 1.2646e-01, 1.2695e-01, 1.2744e-01, 1.2793e-01, 1.2842e-01, 1.2890e-01, 1.2939e-01, 1.2988e-01, 1.3037e-01, 1.3086e-01, 1.3135e-01, 1.3183e-01, 1.3232e-01, 1.3281e-01, 1.3330e-01, 1.3379e-01, 1.3428e-01, 1.3476e-01, 1.3525e-01, 1.3574e-01, 1.3623e-01, 1.3672e-01, 1.3721e-01, 1.3769e-01, 1.3818e-01, 1.3867e-01, 1.3916e-01, 1.3965e-01, 1.4014e-01, 1.4062e-01, 1.4111e-01, 1.4160e-01, 1.4209e-01, 1.4258e-01, 1.4306e-01, 1.4355e-01, 1.4404e-01, 1.4453e-01, 1.4502e-01, 1.4551e-01, 1.4599e-01, 1.4648e-01, 1.4697e-01, 1.4746e-01, 1.4795e-01, 1.4844e-01, 1.4892e-01, 1.4941e-01, 1.4990e-01, 1.5039e-01, 1.5088e-01, 1.5137e-01, 1.5185e-01, 1.5234e-01, 1.5283e-01, 1.5332e-01, 1.5381e-01, 1.5430e-01, 1.5478e-01, 1.5527e-01, 1.5576e-01, 1.5625e-01, + 1.5674e-01, 1.5723e-01, 1.5771e-01, 1.5820e-01, 1.5869e-01, 1.5918e-01, 1.5967e-01, 1.6015e-01, 1.6064e-01, 1.6113e-01, 1.6162e-01, 1.6211e-01, 1.6260e-01, 1.6308e-01, 1.6357e-01, 1.6406e-01, 1.6455e-01, 1.6504e-01, 1.6553e-01, 1.6601e-01, 1.6650e-01, 1.6699e-01, 1.6748e-01, 1.6797e-01, 1.6846e-01, 1.6894e-01, 1.6943e-01, 1.6992e-01, 1.7041e-01, 1.7090e-01, 1.7139e-01, 1.7187e-01, 1.7236e-01, 1.7285e-01, 1.7334e-01, 1.7383e-01, 1.7431e-01, 1.7480e-01, 1.7529e-01, 1.7578e-01, 1.7627e-01, 1.7676e-01, 1.7724e-01, 1.7773e-01, 1.7822e-01, 1.7871e-01, 1.7920e-01, 1.7969e-01, 1.8017e-01, 1.8066e-01, 1.8115e-01, 1.8164e-01, 1.8213e-01, 1.8262e-01, 1.8310e-01, 1.8359e-01, 1.8408e-01, 1.8457e-01, 1.8506e-01, 1.8555e-01, 1.8603e-01, 1.8652e-01, 1.8701e-01, 1.8750e-01, 1.8848e-01, 1.8945e-01, 1.9043e-01, 1.9140e-01, 1.9238e-01, 1.9336e-01, 1.9433e-01, 1.9531e-01, 1.9629e-01, 1.9726e-01, 1.9824e-01, 1.9922e-01, 2.0019e-01, 2.0117e-01, 2.0215e-01, 2.0312e-01, 2.0410e-01, 2.0508e-01, 2.0605e-01, 2.0703e-01, 2.0801e-01, 2.0898e-01, 2.0996e-01, 2.1094e-01, 2.1191e-01, 2.1289e-01, 2.1387e-01, 2.1484e-01, 2.1582e-01, 2.1680e-01, 2.1777e-01, 2.1875e-01, 2.1972e-01, 2.2070e-01, 2.2168e-01, 2.2265e-01, 2.2363e-01, 2.2461e-01, 2.2558e-01, 2.2656e-01, 2.2754e-01, 2.2851e-01, 2.2949e-01, 2.3047e-01, 2.3144e-01, 2.3242e-01, 2.3340e-01, 2.3437e-01, 2.3535e-01, 2.3633e-01, 2.3730e-01, 2.3828e-01, 2.3926e-01, 2.4023e-01, 2.4121e-01, 2.4219e-01, 2.4316e-01, 2.4414e-01, 2.4512e-01, 2.4609e-01, 2.4707e-01, 2.4805e-01, 2.4902e-01, 2.5000e-01, 2.5097e-01, 2.5195e-01, 2.5293e-01, 2.5390e-01, 2.5488e-01, 2.5586e-01, 2.5683e-01, 2.5781e-01, 2.5879e-01, 2.5976e-01, 2.6074e-01, 2.6172e-01, 2.6269e-01, 2.6367e-01, 2.6465e-01, 2.6562e-01, 2.6660e-01, 2.6758e-01, 2.6855e-01, 2.6953e-01, 2.7051e-01, 2.7148e-01, 2.7246e-01, 2.7344e-01, 2.7441e-01, 2.7539e-01, 2.7637e-01, 2.7734e-01, 2.7832e-01, 2.7930e-01, 2.8027e-01, 2.8125e-01, 2.8222e-01, 2.8320e-01, 2.8418e-01, 2.8515e-01, 2.8613e-01, 2.8711e-01, 2.8808e-01, 2.8906e-01, 2.9004e-01, 2.9101e-01, 2.9199e-01, 2.9297e-01, 2.9394e-01, 2.9492e-01, 2.9590e-01, 2.9687e-01, 2.9785e-01, 2.9883e-01, 2.9980e-01, 3.0078e-01, 3.0176e-01, 3.0273e-01, 3.0371e-01, 3.0469e-01, 3.0566e-01, 3.0664e-01, 3.0762e-01, 3.0859e-01, 3.0957e-01, 3.1055e-01, 3.1152e-01, 3.1250e-01, 3.1347e-01, 3.1445e-01, 3.1543e-01, 3.1640e-01, 3.1738e-01, 3.1836e-01, 3.1933e-01, 3.2031e-01, 3.2129e-01, 3.2226e-01, 3.2324e-01, 3.2422e-01, 3.2519e-01, 3.2617e-01, 3.2715e-01, 3.2812e-01, 3.2910e-01, 3.3008e-01, 3.3105e-01, 3.3203e-01, 3.3301e-01, 3.3398e-01, 3.3496e-01, 3.3594e-01, 3.3691e-01, 3.3789e-01, 3.3887e-01, 3.3984e-01, 3.4082e-01, 3.4180e-01, 3.4277e-01, 3.4375e-01, 3.4472e-01, 3.4570e-01, 3.4668e-01, 3.4765e-01, 3.4863e-01, 3.4961e-01, 3.5058e-01, 3.5156e-01, 3.5254e-01, 3.5351e-01, 3.5449e-01, 3.5547e-01, 3.5644e-01, 3.5742e-01, 3.5840e-01, 3.5937e-01, 3.6035e-01, 3.6133e-01, 3.6230e-01, 3.6328e-01, 3.6426e-01, 3.6523e-01, 3.6621e-01, 3.6719e-01, 3.6816e-01, 3.6914e-01, 3.7012e-01, 3.7109e-01, 3.7207e-01, 3.7305e-01, 3.7402e-01, 3.7500e-01, + 3.7695e-01, 3.7890e-01, 3.8086e-01, 3.8281e-01, 3.8476e-01, 3.8672e-01, 3.8867e-01, 3.9062e-01, 3.9258e-01, 3.9453e-01, 3.9648e-01, 3.9844e-01, 4.0039e-01, 4.0234e-01, 4.0430e-01, 4.0625e-01, 4.0820e-01, 4.1015e-01, 4.1211e-01, 4.1406e-01, 4.1601e-01, 4.1797e-01, 4.1992e-01, 4.2187e-01, 4.2383e-01, 4.2578e-01, 4.2773e-01, 4.2969e-01, 4.3164e-01, 4.3359e-01, 4.3555e-01, 4.3750e-01, 4.3945e-01, 4.4140e-01, 4.4336e-01, 4.4531e-01, 4.4726e-01, 4.4922e-01, 4.5117e-01, 4.5312e-01, 4.5508e-01, 4.5703e-01, 4.5898e-01, 4.6094e-01, 4.6289e-01, 4.6484e-01, 4.6680e-01, 4.6875e-01, 4.7070e-01, 4.7265e-01, 4.7461e-01, 4.7656e-01, 4.7851e-01, 4.8047e-01, 4.8242e-01, 4.8437e-01, 4.8633e-01, 4.8828e-01, 4.9023e-01, 4.9219e-01, 4.9414e-01, 4.9609e-01, 4.9805e-01, 5.0000e-01, 5.0195e-01, 5.0390e-01, 5.0586e-01, 5.0781e-01, 5.0976e-01, 5.1172e-01, 5.1367e-01, 5.1562e-01, 5.1758e-01, 5.1953e-01, 5.2148e-01, 5.2344e-01, 5.2539e-01, 5.2734e-01, 5.2930e-01, 5.3125e-01, 5.3320e-01, 5.3515e-01, 5.3711e-01, 5.3906e-01, 5.4101e-01, 5.4297e-01, 5.4492e-01, 5.4687e-01, 5.4883e-01, 5.5078e-01, 5.5273e-01, 5.5469e-01, 5.5664e-01, 5.5859e-01, 5.6055e-01, 5.6250e-01, 5.6445e-01, 5.6640e-01, 5.6836e-01, 5.7031e-01, 5.7226e-01, 5.7422e-01, 5.7617e-01, 5.7812e-01, 5.8008e-01, 5.8203e-01, 5.8398e-01, 5.8594e-01, 5.8789e-01, 5.8984e-01, 5.9180e-01, 5.9375e-01, 5.9570e-01, 5.9765e-01, 5.9961e-01, 6.0156e-01, 6.0351e-01, 6.0547e-01, 6.0742e-01, 6.0937e-01, 6.1133e-01, 6.1328e-01, 6.1523e-01, 6.1719e-01, 6.1914e-01, 6.2109e-01, 6.2305e-01, 6.2500e-01, 6.2695e-01, 6.2890e-01, 6.3086e-01, 6.3281e-01, 6.3476e-01, 6.3672e-01, 6.3867e-01, 6.4062e-01, 6.4258e-01, 6.4453e-01, 6.4648e-01, 6.4844e-01, 6.5039e-01, 6.5234e-01, 6.5430e-01, 6.5625e-01, 6.5820e-01, 6.6015e-01, 6.6211e-01, 6.6406e-01, 6.6601e-01, 6.6797e-01, 6.6992e-01, 6.7187e-01, 6.7383e-01, 6.7578e-01, 6.7773e-01, 6.7969e-01, 6.8164e-01, 6.8359e-01, 6.8554e-01, 6.8750e-01, 6.8945e-01, 6.9140e-01, 6.9336e-01, 6.9531e-01, 6.9726e-01, 6.9922e-01, 7.0117e-01, 7.0312e-01, 7.0508e-01, 7.0703e-01, 7.0898e-01, 7.1094e-01, 7.1289e-01, 7.1484e-01, 7.1679e-01, 7.1875e-01, 7.2070e-01, 7.2265e-01, 7.2461e-01, 7.2656e-01, 7.2851e-01, 7.3047e-01, 7.3242e-01, 7.3437e-01, 7.3633e-01, 7.3828e-01, 7.4023e-01, 7.4219e-01, 7.4414e-01, 7.4609e-01, 7.4804e-01, 7.5000e-01, 7.5390e-01, 7.5781e-01, 7.6172e-01, 7.6562e-01, 7.6953e-01, 7.7344e-01, 7.7734e-01, 7.8125e-01, 7.8515e-01, 7.8906e-01, 7.9297e-01, 7.9687e-01, 8.0078e-01, 8.0469e-01, 8.0859e-01, 8.1250e-01, 8.1640e-01, 8.2031e-01, 8.2422e-01, 8.2812e-01, 8.3203e-01, 8.3594e-01, 8.3984e-01, 8.4375e-01, 8.4765e-01, 8.5156e-01, 8.5547e-01, 8.5937e-01, 8.6328e-01, 8.6719e-01, 8.7109e-01, 8.7500e-01, 8.7890e-01, 8.8281e-01, 8.8672e-01, 8.9062e-01, 8.9453e-01, 8.9844e-01, 9.0234e-01, 9.0625e-01, 9.1015e-01, 9.1406e-01, 9.1797e-01, 9.2187e-01, 9.2578e-01, 9.2969e-01, 9.3359e-01, 9.3750e-01, 9.4140e-01, 9.4531e-01, 9.4922e-01, 9.5312e-01, 9.5703e-01, 9.6094e-01, 9.6484e-01, 9.6875e-01, 9.7265e-01, 9.7656e-01, 9.8047e-01, 9.8437e-01, 9.8828e-01, 9.9219e-01, 9.9609e-01, 1.0000e+00 +}; + float4 val4_from_12(uchar8 pvs, float gain) { uint4 parsed = (uint4)(((uint)pvs.s0<<4) + (pvs.s1>>4), // is from the previous 10 bit ((uint)pvs.s2<<4) + (pvs.s4&0xF), ((uint)pvs.s3<<4) + (pvs.s4>>4), ((uint)pvs.s5<<4) + (pvs.s7&0xF)); + #if IS_OX + // PWL + //float4 pv = (convert_float4(parsed) - 64.0) / (4096.0 - 64.0); + float4 pv = {ox03c10_lut[parsed.s0], ox03c10_lut[parsed.s1], ox03c10_lut[parsed.s2], ox03c10_lut[parsed.s3]}; + + // it's a 24 bit signal, center in the middle 8 bits + return clamp(pv*gain*256.0, 0.0, 1.0); + #else // AR // normalize and scale float4 pv = (convert_float4(parsed) - 168.0) / (4096.0 - 168.0); return clamp(pv*gain, 0.0, 1.0); + #endif + } float get_k(float a, float b, float c, float d) { diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index 284347623..ab51059d9 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -1,48 +1,754 @@ struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; -struct i2c_random_wr_payload start_reg_array_imx390[] = {{0x0, 0}}; -struct i2c_random_wr_payload stop_reg_array_imx390[] = {{0x0, 1}}; +struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; +struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; -struct i2c_random_wr_payload init_array_imx390[] = { - {0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames - {0x200C, 0xe4}, {0x200D, 0x0c}, // MODE_HMAX +struct i2c_random_wr_payload init_array_ox03c10[] = { + {0x103, 1}, + {0x107, 1}, - // crop - {0x3410, 0x88}, {0x3411, 0x7}, // CROP_H_SIZE - {0x3418, 0xb8}, {0x3419, 0x4}, // CROP_V_SIZE - {0x0078, 1}, {0x03c0, 1}, + // X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200 - // external trigger (off) - // while images still come in, they are blank with this - {0x3650, 0}, // CU_MODE + // TPM + {0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf}, - // exposure - {0x000c, 0xc0}, {0x000d, 0x07}, - {0x0010, 0xc0}, {0x0011, 0x07}, + /*) + // group 4 + {0x3208, 0x04}, + {0x4620, 0x04}, + {0x3208, 0x14}, - // WUXGA mode - // not in datasheet, from https://github.com/bogsen/STLinux-Kernel/blob/master/drivers/media/platform/tegra/imx185.c - {0x0086, 0xc4}, {0x0087, 0xff}, // WND_SHIFT_V = -60 - {0x03c6, 0xc4}, {0x03c7, 0xff}, // SM_WND_SHIFT_V_APL = -60 + // group 5 + {0x3208, 0x05}, + {0x4620, 0x04}, + {0x3208, 0x15}, - {0x201c, 0xe1}, {0x201d, 0x12}, // image read amount - {0x21ee, 0xc4}, {0x21ef, 0x04}, // image send amount (1220 is the end) - {0x21f0, 0xc4}, {0x21f1, 0x04}, // image processing amount + // group 2 + {0x3208, 0x02}, + {0x3507, 0x00}, + {0x3208, 0x12}, - // disable a bunch of errors causing blanking - {0x0390, 0x00}, {0x0391, 0x00}, {0x0392, 0x00}, + // delay launch group 2 + {0x3208, 0xa2},*/ - // flip bayer - {0x2D64, 0x64 + 2}, + // PLL setup + {0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix + {0x0303, 0x01}, // pll1_prediv + {0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300 + {0x0306, 0x04}, // pll1_divmipi = 4 + {0x0307, 0x01}, // pll1_divm = 1 + {0x0316, 0x00}, + {0x0317, 0x00}, + {0x0318, 0x00}, + {0x0323, 0x05}, // pll2_prediv + {0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300 - // color correction - {0x0030, 0xf8}, {0x0031, 0x00}, // red gain - {0x0032, 0x9a}, {0x0033, 0x00}, // gr gain - {0x0034, 0x9a}, {0x0035, 0x00}, // gb gain - {0x0036, 0x22}, {0x0037, 0x01}, // blue gain + // SCLK/PCLK + {0x0400, 0xe0}, {0x0401, 0x80}, + {0x0403, 0xde}, {0x0404, 0x34}, + {0x0405, 0x3b}, {0x0406, 0xde}, + {0x0407, 0x08}, + {0x0408, 0xe0}, {0x0409, 0x7f}, + {0x040a, 0xde}, {0x040b, 0x34}, + {0x040c, 0x47}, {0x040d, 0xd8}, + {0x040e, 0x08}, - // hdr enable (noise with this on for now) - {0x00f9, 0} + // xchk + {0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79}, + + // SC ctrl + {0x3001, 0x03}, // io_pad_oen + {0x3002, 0xf8}, // io_pad_oen + {0x3005, 0x80}, // io_pad_out + {0x3007, 0x01}, // io_pad_sel + {0x3008, 0x80}, // io_pad_sel + + // FSIN first frame + /* + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x3822, 0x20}, + {0x3823, 0x58}, + + {0x3826, 0x0}, {0x3827, 0x8}, + {0x3881, 0x4}, + + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + */ + + // FSIN with external pulses + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x383E, 0x80}, + {0x3881, 0x4}, + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + + {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI + {0x3020, 0x05}, // SC_CTRL_20 + + // this is not in the datasheet, listed as RSVD + // but the camera doesn't work without it + {0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23}, + {0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d}, + {0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96}, + {0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73}, + {0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36}, + {0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21}, + {0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28}, + {0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c}, + {0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13}, + {0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02}, + {0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00}, + {0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e}, + {0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02}, + {0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00}, + {0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00}, + {0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00}, + {0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19}, + + // also RSVD + {0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01}, + {0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21}, + {0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3}, + {0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03}, + {0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80}, + {0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c}, + {0x3c73, 0x2c}, {0x3c76, 0x12}, + + // PEC checks + {0x3182, 0x12}, + + {0x320e, 0x00}, {0x320f, 0x00}, // RSVD + {0x3211, 0x61}, + {0x3215, 0xcd}, + {0x3219, 0x08}, + + {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure + {0x350a, 0x04}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain + + {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure + {0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain + + {0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure + {0x354a, 0x01}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain + + {0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure + {0x35ca, 0x01}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain + + // also RSVD + {0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01}, + {0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a}, + {0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b}, + {0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f}, + {0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00}, + {0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a}, + {0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10}, + {0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a}, + {0x3679, 0x19}, + + // Y_ADDR_START = 4 + {0x3802, 0x00}, {0x3803, 0x04}, + // Y_ADDR_END = 0x50b + {0x3806, 0x05}, {0x3807, 0x0b}, + + // X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928) + {0x3808, 0x07}, {0x3809, 0x88}, + + // Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208) + {0x380a, 0x04}, {0x380b, 0xb8}, + + // horizontal timing 0x447 + {0x380c, 0x04}, {0x380d, 0x47}, + + // rows per frame (was 0x2ae) + // 0x8ae = 53.65 ms + {0x380e, 0x08}, {0x380f, 0x15}, + // this should be triggered by FSIN, not free running + + {0x3810, 0x00}, {0x3811, 0x08}, // x cutoff + {0x3812, 0x00}, {0x3813, 0x04}, // y cutoff + {0x3816, 0x01}, + {0x3817, 0x01}, + {0x381c, 0x18}, + {0x381e, 0x01}, + {0x381f, 0x01}, + + // don't mirror, just flip + {0x3820, 0x04}, + + {0x3821, 0x19}, + {0x3832, 0x00}, + {0x3834, 0x00}, + {0x384c, 0x02}, + {0x384d, 0x0d}, + {0x3850, 0x00}, + {0x3851, 0x42}, + {0x3852, 0x00}, + {0x3853, 0x40}, + {0x3858, 0x04}, + {0x388c, 0x02}, + {0x388d, 0x2b}, + + // APC + {0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90}, + {0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20}, + {0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e}, + {0x3b4c, 0x00}, {0x3b4d, 0x00}, + {0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08}, + {0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80}, + {0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00}, + {0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09}, + + // OTP + {0x3d82, 0x73}, + {0x3d85, 0x05}, + {0x3d8a, 0x03}, + {0x3d8b, 0xff}, + {0x3d99, 0x00}, + {0x3d9a, 0x9f}, + {0x3d9b, 0x00}, + {0x3d9c, 0xa0}, + {0x3da4, 0x00}, + {0x3da7, 0x50}, + + // DTR + {0x420e, 0x6b}, + {0x420f, 0x6e}, + {0x4210, 0x06}, + {0x4211, 0xc1}, + {0x421e, 0x02}, + {0x421f, 0x45}, + {0x4220, 0xe1}, + {0x4221, 0x01}, + {0x4301, 0xff}, + {0x4307, 0x03}, + {0x4308, 0x13}, + {0x430a, 0x13}, + {0x430d, 0x93}, + {0x430f, 0x57}, + {0x4310, 0x95}, + {0x4311, 0x16}, + {0x4316, 0x00}, + + {0x4317, 0x38}, // both embedded rows are enabled + + {0x4319, 0x03}, // spd dcg + {0x431a, 0x00}, // 8 bit mipi + {0x431b, 0x00}, + {0x431d, 0x2a}, + {0x431e, 0x11}, + + {0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits + //{0x431f, 0x00}, // disable PWL + + {0x4320, 0x19}, + {0x4323, 0x80}, + {0x4324, 0x00}, + {0x4503, 0x4e}, + {0x4505, 0x00}, + {0x4509, 0x00}, + {0x450a, 0x00}, + {0x4580, 0xf8}, + {0x4583, 0x07}, + {0x4584, 0x6a}, + {0x4585, 0x08}, + {0x4586, 0x05}, + {0x4587, 0x04}, + {0x4588, 0x73}, + {0x4589, 0x05}, + {0x458a, 0x1f}, + {0x458b, 0x02}, + {0x458c, 0xdc}, + {0x458d, 0x03}, + {0x458e, 0x02}, + {0x4597, 0x07}, + {0x4598, 0x40}, + {0x4599, 0x0e}, + {0x459a, 0x0e}, + {0x459b, 0xfb}, + {0x459c, 0xf3}, + {0x4602, 0x00}, + {0x4603, 0x13}, + {0x4604, 0x00}, + {0x4609, 0x0a}, + {0x460a, 0x30}, + {0x4610, 0x00}, + {0x4611, 0x70}, + {0x4612, 0x01}, + {0x4613, 0x00}, + {0x4614, 0x00}, + {0x4615, 0x70}, + {0x4616, 0x01}, + {0x4617, 0x00}, + + {0x4800, 0x04}, // invert output PCLK + {0x480a, 0x22}, + {0x4813, 0xe4}, + + // mipi + {0x4814, 0x2a}, + {0x4837, 0x0d}, + {0x484b, 0x47}, + {0x484f, 0x00}, + {0x4887, 0x51}, + {0x4d00, 0x4a}, + {0x4d01, 0x18}, + {0x4d05, 0xff}, + {0x4d06, 0x88}, + {0x4d08, 0x63}, + {0x4d09, 0xdf}, + {0x4d15, 0x7d}, + {0x4d1a, 0x20}, + {0x4d30, 0x0a}, + {0x4d31, 0x00}, + {0x4d34, 0x7d}, + {0x4d3c, 0x7d}, + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x00}, + {0x4f03, 0x20}, + {0x4f04, 0xe0}, + {0x6a00, 0x00}, + {0x6a01, 0x20}, + {0x6a02, 0x00}, + {0x6a03, 0x20}, + {0x6a04, 0x02}, + {0x6a05, 0x80}, + {0x6a06, 0x01}, + {0x6a07, 0xe0}, + {0x6a08, 0xcf}, + {0x6a09, 0x01}, + {0x6a0a, 0x40}, + {0x6a20, 0x00}, + {0x6a21, 0x02}, + {0x6a22, 0x00}, + {0x6a23, 0x00}, + {0x6a24, 0x00}, + {0x6a25, 0x00}, + {0x6a26, 0x00}, + {0x6a27, 0x00}, + {0x6a28, 0x00}, + + // isp + {0x5000, 0x8f}, + {0x5001, 0x75}, + {0x5002, 0x7f}, // PWL0 + //{0x5002, 0x3f}, // PWL disable + {0x5003, 0x7a}, + + {0x5004, 0x3e}, + {0x5005, 0x1e}, + {0x5006, 0x1e}, + {0x5007, 0x1e}, + + {0x5008, 0x00}, + {0x500c, 0x00}, + {0x502c, 0x00}, + {0x502e, 0x00}, + {0x502f, 0x00}, + {0x504b, 0x00}, + {0x5053, 0x00}, + {0x505b, 0x00}, + {0x5063, 0x00}, + {0x5070, 0x00}, + {0x5074, 0x04}, + {0x507a, 0x04}, + {0x507b, 0x09}, + {0x5500, 0x02}, + {0x5700, 0x02}, + {0x5900, 0x02}, + {0x6007, 0x04}, + {0x6008, 0x05}, + {0x6009, 0x02}, + {0x600b, 0x08}, + {0x600c, 0x07}, + {0x600d, 0x88}, + {0x6016, 0x00}, + {0x6027, 0x04}, + {0x6028, 0x05}, + {0x6029, 0x02}, + {0x602b, 0x08}, + {0x602c, 0x07}, + {0x602d, 0x88}, + {0x6047, 0x04}, + {0x6048, 0x05}, + {0x6049, 0x02}, + {0x604b, 0x08}, + {0x604c, 0x07}, + {0x604d, 0x88}, + {0x6067, 0x04}, + {0x6068, 0x05}, + {0x6069, 0x02}, + {0x606b, 0x08}, + {0x606c, 0x07}, + {0x606d, 0x88}, + {0x6087, 0x04}, + {0x6088, 0x05}, + {0x6089, 0x02}, + {0x608b, 0x08}, + {0x608c, 0x07}, + {0x608d, 0x88}, + + // 12-bit PWL0 + {0x5e00, 0x00}, + + // m_ndX_exp[0:32] + // 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518 + {0x5e01, 0x09}, + {0x5e02, 0x09}, + {0x5e03, 0x0a}, + {0x5e04, 0x0a}, + {0x5e05, 0x0a}, + {0x5e06, 0x0b}, + {0x5e07, 0x0b}, + {0x5e08, 0x0c}, + {0x5e09, 0x0c}, + {0x5e0a, 0x0d}, + {0x5e0b, 0x0d}, + {0x5e0c, 0x0e}, + {0x5e0d, 0x0e}, + {0x5e0e, 0x0f}, + {0x5e0f, 0x0f}, + {0x5e10, 0x10}, + {0x5e11, 0x10}, + {0x5e12, 0x11}, + {0x5e13, 0x11}, + {0x5e14, 0x12}, + {0x5e15, 0x12}, + {0x5e16, 0x12}, + {0x5e17, 0x12}, + {0x5e18, 0x13}, + {0x5e19, 0x13}, + {0x5e1a, 0x13}, + {0x5e1b, 0x14}, + {0x5e1c, 0x14}, + {0x5e1d, 0x14}, + {0x5e1e, 0x15}, + {0x5e1f, 0x15}, + {0x5e20, 0x15}, + {0x5e21, 0x16}, + + // m_ndY_val[0:32] + // 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095 + {0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00}, + {0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff}, + {0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00}, + {0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00}, + {0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00}, + {0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80}, + {0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80}, + {0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80}, + {0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80}, + {0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80}, + {0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80}, + {0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80}, + {0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80}, + {0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80}, + {0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80}, + {0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80}, + {0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80}, + {0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40}, + {0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40}, + {0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40}, + {0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40}, + {0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40}, + {0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40}, + {0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40}, + {0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40}, + {0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40}, + {0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40}, + {0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40}, + {0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40}, + {0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40}, + {0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40}, + {0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40}, + {0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40}, + + // disable PWL + /*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00}, + {0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00}, + {0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00}, + {0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00}, + {0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00}, + {0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00}, + {0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00}, + {0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00}, + {0x5e21, 0x00}, + + {0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/ + + {0x4001, 0x2b}, // BLC_CTRL_1 + {0x4008, 0x02}, {0x4009, 0x03}, + {0x4018, 0x12}, + {0x4022, 0x40}, + {0x4023, 0x20}, + + // all black level targets are 0x40 + {0x4026, 0x00}, {0x4027, 0x40}, + {0x4028, 0x00}, {0x4029, 0x40}, + {0x402a, 0x00}, {0x402b, 0x40}, + {0x402c, 0x00}, {0x402d, 0x40}, + + {0x407e, 0xcc}, + {0x407f, 0x18}, + {0x4080, 0xff}, + {0x4081, 0xff}, + {0x4082, 0x01}, + {0x4083, 0x53}, + {0x4084, 0x01}, + {0x4085, 0x2b}, + {0x4086, 0x00}, + {0x4087, 0xb3}, + + {0x4640, 0x40}, + {0x4641, 0x11}, + {0x4642, 0x0e}, + {0x4643, 0xee}, + {0x4646, 0x0f}, + {0x4648, 0x00}, + {0x4649, 0x03}, + + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x80}, + {0x4f03, 0x2c}, + {0x4f04, 0xf8}, + + {0x4d09, 0xff}, + {0x4d09, 0xdf}, + + {0x5003, 0x7a}, + {0x5b80, 0x08}, + {0x5c00, 0x08}, + {0x5c80, 0x00}, + {0x5bbe, 0x12}, + {0x5c3e, 0x12}, + {0x5cbe, 0x12}, + {0x5b8a, 0x80}, + {0x5b8b, 0x80}, + {0x5b8c, 0x80}, + {0x5b8d, 0x80}, + {0x5b8e, 0x60}, + {0x5b8f, 0x80}, + {0x5b90, 0x80}, + {0x5b91, 0x80}, + {0x5b92, 0x80}, + {0x5b93, 0x20}, + {0x5b94, 0x80}, + {0x5b95, 0x80}, + {0x5b96, 0x80}, + {0x5b97, 0x20}, + {0x5b98, 0x00}, + {0x5b99, 0x80}, + {0x5b9a, 0x40}, + {0x5b9b, 0x20}, + {0x5b9c, 0x00}, + {0x5b9d, 0x00}, + {0x5b9e, 0x80}, + {0x5b9f, 0x00}, + {0x5ba0, 0x00}, + {0x5ba1, 0x00}, + {0x5ba2, 0x00}, + {0x5ba3, 0x00}, + {0x5ba4, 0x00}, + {0x5ba5, 0x00}, + {0x5ba6, 0x00}, + {0x5ba7, 0x00}, + {0x5ba8, 0x02}, + {0x5ba9, 0x00}, + {0x5baa, 0x02}, + {0x5bab, 0x76}, + {0x5bac, 0x03}, + {0x5bad, 0x08}, + {0x5bae, 0x00}, + {0x5baf, 0x80}, + {0x5bb0, 0x00}, + {0x5bb1, 0xc0}, + {0x5bb2, 0x01}, + {0x5bb3, 0x00}, + + // m_nNormCombineWeight + {0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60}, + {0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20}, + {0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00}, + {0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00}, + {0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00}, + {0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00}, + + // m_nCombinThreL + {0x5c28, 0x02}, {0x5c29, 0x00}, + {0x5c2a, 0x02}, {0x5c2b, 0x76}, + {0x5c2c, 0x03}, {0x5c2d, 0x08}, + + // m_nCombinThreS + {0x5c2e, 0x00}, {0x5c2f, 0x80}, + {0x5c30, 0x00}, {0x5c31, 0xc0}, + {0x5c32, 0x01}, {0x5c33, 0x00}, + + // m_nNormCombineWeight + {0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80}, + {0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60}, + {0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40}, + {0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00}, + {0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00}, + {0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00}, + + {0x5ca8, 0x01}, {0x5ca9, 0x00}, + {0x5caa, 0x02}, {0x5cab, 0x00}, + {0x5cac, 0x03}, {0x5cad, 0x08}, + + {0x5cae, 0x01}, {0x5caf, 0x00}, + {0x5cb0, 0x02}, {0x5cb1, 0x00}, + {0x5cb2, 0x03}, {0x5cb3, 0x08}, + + // combine ISP + {0x5be7, 0x80}, + {0x5bc9, 0x80}, + {0x5bca, 0x80}, + {0x5bcb, 0x80}, + {0x5bcc, 0x80}, + {0x5bcd, 0x80}, + {0x5bce, 0x80}, + {0x5bcf, 0x80}, + {0x5bd0, 0x80}, + {0x5bd1, 0x80}, + {0x5bd2, 0x20}, + {0x5bd3, 0x80}, + {0x5bd4, 0x40}, + {0x5bd5, 0x20}, + {0x5bd6, 0x00}, + {0x5bd7, 0x00}, + {0x5bd8, 0x00}, + {0x5bd9, 0x00}, + {0x5bda, 0x00}, + {0x5bdb, 0x00}, + {0x5bdc, 0x00}, + {0x5bdd, 0x00}, + {0x5bde, 0x00}, + {0x5bdf, 0x00}, + {0x5be0, 0x00}, + {0x5be1, 0x00}, + {0x5be2, 0x00}, + {0x5be3, 0x00}, + {0x5be4, 0x00}, + {0x5be5, 0x00}, + {0x5be6, 0x00}, + + // m_nSPDCombineWeight + {0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40}, + {0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20}, + {0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00}, + {0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00}, + {0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00}, + {0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00}, + + // m_nSPDCombineWeight + {0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80}, + {0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60}, + {0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40}, + {0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20}, + {0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00}, + {0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00}, + + {0x5d74, 0x01}, + {0x5d75, 0x00}, + + {0x5d1f, 0x81}, + {0x5d11, 0x00}, + {0x5d12, 0x10}, + {0x5d13, 0x10}, + {0x5d15, 0x05}, + {0x5d16, 0x05}, + {0x5d17, 0x05}, + {0x5d08, 0x03}, + {0x5d09, 0xb6}, + {0x5d0a, 0x03}, + {0x5d0b, 0xb6}, + {0x5d18, 0x03}, + {0x5d19, 0xb6}, + {0x5d62, 0x01}, + {0x5d40, 0x02}, + {0x5d41, 0x01}, + {0x5d63, 0x1f}, + {0x5d64, 0x00}, + {0x5d65, 0x80}, + {0x5d56, 0x00}, + {0x5d57, 0x20}, + {0x5d58, 0x00}, + {0x5d59, 0x20}, + {0x5d5a, 0x00}, + {0x5d5b, 0x0c}, + {0x5d5c, 0x02}, + {0x5d5d, 0x40}, + {0x5d5e, 0x02}, + {0x5d5f, 0x40}, + {0x5d60, 0x03}, + {0x5d61, 0x40}, + {0x5d4a, 0x02}, + {0x5d4b, 0x40}, + {0x5d4c, 0x02}, + {0x5d4d, 0x40}, + {0x5d4e, 0x02}, + {0x5d4f, 0x40}, + {0x5d50, 0x18}, + {0x5d51, 0x80}, + {0x5d52, 0x18}, + {0x5d53, 0x80}, + {0x5d54, 0x18}, + {0x5d55, 0x80}, + {0x5d46, 0x20}, + {0x5d47, 0x00}, + {0x5d48, 0x22}, + {0x5d49, 0x00}, + {0x5d42, 0x20}, + {0x5d43, 0x00}, + {0x5d44, 0x22}, + {0x5d45, 0x00}, + + {0x5004, 0x1e}, + {0x4221, 0x03}, // this is changed from 1 -> 3 + + // DCG exposure coarse + {0x3501, 0x01}, {0x3502, 0xc8}, + // SPD exposure coarse + {0x3541, 0x01}, {0x3542, 0xc8}, + // VS exposure coarse + {0x35c1, 0x00}, {0x35c2, 0x01}, + + // crc reference + {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55}, + // crc stat check + {0x507a, 0x5f}, {0x507b, 0x46}, + + // watchdog control + {0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c}, + + // color balance gains + // blue + {0x5280, 0x06}, {0x5281, 0xCB}, // hcg + {0x5480, 0x06}, {0x5481, 0xCB}, // lcg + {0x5680, 0x06}, {0x5681, 0xCB}, // spd + {0x5880, 0x06}, {0x5881, 0xCB}, // vs + + // green(blue) + {0x5282, 0x04}, {0x5283, 0x00}, + {0x5482, 0x04}, {0x5483, 0x00}, + {0x5682, 0x04}, {0x5683, 0x00}, + {0x5882, 0x04}, {0x5883, 0x00}, + + // green(red) + {0x5284, 0x04}, {0x5285, 0x00}, + {0x5484, 0x04}, {0x5485, 0x00}, + {0x5684, 0x04}, {0x5685, 0x00}, + {0x5884, 0x04}, {0x5885, 0x00}, + + // red + {0x5286, 0x08}, {0x5287, 0xDE}, + {0x5486, 0x08}, {0x5487, 0xDE}, + {0x5686, 0x08}, {0x5687, 0xDE}, + {0x5886, 0x08}, {0x5887, 0xDE}, }; struct i2c_random_wr_payload init_array_ar0231[] = { diff --git a/system/camerad/main.cc b/system/camerad/main.cc index c1f38f222..35a3329f3 100644 --- a/system/camerad/main.cc +++ b/system/camerad/main.cc @@ -7,14 +7,17 @@ #include "system/hardware/hw.h" int main(int argc, char *argv[]) { - if (!Hardware::PC()) { - int ret; - ret = util::set_realtime_priority(53); - assert(ret == 0); - ret = util::set_core_affinity({6}); - assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores + if (Hardware::PC()) { + printf("camerad is not meant to run on PC\n"); + return 0; } + int ret; + ret = util::set_realtime_priority(53); + assert(ret == 0); + ret = util::set_core_affinity({6}); + assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores + camerad_thread(); return 0; } diff --git a/system/camerad/snapshot/snapshot.py b/system/camerad/snapshot/snapshot.py index 946c220a7..48dfc9e02 100755 --- a/system/camerad/snapshot/snapshot.py +++ b/system/camerad/snapshot/snapshot.py @@ -91,7 +91,7 @@ def snapshot(): subprocess.check_call(["pgrep", "camerad"]) print("Camerad already running") params.put_bool("IsTakingSnapshot", False) - params.delete("Offroad_IsTakingSnapshot") + params.remove("Offroad_IsTakingSnapshot") return None, None except subprocess.CalledProcessError: pass diff --git a/system/hardware/base.py b/system/hardware/base.py index c9c02f3a5..31df1babe 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -78,10 +78,6 @@ class HardwareBase(ABC): def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None: pass - @abstractmethod - def get_usb_present(self): - pass - @abstractmethod def get_current_power_draw(self): pass diff --git a/system/hardware/pc/hardware.py b/system/hardware/pc/hardware.py index 60d14e4a6..564f9e483 100644 --- a/system/hardware/pc/hardware.py +++ b/system/hardware/pc/hardware.py @@ -50,12 +50,9 @@ class Pc(HardwareBase): def get_network_strength(self, network_type): return NetworkStrength.unknown - def get_usb_present(self): - return False - def get_current_power_draw(self): return 0 - + def get_som_power_draw(self): return 0 diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 7ccea95ee..7876b1af1 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -1,9 +1,9 @@ [ { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-243ddbb9e2256aa7af7fed0daf8cff4017a3c838c759373a634b8539f271bfb8.img.xz", - "hash": "243ddbb9e2256aa7af7fed0daf8cff4017a3c838c759373a634b8539f271bfb8", - "hash_raw": "243ddbb9e2256aa7af7fed0daf8cff4017a3c838c759373a634b8539f271bfb8", + "url": "https://commadist.azureedge.net/agnosupdate/boot-72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a.img.xz", + "hash": "72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a", + "hash_raw": "72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a", "size": 14780416, "sparse": false, "full_check": true, @@ -41,9 +41,9 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-59622eddd068d49f2e9df69ef5115e3f205ad369539690a5b240c8c93796dd13.img.xz", - "hash": "44da205d17b44b2be7c94854a6bb3efb2928ec9a9889fe62af8b322d2295b74f", - "hash_raw": "59622eddd068d49f2e9df69ef5115e3f205ad369539690a5b240c8c93796dd13", + "url": "https://commadist.azureedge.net/agnosupdate/system-9efdc9368f05e06008a7a1dbbee21b564e89988dc94d6ddee3a3a88e42268f0e.img.xz", + "hash": "48209ce7e8cc2fff4ec024f0cd82fc2e3e097b5c0629be2b292acf64e6701449", + "hash_raw": "9efdc9368f05e06008a7a1dbbee21b564e89988dc94d6ddee3a3a88e42268f0e", "size": 10737418240, "sparse": true, "full_check": false, diff --git a/system/hardware/tici/agnos.py b/system/hardware/tici/agnos.py index ca2498a00..5f446a8e9 100755 --- a/system/hardware/tici/agnos.py +++ b/system/hardware/tici/agnos.py @@ -20,7 +20,7 @@ class StreamingDecompressor: def __init__(self, url: str) -> None: self.buf = b"" - self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}) # type: ignore + self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}, timeout=60) # type: ignore self.it = self.req.iter_content(chunk_size=1024 * 1024) self.decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO) self.eof = False @@ -241,7 +241,7 @@ def flash_partition(target_slot_number: int, partition: dict, cloudlog, standalo else: extract_compressed_image(target_slot_number, partition, cloudlog) - # Write hash after successfull flash + # Write hash after successful flash if not full_check: with open(path, 'wb+') as out: out.seek(partition['size']) @@ -257,7 +257,7 @@ def swap(manifest_path: str, target_slot_number: int, cloudlog) -> None: while True: out = subprocess.check_output(f"abctl --set_active {target_slot_number}", shell=True, stderr=subprocess.STDOUT, encoding='utf8') if ("No such file or directory" not in out) and ("lun as boot lun" in out): - cloudlog.info(f"Swap successfull {out}") + cloudlog.info(f"Swap successful {out}") break else: cloudlog.error(f"Swap failed {out}") diff --git a/system/hardware/tici/casync.py b/system/hardware/tici/casync.py index 8ae42fa71..993336616 100755 --- a/system/hardware/tici/casync.py +++ b/system/hardware/tici/casync.py @@ -86,6 +86,7 @@ class RemoteChunkReader(ChunkReader): def parse_caibx(caibx_path: str) -> List[Chunk]: """Parses the chunks from a caibx file. Can handle both local and remote files. Returns a list of chunks with hash, offset and length""" + caibx: io.BufferedIOBase if os.path.isfile(caibx_path): caibx = open(caibx_path, 'rb') else: diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index c3f8c7f82..e2fd20c1b 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -287,7 +287,7 @@ class Tici(HardwareBase): ] upload = [ - # Create root Hierarchy Token Bucket that sends all trafic to 1:20 + # Create root Hierarchy Token Bucket that sends all traffic to 1:20 (True, tc + ["qdisc", "add", "dev", adapter, "root", "handle", "1:", "htb", "default", "20"]), # Create class 1:20 with specified rate limit @@ -372,7 +372,6 @@ class Tici(HardwareBase): return (self.read_param_file("/sys/class/power_supply/bms/voltage_now", int) * self.read_param_file("/sys/class/power_supply/bms/current_now", int) / 1e12) def shutdown(self): - # Note that for this to work and have the device stay powered off, the panda needs to be in UsbPowerMode::CLIENT! os.system("sudo poweroff") def get_thermal_config(self): @@ -481,7 +480,7 @@ class Tici(HardwareBase): # blue prime config if sim_id.startswith('8901410'): - os.system('mmcli -m 0 --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') + os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') def get_networks(self): r = {} diff --git a/system/hardware/tici/pins.py b/system/hardware/tici/pins.py index 61e528d30..fe31b9311 100644 --- a/system/hardware/tici/pins.py +++ b/system/hardware/tici/pins.py @@ -19,3 +19,9 @@ class GPIO: CAM0_RSTN = 9 CAM1_RSTN = 7 CAM2_RSTN = 12 + + # Sensor interrupts + BMX055_ACCEL_INT = 21 + BMX055_GYRO_INT = 23 + BMX055_MAGN_INT = 87 + LSM_INT = 84 diff --git a/system/proclogd/proclog.cc b/system/proclogd/proclog.cc index 9064547d2..09ab4f559 100644 --- a/system/proclogd/proclog.cc +++ b/system/proclogd/proclog.cc @@ -72,7 +72,7 @@ std::optional procStat(std::string stat) { } std::string name = stat.substr(open_paren + 1, close_paren - open_paren - 1); - // repace space in name with _ + // replace space in name with _ std::replace(&stat[open_paren], &stat[close_paren], ' ', '_'); std::istringstream iss(stat); std::vector v{std::istream_iterator(iss), @@ -95,7 +95,7 @@ std::optional procStat(std::string stat) { .num_threads = stol(v[StatPos::num_threads - 1]), .starttime = stoull(v[StatPos::starttime - 1]), .vms = stoul(v[StatPos::vsize - 1]), - .rss = stoul(v[StatPos::rss - 1]), + .rss = stol(v[StatPos::rss - 1]), .processor = stoi(v[StatPos::processor - 1]), }; return p; diff --git a/system/proclogd/proclog.h b/system/proclogd/proclog.h index 9ed53d1ba..49f97cdd3 100644 --- a/system/proclogd/proclog.h +++ b/system/proclogd/proclog.h @@ -20,8 +20,8 @@ struct ProcCache { struct ProcStat { int pid, ppid, processor; char state; - long cutime, cstime, priority, nice, num_threads; - unsigned long utime, stime, vms, rss; + long cutime, cstime, priority, nice, num_threads, rss; + unsigned long utime, stime, vms; unsigned long long starttime; std::string name; }; diff --git a/third_party/SConscript b/third_party/SConscript index 5a7a62ae5..e5bbfaa07 100644 --- a/third_party/SConscript +++ b/third_party/SConscript @@ -1,6 +1,6 @@ Import('env') -env.Library('json11', ['json11/json11.cpp']) +env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unqualified-std-cast-call']) env.Append(CPPPATH=[Dir('json11')]) env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) diff --git a/tinygrad_repo/accel/opencl/conv.cl b/tinygrad_repo/accel/opencl/conv.cl new file mode 100644 index 000000000..c92a78ac3 --- /dev/null +++ b/tinygrad_repo/accel/opencl/conv.cl @@ -0,0 +1,154 @@ +//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 new file mode 100644 index 000000000..4f878e022 --- /dev/null +++ b/tinygrad_repo/accel/opencl/matmul.cl @@ -0,0 +1,49 @@ +//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 new file mode 100644 index 000000000..7c684ca21 --- /dev/null +++ b/tinygrad_repo/accel/opencl/ops_opencl.py @@ -0,0 +1,337 @@ +# 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 +from tinygrad.helpers import prod, ConvArgs +from typing import List, Tuple, Optional, Dict, Set +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)" + } + 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_op()), ("weight", w.contiguous_op())], "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);" + #ewtypes.append(f"read_only image2d_t {name}_g") + return super().contiguous_view_constant_fold(name, reduce) + + 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"): + if C is None or earlycode != "acc": + # TODO: handle an opencl conv without the conv part + return super()._processing_op(bufs, code, C, op, reduce_shape, earlybufs, earlycode) + 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_op(), w.contiguous_op() + 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)) + ) + 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)) + ) + 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 new file mode 100644 index 000000000..120b2ab0c --- /dev/null +++ b/tinygrad_repo/accel/opencl/preprocessing.py @@ -0,0 +1,86 @@ +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): + 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_op() + w = w.contiguous_op() + + # 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 + w.realize().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 new file mode 100644 index 000000000..b5d28a9ed --- /dev/null +++ b/tinygrad_repo/extra/onnx.py @@ -0,0 +1,178 @@ +import os +import numpy as np +import functools +from onnx.mapping import TENSOR_TYPE_TO_NP_TYPE +from tinygrad.tensor import Tensor +from tinygrad.helpers import prod +from tinygrad.nn import batch_normalize +from tinygrad.ops import DEBUG + +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) + 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]) + elif a.type == 4: return buffer_parse(a.t) # TENSOR + 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}") + def attribute_to_dict(a): return {x.name:attribute_parse(x) for x in a} + + tensors = {} + + # get weights and biases + for inp in onnx_model.graph.initializer: + if len(inp.raw_data) > 0: + tensors[inp.name] = buffer_parse(inp) + elif len(inp.float_data) > 0: + tensors[inp.name] = Tensor(np.array(inp.float_data, dtype=np.float32).reshape(inp.dims), requires_grad=False) + elif len(inp.int64_data) > 0: + tensors[inp.name] = Tensor(np.array(inp.int64_data, dtype=np.float32).reshape(inp.dims), requires_grad=False) + else: + print(inp.name, inp.dims, inp.data_type, len(inp.raw_data)) + print(inp) + raise Exception("no data") + if DEBUG >= 1: + print("realize", inp.name) + tensors[inp.name].realize() + + # preparse the attributes + 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] + + def run_onnx(inputs={}, debug=False): + input_tensors = {} + intermediate_tensors = {} + + # get inputs + 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 inp.name in inputs: + input_shape = inputs[inp.name].shape + 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] + else: + input_tensors[inp.name] = Tensor(inputs[inp.name], requires_grad=False) + for _,v in input_tensors.items(): v.realize() + else: + 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] + opt = attribute_dict[num] + + # 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 == "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 == "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 == "Gather": + # TODO: is this correct? seems to work for simple gather ops + axis = opt['axis'] + shape = list(inp[0].shape) + indices = [shape[axis]+int(x) if x<0 else int(x) for x in safe_numpy(inp[1])] + 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 = batch_normalize(inp[0], 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"]: + # 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) + # TODO: is this right? + if 'broadcast' in opt: inp[1] = inp[1].reshape([-1 if i == opt['broadcast'] else 1 for i in range(len(inp[0].shape))]) + 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] + elif n.op_type == "Split": + i = 0 + arg = [(0,x) for x in inp[0].shape] + for o,s in zip(n.output, opt['split']): + arg[opt['axis']] = (i,i+s) + 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 + arg = [(0,x) for x in inp[0].shape] + starts, ends, axes = inp[1:4] + assert axes.shape == (1,) + axis, starts, ends = int(safe_numpy(axes)[0]), int(safe_numpy(starts)[0]), int(safe_numpy(ends)[0]) + ends = min(ends, inp[0].shape[axis]) + starts = starts + inp[0].shape[axis] if starts < 0 else starts + arg[axis] = (starts, ends) + ret = inp[0].slice(arg=arg) + 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()) + + return {outp.name:intermediate_tensors[outp.name] for outp in onnx_model.graph.output} + return run_onnx diff --git a/tinygrad_repo/extra/thneed.py b/tinygrad_repo/extra/thneed.py new file mode 100644 index 000000000..58d4124ae --- /dev/null +++ b/tinygrad_repo/extra/thneed.py @@ -0,0 +1,319 @@ +# 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 collections import defaultdict +import pyopencl as cl + +DEBUGCL = int(os.getenv("DEBUGCL", 0)) +FLOAT16 = int(os.getenv("FLOAT16", 0)) + +class Thneed: + def __init__(self, cl_cache=[], inputs={}): + self.cl_cache, self.inputs = cl_cache[:], inputs + self.gobj = 0 + + # build graph + nodes = defaultdict(lambda: {'in_edges': [], 'out_edges': []}) + for _, args in self.cl_cache: + # output is always the first parameter + for a in args[3:]: + nodes[a]['out_edges'].append(args[2]) + nodes[args[2]]['in_edges'].append(a) + + # get buffers to save + self.buffers_to_save = set() + self.outputs = [] + for n in nodes.keys(): + if len(nodes[n]['in_edges']) == 0: + self.buffers_to_save.add(n) + if len(nodes[n]['out_edges']) == 0: + self.outputs.append(n) + + for n in self.inputs.values(): + assert n in self.buffers_to_save, f"{n} was not an input" + self.buffers_to_save.remove(n) + + def load(self, input_fn): + float32 = not FLOAT16 + + mf = cl.mem_flags + image_fmt = cl.ImageFormat(cl.channel_order.RGBA, cl.channel_type.FLOAT if float32 else cl.channel_type.HALF_FLOAT) + image_fmt_32 = cl.ImageFormat(cl.channel_order.RGBA, cl.channel_type.FLOAT) + + with open(input_fn, "rb") as f: + json_len = struct.unpack("I", f.read(4))[0] + jdat = json.loads(f.read(json_len).decode('latin_1')) + weights = f.read() + + # load in the buffers + bufs = {'\x00\x00\x00\x00\x00\x00\x00\x00': None} + bufs_loaded = {} + ptr = 0 + for o in jdat['objects']: + #print(o) + if o['needs_load']: + nptr = ptr + o['size'] + o['data'] = weights[ptr:nptr] + ptr = nptr + + if o['arg_type'] == "image2d_t" or o['arg_type'] == "image1d_t": + tfmt = image_fmt_32 if 'float32' in o and o['float32'] else image_fmt + 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']]) + 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, + 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, + 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'])) + 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']]) + else: + if 'data' in o: + 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']) + + bufs[o['id']] = buf + bufs_loaded[o['id']] = 'data' in o + # if it's loaded, it's saved + if 'data' in o: + self.buffers_to_save.add(buf) + + # load in the programs (this isn't used) + prgs = {} + for k,v in jdat['programs'].items(): + print("building", k) + try: + prgs[k] = CLProgram(k, v, rename=False) + except Exception: + print("FAILED", k) + traceback.print_exc() + exit(0) + + # load binaries + for o in jdat['binaries']: + nptr = ptr + o['length'] + prgs[o['name']] = CLProgram(o['name'], weights[ptr:nptr], rename=False, binary=True) + ptr = nptr + + # populate the cl_cache + for i,k in enumerate(jdat['kernels']): + kernel = prgs[k['name']] + aaa = [] + for j,(a,sz) in enumerate(zip(k['args'], k['args_size'])): + if len(a) == 0: + aa = cl.LocalMemory(sz) + elif len(a) == 4: + a = a.encode('latin_1') + aa = np.uint32(struct.unpack("I", a)[0]) + elif len(a) == 2: + a = a.encode('latin_1') + aa = np.uint16(struct.unpack("H", a)[0]) + elif len(a) == 8: + #print(i,j,struct.unpack("Q", a.encode('latin_1'))[0]) + aa = bufs[a] + aaa.append(aa) + self.cl_cache.append((kernel, [k['global_work_size'], k['local_work_size'], *aaa])) + + # load inputs + for k in jdat['inputs']: + self.inputs[k['name']] = bufs[k['buffer_id']] + + # load outputs + for k in jdat['outputs']: + self.outputs.append(bufs[k['buffer_id']]) + + + def save(self, output_fn): + # this is the struct that will be saved + jdat = {"binaries": [], "programs": {}, "kernels": [], "objects": []} + + # build the pieces of this struct + weights = [] + binaries = [] + saved_objs = set() + saved_binaries = set() + for prg, args in self.cl_cache: + # get binaries for saving + if prg.name not in saved_binaries: + binary = prg.clprogram.get_info(cl.program_info.BINARIES) + assert len(binary) == 1 + jdat['binaries'].append({"name":prg.name, "length":len(binary[0])}) + binaries.append(binary[0]) + saved_binaries.add(prg.name) + + # get the args from the kernel, some need the data saved + targs, args_size = [], [] + argdtypes = prg.argdtypes if prg.argdtypes is not None else [None]*(len(args)-2) + for a,d in zip(args[2:], argdtypes): + if d == np.int16: + targs.append(struct.pack("H", a).decode("latin_1")) + args_size.append(2) + elif d == np.int32: + targs.append(struct.pack("I", a).decode("latin_1")) + args_size.append(4) + elif isinstance(a, cl.LocalMemory): + targs.append("") + args_size.append(a.size) + elif d is None: + if getattr(a, "global_id", None) is None: + setattr(a, "global_id", self.gobj) + self.gobj += 1 + ptr = struct.pack("Q", a.global_id).decode("latin_1") + if ptr not in saved_objs: + if isinstance(a, cl.Buffer): + needs_load = a in self.buffers_to_save + jdat['objects'].append({ + "id": ptr, "arg_type": "float*", "needs_load": needs_load, "size": a.size, + }) + if needs_load: + data = np.empty(a.size//4, dtype=np.float32) + 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)) + + # zero out the buffer + 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) { + 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); + out[l.y*row_pitch + l.x] = read_imagef(in, smp, l); + } + """, argdtypes=(None, None, np.int32))(a.shape, None, a, buf, row_pitch//(4*(2 if FLOAT16 else 4))) + + # multiple of 32 isn't enough + jdat['objects'].append({ + "id": ptr, "needs_load": needs_load, "size": size, "arg_type": "image2d_t", + "width": a.shape[0], "height": a.shape[1], "row_pitch": row_pitch, "float32": not FLOAT16, + }) + + 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) + if FLOAT16: data = data.astype(np.float16) + weights.append(data.tobytes()) + else: + raise Exception("unknown object", a) + #print(jdat['objects'][-1]) + saved_objs.add(ptr) + targs.append(ptr) + args_size.append(8) + else: + raise Exception("idk this type") + + # save the kernel itself + jdat['kernels'].append({ + "name": prg.name, + "work_dim": len(args[0]), + "global_work_size": args[0], + # TODO: C++ thneed requires a local_work_size, so we fill it with ones + "local_work_size": [1 for _ in args[0]] if args[1] is None else args[1], + "num_args": len(args)-2, + "args": targs, + "args_size": args_size + }) + + jdat['outputs'] = [{ + "buffer_id": struct.pack("Q", x.global_id).decode("latin_1"), + "size": x.size, + } for x in self.outputs] + + jdat['inputs'] = [{ + "buffer_id": struct.pack("Q", v.global_id).decode("latin_1"), + "size": v.size, + "name": k + } for k,v in self.inputs.items()][::-1] + + print(f"saving thneed to {output_fn}") + with open(output_fn, "wb") as f: + j = json.dumps(jdat, ensure_ascii=False).encode('latin_1') + f.write(struct.pack("I", len(j))) + f.write(j) + f.write(b''.join(weights)) + f.write(b''.join(binaries)) + + def run(self): + events = [] + st = time.monotonic() + for prg, args in self.cl_cache: + events.append(prg.clprg(CL().cl_queue, *args)) + mt = time.monotonic() + CL().cl_queue.finish() + et = time.monotonic() + print(f"submit in {(mt-st)*1000.0:.2f} ms, total runtime is {(et-st)*1000.0:.2f} ms") + + if DEBUGCL: + total_runtime = 0 + for i, ((prg, args), e) in enumerate(zip(self.cl_cache, events)): + runtime = (e.profile.end - e.profile.start) + 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.options}") + total_runtime += runtime + print(f"total runtime: {total_runtime/1e6:.2f} ms") + + 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: + args = list(args) + if args[1] is None and len(args[0]) == 2: + args[1] = [min(MAX_WORKGROUP, args[0][0]), 1] + try: + e = prg.clprg(CL().cl_queue, *args) + except cl.LogicError: + # INVALID_WORK_GROUP_SIZE + args[1] = None + + if args[1] is None and len(args[0]) == 3: + runtimes = [] + 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 + local_args = (l1, l2, l3) + if prod(local_args) > MAX_WORKGROUP: continue + args[1] = local_args + 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)) + #print(sorted(runtimes)[0:5]) + 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 new file mode 100644 index 000000000..23f1e9030 --- /dev/null +++ b/tinygrad_repo/extra/utils.py @@ -0,0 +1,134 @@ +from tinygrad.tensor import Tensor +import pickle +import numpy as np +from tinygrad.helpers import prod + +def fetch(url): + if url.startswith("/"): + with open(url, "rb") as f: + dat = f.read() + return dat + import requests, 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 + +from tinygrad.nn.optim import get_parameters + +def my_unpickle(fb0): + key_prelookup = {} + class HackTensor: + def __new__(cls, *args): + #print(args) + ident, storage_type, obj_key, location, obj_size = args[0][0:5] + assert ident == 'storage' + + assert prod(args[2]) == obj_size + ret = np.zeros(args[2], dtype=storage_type) + key_prelookup[obj_key] = (storage_type, obj_size, ret, args[2], args[3]) + return ret + + class HackParameter: + def __new__(cls, *args): + #print(args) + pass + + class Dummy: + pass + + class MyPickle(pickle.Unpickler): + def find_class(self, module, name): + #print(module, name) + if name == 'FloatStorage': + return np.float32 + if name == 'LongStorage': + return np.int64 + if name == 'HalfStorage': + return np.float16 + if module == "torch._utils": + if name == "_rebuild_tensor_v2": + return HackTensor + elif name == "_rebuild_parameter": + return HackParameter + else: + try: + return pickle.Unpickler.find_class(self, module, name) + except Exception: + return Dummy + + def persistent_load(self, pid): + return pid + + return MyPickle(fb0).load(), key_prelookup + +def fake_torch_load_zipped(fb0, load_weights=True): + import zipfile + with zipfile.ZipFile(fb0, 'r') as myzip: + with myzip.open('archive/data.pkl') as myfile: + ret = my_unpickle(myfile) + if load_weights: + for k,v in ret[1].items(): + with myzip.open(f'archive/data/{k}') as myfile: + if v[2].dtype == "object": + print(f"issue assigning object on {k}") + continue + np.copyto(v[2], np.frombuffer(myfile.read(), v[2].dtype).reshape(v[3])) + return ret[0] + +def fake_torch_load(b0): + import io + import struct + + # convert it to a file + fb0 = io.BytesIO(b0) + + if b0[0:2] == b"\x50\x4b": + return fake_torch_load_zipped(fb0) + + # skip three junk pickles + pickle.load(fb0) + pickle.load(fb0) + pickle.load(fb0) + + ret, key_prelookup = my_unpickle(fb0) + + # create key_lookup + key_lookup = pickle.load(fb0) + key_real = [None] * len(key_lookup) + for k,v in key_prelookup.items(): + key_real[key_lookup.index(k)] = v + + # read in the actual data + for storage_type, obj_size, np_array, np_shape, np_strides in key_real: + ll = struct.unpack("Q", fb0.read(8))[0] + assert ll == obj_size + bytes_size = {np.float32: 4, np.int64: 8}[storage_type] + mydat = fb0.read(ll * bytes_size) + np.copyto(np_array, np.frombuffer(mydat, storage_type).reshape(np_shape)) + + # numpy stores its strides in bytes + real_strides = tuple([x*bytes_size for x in np_strides]) + np_array.strides = real_strides + + return ret + +def get_child(parent, key): + obj = parent + for k in key.split('.'): + if k.isnumeric(): + obj = obj[int(k)] + elif isinstance(obj, dict): + obj = obj[k] + else: + obj = getattr(obj, k) + return obj diff --git a/tinygrad_repo/openpilot/compile.py b/tinygrad_repo/openpilot/compile.py new file mode 100644 index 000000000..b66c551fa --- /dev/null +++ b/tinygrad_repo/openpilot/compile.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 +from ast import Assert +import pathlib, sys +sys.path.insert(0, str(pathlib.Path(__file__).parent.parent)) +from collections import defaultdict +import pyopencl as cl + +import os +import time +import io + +os.environ['OPT'] = '99' +if os.getenv("GPU", None) is None: + os.environ['OPENCL'] = '1' + +DEBUGCL = int(os.getenv("DEBUGCL", 0)) + +import onnx +import numpy as np + +import tinygrad.ops as ops + +from tinygrad.llops.ops_gpu import CL, CLProgram, CLBuffer +from extra.utils import fetch +from extra.onnx import get_run_onnx +from tinygrad.tensor import Tensor +from tinygrad.helpers import prod + +OPENPILOT_MODEL = "https://github.com/commaai/openpilot/raw/6c5693e965b9c63f8678f52b9e9b5abe35f23feb/selfdrive/modeld/models/supercombo.onnx" + +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() + return inputs, np_inputs + +def compile(dat, output_fn): + Tensor.no_grad = True + using_graph = ops.GRAPH + ops.GRAPH = False + + onnx_model = onnx.load(io.BytesIO(dat)) + run_onnx = get_run_onnx(onnx_model) + + 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 = run_onnx(inputs)['outputs'] + 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 = run_onnx(inputs)['outputs'] + + # note, since CL.CACHE is enabled, it doesn't actually run the kernels + CL.CACHE = [] + if using_graph: ops.GRAPH = True + CL.kernel_count = -1 + tinygrad_out.realize() + ops.GRAPH = False + print("kernel count:", len(CL.CACHE)) + + from extra.thneed import Thneed + t = Thneed(CL.CACHE, {k:inputs[k].lazydata.realized.cl for k in inputs.keys()}) + CL.CACHE = None + t.optimize_local_workgroup() + + # save thneed (before run) + t.save(output_fn) + + print(f"buffers to save: {len(t.buffers_to_save)}, outputs: {t.outputs}") + t.run() + + # 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) + np.testing.assert_allclose(thneed_out, tinygrad_out.numpy()) + + # float32 only (fix this) + FLOAT16 = int(os.getenv("FLOAT16", 0)) + if FLOAT16 == 0: + try: + from test.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) + + # test loading/run thneed + _, new_np_inputs = get_random_input_tensors(input_shapes) + new_torch_out = run_onnx_torch(onnx_model, new_np_inputs).numpy() + + # 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) + + 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) + + # compare thneed (rerun) with torch + np.testing.assert_allclose(new_torch_out, old_thneed_out, atol=1e-4, rtol=1e-2) + + # load thneed and try that + _, new_np_inputs = get_random_input_tensors(input_shapes) + new_torch_out = run_onnx_torch(onnx_model, new_np_inputs).numpy() + nt = Thneed() + nt.load(output_fn) + + # inputs + for k,v in nt.inputs.items(): + CL.enqueue_copy(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) + + # 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 + + + +# UNSAFE_FLOAT4=1 DEBUGCL=1 FLOAT16=1 python3 openpilot/compile.py +# 22.59 ms +if __name__ == "__main__": + if len(sys.argv) >= 3: + with open(sys.argv[1], "rb") as f: + dat = f.read() + compile(dat, sys.argv[2]) + else: + dat = fetch(OPENPILOT_MODEL) + compile(dat, "/tmp/output.thneed") diff --git a/tinygrad_repo/tinygrad/helpers.py b/tinygrad_repo/tinygrad/helpers.py new file mode 100644 index 000000000..d9a55049a --- /dev/null +++ b/tinygrad_repo/tinygrad/helpers.py @@ -0,0 +1,47 @@ +from collections import namedtuple +import os, math + +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 argsort(x): return sorted(range(len(x)), key=x.__getitem__) # https://stackoverflow.com/questions/3382352/equivalent-of-numpy-argsort-in-basic-python +def reduce_shape(shape, axis): return tuple(1 if i in axis else shape[i] for i in range(len(shape))) + +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 diff --git a/tinygrad_repo/tinygrad/llops/ops_gpu.py b/tinygrad_repo/tinygrad/llops/ops_gpu.py new file mode 100644 index 000000000..4d9f7cb47 --- /dev/null +++ b/tinygrad_repo/tinygrad/llops/ops_gpu.py @@ -0,0 +1,178 @@ +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, ConvArgs +from tinygrad.ops import DEBUG, UnaryOps, BinaryOps, ReduceOps, MovementOps +from tinygrad.shapetracker import ShapeTracker + +CLCACHE = int(os.getenv("CLCACHE", "1")) +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 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: # already initted + return + 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, "can't copy 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 = 0 + def __init__(self, name:str, prg:str, options:Tuple[str, ...]=tuple(), argdtypes=None, rename=True, binary=False): + self.name, self.prg, self.options, self.argdtypes = f"{name}_{CLProgram.kernel_cnt}" if rename else name, prg.replace(f"{name}(", f"{name}_{CLProgram.kernel_cnt}(") if rename else prg, options, argdtypes + 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 + self.clprg = self.clprogram.build(options=list(self.options)).__getattr__(self.name) + if self.argdtypes is not None: + self.clprg.set_scalar_arg_dtypes(self.argdtypes) + CLProgram.kernel_cnt += 1 + def __call__(self, *args, op_estimate=0): + 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 += op_estimate + print(f"**CL** {CL.kernel_count:6d} {self.name:20s} args {len(args[2:]):5d} kernels {str(args[0]):18s} {str(args[1]):12s} OPs {op_estimate/1e6:7.1f}M/{CL.ops_sum/1e9:7.2f}G mem {CL.mem_used/1e9:5.2f} GB " + + ("" 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 ({op_estimate/(e.profile.end - e.profile.start):8.2f} GFLOPS)")) + +# **** end CL wrappers **** + +class GPUBuffer: + code_for_op = { + UnaryOps.NOOP: "(A)", UnaryOps.NEG: "(-(A))", UnaryOps.RELU: "max(A, (float)0.)", + UnaryOps.EXP: "exp(A)", UnaryOps.LOG: "log(A)", UnaryOps.SIGN: "sign(A)", UnaryOps.RECIPROCAL: "((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)" + } + start_for_op = {ReduceOps.SUM: "0.0", ReduceOps.MAX: "-INFINITY"} + + def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]], hostbuf:Optional[GPUBuffer]=None, backing:Optional[np.ndarray]=None): + self.st = shape if isinstance(shape, ShapeTracker) else ShapeTracker(tuple(shape)) + self.shape = self.st.shape + 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,): + self.cl + + @property + def cl(self): + if self._buf is None: + self._buf = 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"" + + @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.enqueue_copy(data, self.contiguous_op().cl, is_blocking=True) + return data + + def contiguous_view_constant_fold(x, name:str, reduce:Optional[int]=None) -> Tuple[str, Optional[str], str]: + 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 ''});" + + def unary_op(x, op:UnaryOps): return type(x)(x.shape)._processing_op([("A", x)], GPUBuffer.code_for_op[op]) + def binary_op(x, op:BinaryOps, y:GPUBuffer): return type(x)(x.shape)._processing_op([("A", x), ("B", y)], GPUBuffer.code_for_op[op]) + def contiguous_op(x): return x if x.st.contiguous else x.unary_op(UnaryOps.NOOP) + def movement_op(x, op:MovementOps, arg) -> GPUBuffer: return type(x)(ShapeTracker(x.st).movement_op(op, arg), x) + def reduce_op(x, op:ReduceOps, new_shape:Tuple[int, ...]): return type(x)(new_shape)._processing_op([("A", x)], code="acc", earlycode=GPUBuffer.code_for_op[op], earlybufs=set("A"), op=op) + + def _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") -> GPUBuffer: + assert C is None + + # 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}; + }} + }}""") + + conv_prg([prod(ret.shape), inter_red, 1], [1, inter_red, 1] if inter_red > 1 else None, ret.cl, *buf_cl, op_estimate=prod(reduce_shape[0])*len(earlybufs) + prod(reduce_shape[1])*len(bufs)) + return ret diff --git a/tinygrad_repo/tinygrad/llops/ops_opencl.py b/tinygrad_repo/tinygrad/llops/ops_opencl.py new file mode 120000 index 000000000..02c9307b0 --- /dev/null +++ b/tinygrad_repo/tinygrad/llops/ops_opencl.py @@ -0,0 +1 @@ +../../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 new file mode 100644 index 000000000..a0007ad07 --- /dev/null +++ b/tinygrad_repo/tinygrad/mlops.py @@ -0,0 +1,190 @@ +from tinygrad.helpers import prod, argsort, reduce_shape, get_conv_args +from tinygrad.ops import UnaryOps, BinaryOps, ReduceOps, MovementOps, ProcessingOps +from tinygrad.tensor import Function + +# ************* unary ops ************* + +class ReLU(Function): + def forward(self, x): + self.save_for_backward(x) + return x.unary_op(UnaryOps.RELU) + + def backward(self, grad_output): + return self.saved_tensors[0].unary_op(UnaryOps.SIGN).unary_op(UnaryOps.RELU).binary_op(BinaryOps.MUL, grad_output) + +class Log(Function): + def forward(self, x): + self.save_for_backward(x) + return x.unary_op(UnaryOps.LOG) + + def backward(self, grad_output): + return grad_output.binary_op(BinaryOps.DIV, self.saved_tensors[0]) + +class Exp(Function): + def forward(self, x): + ret = x.unary_op(UnaryOps.EXP) + self.save_for_backward(ret) # we save the output here, not the input + return 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 + +# ************* reduce ops ************* + +class Sum(Function): + def forward(self, x, axis=None): + self.input_shape = x.shape + return x.reduce_op(ReduceOps.SUM, reduce_shape(x.shape, axis)) + + 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 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)) + + # sum of locations, averaged + div = max_is_1s.reduce_op(ReduceOps.SUM, grad_output.shape) + div = div.movement_op(MovementOps.EXPAND, x.shape) + max_is_amount = max_is_1s.binary_op(BinaryOps.DIV, div) + + grad_output_expanded = grad_output.movement_op(MovementOps.EXPAND, x.shape) + return max_is_amount.binary_op(BinaryOps.MUL, grad_output_expanded) + +# ************* binary ops ************* + +class Add(Function): + def forward(self, x, y): + return x.binary_op(BinaryOps.ADD, y) + + def backward(self, grad_output): + return grad_output if self.needs_input_grad[0] else None, \ + grad_output if self.needs_input_grad[1] else None + +class Sub(Function): + def forward(self, x, y): + return x.binary_op(BinaryOps.SUB, y) + + def backward(self, grad_output): + return grad_output if self.needs_input_grad[0] else None, \ + grad_output.unary_op(UnaryOps.NEG) if self.needs_input_grad[1] else None + +class Mul(Function): + def forward(self, x, y): + self.save_for_backward(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 + +class Pow(Function): + def forward(self, x, y): + ret = x.binary_op(BinaryOps.POW, y) + self.save_for_backward(x, y, ret) + return 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 + +# ************* movement ops ************* + +# NOTE: this is sum in reverse +class Expand(Function): + def forward(self, x, shape): + self.input_shape = x.shape + return x.movement_op(MovementOps.EXPAND, shape) + + def backward(self, grad_output): + return grad_output.reduce_op(ReduceOps.SUM, self.input_shape) + +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): + return grad_output.movement_op(MovementOps.RESHAPE, self.input_shape) + +class Permute(Function): + def forward(self, x, order=(1,0)): + self.input_order = order + return x.movement_op(MovementOps.PERMUTE, order) + + 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)) + + def backward(self, grad_output): + return grad_output.slice(self.narg) + +class Flip(Function): + def forward(self, x, axis): + self.axis = axis + return x.movement_op(MovementOps.FLIP, axis) + + 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 new file mode 100644 index 000000000..9563234e6 --- /dev/null +++ b/tinygrad_repo/tinygrad/nn/__init__.py @@ -0,0 +1,61 @@ +from tinygrad.tensor import Tensor + +def batch_normalize(x, weight, bias, mean, invstd): + x = (x - 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]) + +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" + self.eps, self.track_running_stats, self.momentum = eps, track_running_stats, momentum + + self.weight, self.bias = Tensor.ones(sz), Tensor.zeros(sz) + + 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): + 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 + # There's "online" algorithms that fix this, like https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_Online_algorithm + x_detached = x.detach() + 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 + 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.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 getattr(self, "batch_invstd", None) is None: + self.batch_invstd = batch_var.add(self.eps)**-0.5 + batch_invstd = self.batch_invstd + + return batch_normalize(x, self.weight, self.bias, batch_mean, batch_invstd) + +# TODO: is this good weight init? +class Conv2d: + def __init__(self, in_channels, out_channels, kernel_size, stride=1, padding=0, bias=True): + 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 + + 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 + + def __call__(self, x): + return x.linear(self.weight.transpose(), self.bias) diff --git a/tinygrad_repo/tinygrad/nn/optim.py b/tinygrad_repo/tinygrad/nn/optim.py new file mode 100644 index 000000000..db513a41c --- /dev/null +++ b/tinygrad_repo/tinygrad/nn/optim.py @@ -0,0 +1,78 @@ +# sorted in order of increasing complexity +from tinygrad.tensor import Tensor + +class Optimizer: + def __init__(self, params): + # 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 + + self.params = [x for x in params if x.requires_grad] + + # 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: + # 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 + + def realize(self, extra=None): + # TODO: corealize + for p in self.params + extra if extra is not None else self.params: + p.realize() + +class SGD(Optimizer): + def __init__(self, params, lr=0.001): + super().__init__(params) + self.lr = lr + + def step(self): + for t in self.params: + t.assign(t.detach() - t.grad * self.lr) + self.realize() + +class RMSprop(Optimizer): + def __init__(self, params, 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): + for i, t in enumerate(self.params): + self.v[i] = 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): + super().__init__(params) + self.lr, self.b1, self.b2, self.eps, self.t = lr, b1, b2, eps, 0 + + 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): + 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) + t.assign(t.detach() - a * self.m[i].div(self.v[i].sqrt() + self.eps)) + self.realize(self.m + self.v) + +def get_parameters(obj): + parameters = [] + 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 hasattr(obj, '__dict__'): + 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 new file mode 100644 index 000000000..fc0e914e0 --- /dev/null +++ b/tinygrad_repo/tinygrad/ops.py @@ -0,0 +1,380 @@ +from __future__ import annotations +from enum import Enum +from typing import Optional, Tuple, NamedTuple, Union, Any, List, Dict, Type +from copy import copy +import os, sys, functools, itertools, operator, weakref +from tinygrad.helpers import ConvArgs, get_available_llops, prod +from tinygrad.shapetracker import ShapeTracker + +# lazy can recurse a lot +sys.setrecursionlimit(10000) + +# 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"]) + +Op = Union[UnaryOps, BinaryOps, ReduceOps, MovementOps, ProcessingOps, LoadOps] +OpType = Union[Type[UnaryOps], Type[BinaryOps], Type[ReduceOps], Type[MovementOps], Type[ProcessingOps], Type[LoadOps]] + +DEBUG = int(os.getenv("DEBUG", "0")) +GRAPH = int(os.getenv("GRAPH", "0")) +OPT = int(os.getenv("OPT", "1")) +NOCONV = int(os.getenv("NOCONV", "0")) + +# 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 + +# TODO: get device buffer types +DeviceBuffer = Any + +# **** debugging and graphing **** + +import atexit +from collections import defaultdict +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) + print("saving", G) + nx.drawing.nx_pydot.write_dot(G, '/tmp/net.dot') + # -Gnslimit=100 can make it finish, but you won't like results + os.system('dot -Tsvg /tmp/net.dot -o /tmp/net.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 getattr(x, 'global_num', None) is None: + setattr(x, 'global_num', global_num_max) + global_num_max += 1 + return f"<<< {x.global_num} >>>" + + top_colors = {LoadOps: '#FFFF80', UnaryOps: "#c0c0c0", ReduceOps: "#8080ff", BinaryOps: "#c0c0c0", MovementOps: "#80ff80", ProcessingOps: "#ff8080"} + dashed = (optype == LoadOps and getattr(ret, "_backing", None) is not None) or (getattr(ret, "st", None) is not None and not ret.st.contiguous) + + 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) + 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 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)]['style'] = 'filled, dashed' if dashed else 'filled' + + +# **** realize helpers **** + +def _ast(x: Union[LazyBuffer, LazyOp], buf_names: Dict[LazyBuffer, str], code_for_op: Dict[Op, str]) -> str: + if isinstance(x, LazyBuffer): + return buf_names[x] + 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 + +# **** realize functions **** + +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_op() + return ret, [real_src], LoadOps if ret != real_src else None + else: + raise NotImplementedError(f"unknown LoadOp {self.op.op}") + +def _realize_movementops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: + real_src = self.op.src[0].realize(self.device) + return real_src.movement_op(self.op.op, self.op.arg), [real_src], MovementOps + +# TODO: unify _realize_reduceops, _realize_processingops, and _realize_binaryops +def _realize_reduceops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: + # 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 getattr(self.dbuffer, "start_for_op", None) and src.realized is None and src.optype == BinaryOps and len(src.children) <= 1: + # TODO: this code is (somewhat) repeated in _realize_binaryops + real_srcs : Dict[LazyBuffer, DeviceBuffer] = {x:x.realize(self.device) for x in get_lazybuffers(src.op)} + buf_names : Dict[LazyBuffer, str] = {x:f"arg_{i}" for i,x in enumerate(real_srcs.keys())} + + return self.dbuffer(self.shape)._processing_op([(buf_names[lb], db) for lb,db in real_srcs.items()], + earlycode=_ast(LazyOp(self.op.op, (src.op,), self.op.arg), buf_names, self.dbuffer.code_for_op), earlybufs=buf_names.values(), op=self.op.op), \ + list(real_srcs.values()), ReduceOps + else: + real_src = src.realize(self.device) + return real_src.reduce_op(self.op.op, self.op.arg), [real_src], ReduceOps + +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 + +def _realize_binaryops(self:LazyBuffer) -> Tuple[DeviceBuffer, List[DeviceBuffer], OpType]: + real_srcs : Dict[LazyBuffer, DeviceBuffer] = {x:None for x in get_lazybuffers(self.op)} + if getattr(self.dbuffer, "_processing_op", None) is not None: + buf_names : Dict[LazyBuffer, str] = {x:f"arg_{i}" for i,x in enumerate(real_srcs.keys())} + reduce_shape = (list(real_srcs.keys())[0].shape, list(real_srcs.keys())[0].shape) + earlycode = "acc" + conv_args : Optional[ConvArgs] = None + + # if there's *one* processing or reduce op in here, we can corealize it. we can corealize binary op siblings as well + # NOTE: if it references the same conv multiple times, they should already be merged by the dictionary + #for k,x in zip(real_srcs.keys(), map(get_movementroot_contiguous, real_srcs.keys())): + # print(k,x, len(x.children), [x for x in x.children]) + 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 len(x.children) <= 1 and len(k.children) <= 1] + 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: + # TODO: do something similar to what i did with reduceop to use the ast engine? + # it's hard because conv also has convargs + conv_args = psrcs[0][1].op.arg + real_srcs[psrcs[0][1].op.src[0]], real_srcs[psrcs[0][1].op.src[1]] = None, None + buf_names[psrcs[0][1].op.src[0]], buf_names[psrcs[0][1].op.src[1]] = "input", "weight" # NOTE: these will not be in the ast + elif psrcs[0][1].optype == ReduceOps: + src = psrcs[0][1].op.src[0] + reduce_shape = (src.shape, psrcs[0][1].shape) + + if MERGE_ELEMENTWISE_INTO_REDUCE and getattr(self.dbuffer, "start_for_op", None) and src.realized is None and src.optype == BinaryOps and len(src.children) <= 1: + src = src.op + for i,x in enumerate(get_lazybuffers(src) if isinstance(src, LazyOp) else [src]): + real_srcs[x] = None + buf_names[x] = f"earlyarg_{i}" + earlycode = _ast(LazyOp(psrcs[0][1].op.op, (src,), psrcs[0][1].op.arg), buf_names, self.dbuffer.code_for_op) + + del real_srcs[psrcs[0][0]] + buf_names[psrcs[0][0]] = "acc" + + for x in real_srcs.keys(): + real_srcs[x] = x.realize(self.device) + # fast path, no middle buffers + return self.dbuffer(self.shape)._processing_op([(buf_names[lb], db) for lb,db in real_srcs.items()], + _ast(self.op, buf_names, self.dbuffer.code_for_op), earlycode=earlycode, earlybufs=set(x for x in buf_names.values() if x.startswith("earlyarg_")), + C=conv_args, reduce_shape=reduce_shape), \ + list(real_srcs.values()), ProcessingOps if conv_args is not None else (ReduceOps if reduce_shape[0] != reduce_shape[1] else BinaryOps) + else: + for x in real_srcs.keys(): + real_srcs[x] = x.realize(self.device) + # slow path, creates middle buffers + def ast_eval(x: Union[LazyBuffer, LazyOp]) -> DeviceBuffer: + if isinstance(x, LazyBuffer): + return real_srcs[x] + if x.op in UnaryOps: + return ast_eval(x.src[0]).unary_op(x.op) + if x.op in BinaryOps: + return ast_eval(x.src[0]).binary_op(x.op, ast_eval(x.src[1])) + return ast_eval(self.op), list(real_srcs.values()), BinaryOps + +_realize = {LoadOps:_realize_loadops, ReduceOps:_realize_reduceops, MovementOps:_realize_movementops, BinaryOps:_realize_binaryops, ProcessingOps:_realize_processingops} + +# **** lazy operations **** + +class LazyOp(NamedTuple): + op: Op + src: Tuple[Union[LazyOp, LazyBuffer], ...] # type: ignore + arg: Any = None + # TODO: add dest to support multiple outputs + +def get_lazybuffers(op:LazyOp) -> List[LazyBuffer]: return functools.reduce(operator.add, [get_lazybuffers(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 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.optype == MovementOps and root.realized is None else root +def get_movementroot_contiguous(x:LazyBuffer) -> LazyBuffer: return get_movementroot(x) if x.optype == MovementOps and x.st.contiguous else x + +LAZY = int(os.getenv("LAZY", "1")) + +class LazyBuffer: + lazycache : weakref.WeakValueDictionary[LazyOp, LazyBuffer] = weakref.WeakValueDictionary() + def __new__(cls, device, shape, optype, op): + # loadops aren't cached + if optype == LoadOps: + 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] + + def __init__(self, device, shape:Union[ShapeTracker, Tuple[int, ...]], optype:OpType, op:LazyOp): + if getattr(self, 'device', None) is not None: + 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.device, self.dbuffer = device, Device._buffers[device] + self.children : weakref.WeakSet[LazyBuffer] = weakref.WeakSet() + # NOTE: op should be read only after construction of LazyBuffer + for x in get_lazybuffers(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 + 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) + # no need to keep the op after realization + del self.op + + assert self.realized.shape == 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 toCPU(self): return self.realize().toCPU() + + def unary_op(self:LazyBuffer, op:UnaryOps) -> LazyBuffer: return elementwise_op(op, self) + def binary_op(self:LazyBuffer, op:BinaryOps, y:LazyBuffer) -> LazyBuffer: return elementwise_op(op, self, y) + def contiguous_op(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 + 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]) + 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) -> LazyBuffer: + # TODO: look into why that copy is needed + arg = tuple(copy(arg)) + + # instant nops + if op in [MovementOps.RESHAPE, MovementOps.EXPAND] and arg == self.shape: + return self + if op == MovementOps.PERMUTE and arg == tuple(range(len(self.shape))): + return self + if op == MovementOps.SHRINK and arg == tuple((0,i) for i in self.shape): + return self + if op == MovementOps.PAD and arg == tuple((0,0) for _ in self.shape): + return self + if op == MovementOps.FLIP and all(s == 1 or i not in arg for i,s in enumerate(self.shape)): + return self + + # two ops in a row is one op + if op in [MovementOps.RESHAPE, MovementOps.EXPAND, MovementOps.SHRINK] and self.realized is None and self.op.op == op: + return self.op.src[0].movement_op(op, arg) + if op == MovementOps.PERMUTE and self.realized is None and self.op.op == op: + return self.op.src[0].movement_op(op, tuple(self.op.arg[i] for i in arg)) + if op == MovementOps.PAD and self.realized is None and self.op.op == op: + return self.op.src[0].movement_op(op, tuple((b1+b2, e1+e2) for (b1,e1),(b2,e2) in zip(self.op.arg, arg))) + + # some permutes are actually just reshapes + if op == MovementOps.PERMUTE and ShapeTracker(self.shape).movement_op(op, arg).contiguous: + return self.movement_op(MovementOps.RESHAPE, tuple(self.shape[i] for i in arg)) + + 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]: + # if this MovementOp is being applied to a BinaryOp, apply the MovementOp to all the BinaryOp inputs instead + 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) + + # create the buffer + ret = LazyBuffer(self.device, ShapeTracker(self.st).movement_op(op, arg), MovementOps, LazyOp(op, (self,), arg)) + + # NOTE: if ret is in the cache, it can already be realized + if REMOVE_MOVEMENT_NOPS and ret.realized is None and self.realized is None and ret.st.contiguous: + # 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 ret + + def processing_op(self:LazyBuffer, op:ProcessingOps, w:LazyBuffer, C:ConvArgs) -> LazyBuffer: + x = self + # 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? + 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]), + (C.rcout, 0), (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))) + 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)) + #print(x.st.views, w.st.views) + 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 + + 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 + + return LazyBuffer(out_device, out_shape, BinaryOps, LazyOp(op, srcs)) diff --git a/tinygrad_repo/tinygrad/shapetracker.py b/tinygrad_repo/tinygrad/shapetracker.py new file mode 100644 index 000000000..18dc4576b --- /dev/null +++ b/tinygrad_repo/tinygrad/shapetracker.py @@ -0,0 +1,158 @@ +# ShapeTracker allows movement operations to a buffer that don't require a copy to be made. +from __future__ import annotations +import functools +from typing import Tuple, Union, List +from tinygrad.helpers import prod + +def divmodidx(acc, d, mod=True): + lr = f"(idx//{acc})" if acc != 1 else "idx" + return f"({lr}%{d})" if mod else lr # don't mod the top shape dimension + +@functools.lru_cache(maxsize=None) +def to_shape_strides(shape:Tuple[int, ...], strides:Tuple[int, ...]) -> List[Tuple[int, int]]: + assert len(shape) == len(strides) + ret = [(shape[0], strides[0])] + for i in range(1, len(shape)): + if (strides[i] != 0 and ret[-1][1] == shape[i]*strides[i]) or (strides[i] == 0 and ret[-1][1] == 0): + ret[-1] = (ret[-1][0] * shape[i], strides[i]) + else: + ret.append((shape[i], strides[i])) + return ret + +class View: + def __init__(self, shape, strides, offset:int=0): + self.shape, self.strides, self.offset = tuple(shape), tuple(strides), offset + self.shape_strides = to_shape_strides(self.shape, self.strides) + + 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))) + + @functools.cached_property + def expr(self): + ret = [f"{self.offset}"] if self.offset != 0 else [] + acc = 1 + for i,(d,s) in enumerate(self.shape_strides[::-1]): + if d != 1 and s != 0: + lr = divmodidx(acc, d, i != len(self.shape_strides)-1 and d != prod(self.shape)) + lr = f"({lr}*{s})" if s != 1 else lr + ret.append(lr) + acc *= d + return 'idx=' + ('+'.join(ret) if len(ret) > 0 else "0") + +class ZeroView: + def __init__(self, old_shape, arg): + self.shape = [] + expr, acc = ['valid'], 1 + for s,(x,y) in list(zip(old_shape, arg))[::-1]: + self.shape = [y-x] + self.shape + base = divmodidx(acc, self.shape[0], len(self.shape) != len(old_shape)) + f"+{x}" + expr += ([f"(({base}) >= 0)"] if x < 0 else []) + ([f"(({base}) < {s})"] if y > s else []) + acc *= self.shape[0] + self.expr = 'valid=' + ' && '.join(expr) + +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) + +@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)) + +class ShapeTracker: + def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]]): + self.views : List[ViewTypes] = shape.views[:] if isinstance(shape, ShapeTracker) else [view_from_shape(shape)] + def __repr__(self): return f"{'Complex' if len(self.views) > 1 else ''}ShapeTracker<{self.shape}, {self.views}>" + + @property + def contiguous(self): return len(self.views) == 1 and self.views[-1].contiguous + + @property + def shape(self): return self.views[-1].shape + + @property + def strides(self): return self.views[-1].strides + + @property + def offset(self): return self.views[-1].offset + + 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): + getattr(self, str(op).split(".")[1].lower())(*arg) + return self + def needs_valid(self): return any(isinstance(v, ZeroView) for v in self.views) + + # 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]) + if self.contiguous: + self.views[-1] = view + else: + self.views.append(view) + + def reshape(self, *new_shape): + assert all(isinstance(x, int) for x in 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 + + 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 + else: + self.views.append(view) + + def permute(self, *axis): + assert all(isinstance(x, int) and x >= 0 and x < len(self.shape) for x in axis) + 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) + + # 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): self.stride(*[-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): + 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)]) + + # TODO: take the pad functionality out of shrink + def shrink(self, *arg): + 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": + # if we add a ZeroView, we add another (stock) view also for modding + self.views += [zeroview, View(self.shape, strides_for_shape(self.shape))] + + def expand(self, *new_shape): + assert all(isinstance(x, int) for x 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))] + self.views[-1] = View(new_shape, strides, self.offset) + + # TODO: combine with slice? this doesn't require a ZeroView, though slice shouldn't always either + def stride(self, *mul): + 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)] + 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) + diff --git a/tinygrad_repo/tinygrad/tensor.py b/tinygrad_repo/tinygrad/tensor.py new file mode 100644 index 000000000..447de56d0 --- /dev/null +++ b/tinygrad_repo/tinygrad/tensor.py @@ -0,0 +1,359 @@ +# inspired by https://github.com/karpathy/micrograd/blob/master/micrograd/engine.py +from __future__ import annotations +import inspect, functools, importlib, itertools +import numpy as np +from tinygrad.helpers import prod, argfix +from typing import List, Tuple, Callable, Optional +from tinygrad.ops import Device + +from tinygrad.ops import LazyBuffer + +# **** start with two base classes, Tensor and Function **** + +class Tensor: + training, no_grad = False, False + + def __init__(self, data, device=Device.DEFAULT, requires_grad=None): + if isinstance(data, list): + data = np.array(data, dtype=np.float32) + elif isinstance(data, LazyBuffer) and data.device != device: + # TODO: this has to realize, it shouldn't have to + data = data.realize().toCPU() + + if isinstance(data, np.ndarray): + if data.shape == tuple(): + data = data.reshape((1,)) + self.lazydata = LazyBuffer.fromCPU(data.astype(np.float32), device) + elif isinstance(data, LazyBuffer): + self.lazydata = data + else: + raise Exception(f"can't create Tensor from {data}") + + # tensors have gradients, buffers do not + self.grad : Optional[Tensor] = None + + # NOTE: this can be in three states. False and None: no gradient, True: gradient + # None (the default) will be updated to True if it's put in an optimizer + self.requires_grad : Optional[bool] = requires_grad + + # internal variables used for autograd graph construction + self._ctx : Optional[Function] = None + + def __repr__(self): + return f"" + + @property + def shape(self): return self.lazydata.shape + + # dtype handling was very broken. it's always float32 now + @property + def dtype(self): return np.float32 + + @property + def device(self): return self.lazydata.device + + # ***** data handlers **** + + def realize(self): + self.lazydata.realize() + return self + + def assign(self, x): + if not isinstance(x, Tensor): + x = Tensor(x) + assert self.shape == x.shape + self.lazydata = x.lazydata + return x + + 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() + + # TODO: if things are realized this won't work + def to_(self, device:str): + assert self.lazydata.realized is None + self.lazydata.device = device + if self.grad: + self.grad.lazydata.device = device + + def to(self, device:str): + ret = Tensor(self.lazydata, device) + if self.grad: + ret.grad = self.grad.to(device) + return ret + + # ***** creation helper functions ***** + + # TODO: remove use of numpy here + + @classmethod + def zeros(cls, *shape, **kwargs): return cls(np.zeros(shape, dtype=np.float32), **kwargs) + + @classmethod + def ones(cls, *shape, **kwargs): return cls(np.ones(shape, dtype=np.float32), **kwargs) + + @classmethod + def empty(cls, *shape, **kwargs): return cls(np.empty(shape, dtype=np.float32), **kwargs) + + @classmethod + def randn(cls, *shape, **kwargs): return cls(np.random.default_rng().standard_normal(size=shape, dtype=np.float32), **kwargs) + + @classmethod + def arange(cls, stop, start=0, **kwargs): return cls(np.arange(start=start, stop=stop, dtype=np.float32), **kwargs) + + # 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) + + @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) + + @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) + + # ***** toposort and backward pass ***** + + def deepwalk(self): + 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] + nodes.append(node) + return nodes + return _deepwalk(self, set(), []) + + def backward(self): + assert self.shape == (1,) + + # fill in the first grad with one + # this is "implicit gradient creation" + self.grad = Tensor.ones(*self.shape, device=self.device, requires_grad=False) + + for t0 in reversed(self.deepwalk()): + if not any(x.requires_grad for x in t0._ctx.parents): + continue + assert (t0.grad is not None) + grads = t0._ctx.backward(t0.grad.lazydata) + grads = [Tensor(g, device=self.device, requires_grad=False) if g is not None else None + for g in ([grads] if len(t0._ctx.parents) == 1 else grads)] + for t, g in zip(t0._ctx.parents, grads): + if g is not None and t.requires_grad: + assert g.shape == t.shape, f"grad shape must match tensor shape in {self._ctx!r}, {g.shape!r} != {t.shape!r}" + t.grad = g if t.grad is None else (t.grad + g) + del t0._ctx + + # ***** non first class ops (hlops) ***** + + 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 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] + 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)]) + + # 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] + + def matmul(self:Tensor, w: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]) + if len(self.shape) > 1: + order = tuple(list(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]) + + # NOTE: with NHWC we can remove the transposes + # bs x groups*cin x H x W + cx = self.transpose(order=order).reshape(shape=(bs//groups, groups*cin, -1, 1)) + # groups*cout x cin x H, W + 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 + + # (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])) + + # ***** math functions (unary) ***** + + def __neg__(self): return 0.0-self + def sqrt(self): return self.pow(0.5) + def square(self): return self*self + 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) + + # ***** 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 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 + def gelu(self): return 0.5 * self * (1 + (self * 0.7978845608 * (1 + 0.044715 * self * self)).tanh()) + 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() + + # ***** broadcasted binary ops ***** + + @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]] + 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)) + + # 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)) + + # ***** 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=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): + y = (self - self.mean(axis=axis, keepdim=True)) + return y.div((y*y).mean(axis=axis, keepdim=True).add(eps).sqrt()) + +# 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 + +# 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 (getattr(Tensor, name, None) is not None) 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')) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 35ecca351..b31dab83f 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -38,13 +38,20 @@ class Keyboard: class Joystick: - def __init__(self): + def __init__(self, gamepad=False): # TODO: find a way to get this from API, perhaps "inputs" doesn't support it - self.min_axis_value = {'ABS_Y': 0., 'ABS_RZ': 0.} - self.max_axis_value = {'ABS_Y': 255., 'ABS_RZ': 255.} - self.cancel_button = 'BTN_TRIGGER' - self.axes_values = {'ABS_Y': 0., 'ABS_RZ': 0.} # gb, steer - self.axes_order = ['ABS_Y', 'ABS_RZ'] + if gamepad: + self.cancel_button = 'BTN_NORTH' # (BTN_NORTH=X, ABS_RZ=Right Trigger) + accel_axis = 'ABS_Y' + steer_axis = 'ABS_RX' + else: + self.cancel_button = 'BTN_TRIGGER' + accel_axis = 'ABS_Y' + steer_axis = 'ABS_RZ' + self.min_axis_value = {accel_axis: 0., steer_axis: 0.} + self.max_axis_value = {accel_axis: 255., steer_axis: 255.} + self.axes_values = {accel_axis: 0., steer_axis: 0.} + self.axes_order = [accel_axis, steer_axis] self.cancel = False def update(self): @@ -77,12 +84,11 @@ def send_thread(joystick): print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) if "WEB" in os.environ: import requests - requests.get("http://"+os.environ["WEB"]+":5000/control/%f/%f" % tuple([joystick.axes_values[a] for a in joystick.axes_order][::-1])) + requests.get("http://"+os.environ["WEB"]+":5000/control/%f/%f" % tuple([joystick.axes_values[a] for a in joystick.axes_order][::-1]), timeout=None) rk.keep_time() -def joystick_thread(use_keyboard): +def joystick_thread(joystick): Params().put_bool('JoystickDebugMode', True) - joystick = Keyboard() if use_keyboard else Joystick() threading.Thread(target=send_thread, args=(joystick,), daemon=True).start() while True: joystick.update() @@ -92,6 +98,7 @@ if __name__ == '__main__': 'openpilot must be offroad before starting joysticked.', formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') + parser.add_argument('--gamepad', action='store_true', help='Use gamepad configuration instead of joystick') args = parser.parse_args() if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ and "WEB" not in os.environ: @@ -108,4 +115,5 @@ if __name__ == '__main__': else: print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!') - joystick_thread(args.keyboard) + joystick = Keyboard() if args.keyboard else Joystick(args.gamepad) + joystick_thread(joystick) diff --git a/tools/lib/README.md b/tools/lib/README.md index 241232eae..3cf239d2d 100644 --- a/tools/lib/README.md +++ b/tools/lib/README.md @@ -1,6 +1,6 @@ ## LogReader -Route is a class for conviently 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](/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. ```python from tools.lib.route import Route diff --git a/tools/lib/bootlog.py b/tools/lib/bootlog.py index 351537082..1e474e5dd 100644 --- a/tools/lib/bootlog.py +++ b/tools/lib/bootlog.py @@ -1,6 +1,7 @@ import datetime import functools import re +from typing import List, Optional from tools.lib.auth_config import get_token from tools.lib.api import CommaApi @@ -48,8 +49,15 @@ class Bootlog: return False return self.datetime < b.datetime +def get_bootlog_from_id(bootlog_id: str) -> Optional[Bootlog]: + # TODO: implement an API endpoint for this + bl = Bootlog(bootlog_id) + for b in get_bootlogs(bl.dongle_id): + if b == bl: + return b + return None -def get_bootlogs(dongle_id: str): +def get_bootlogs(dongle_id: str) -> List[Bootlog]: api = CommaApi(get_token()) r = api.get(f'v1/devices/{dongle_id}/bootlogs') return [Bootlog(b) for b in r] diff --git a/tools/lib/filereader.py b/tools/lib/filereader.py index 4ea9ad429..215f7b218 100644 --- a/tools/lib/filereader.py +++ b/tools/lib/filereader.py @@ -1,7 +1,7 @@ import os from tools.lib.url_file import URLFile -DATA_ENDPOINT = os.getenv("DATA_ENDPOINT", "http://data-raw.internal/") +DATA_ENDPOINT = os.getenv("DATA_ENDPOINT", "http://data-raw.comma.internal/") def FileReader(fn, debug=False): if fn.startswith("cd:/"): diff --git a/tools/lib/tests/test_readers.py b/tools/lib/tests/test_readers.py index 1d8918ba5..6efa0dc35 100755 --- a/tools/lib/tests/test_readers.py +++ b/tools/lib/tests/test_readers.py @@ -10,7 +10,7 @@ from tools.lib.logreader import LogReader class TestReaders(unittest.TestCase): - @unittest.skip("skip for bandwith reasons") + @unittest.skip("skip for bandwidth reasons") def test_logreader(self): def _check_data(lr): hist = defaultdict(int) @@ -21,7 +21,7 @@ class TestReaders(unittest.TestCase): self.assertEqual(hist['logMessage'], 6857) with tempfile.NamedTemporaryFile(suffix=".bz2") as fp: - r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true") + r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true", timeout=10) fp.write(r.content) fp.flush() @@ -31,7 +31,7 @@ class TestReaders(unittest.TestCase): lr_url = LogReader("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true") _check_data(lr_url) - @unittest.skip("skip for bandwith reasons") + @unittest.skip("skip for bandwidth reasons") def test_framereader(self): def _check_data(f): self.assertEqual(f.frame_count, 1200) @@ -53,7 +53,7 @@ class TestReaders(unittest.TestCase): assert np.all(frame_first_30[15] == frame_15[0]) with tempfile.NamedTemporaryFile(suffix=".hevc") as fp: - r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true") + r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true", timeout=10) fp.write(r.content) fp.flush() diff --git a/tools/replay/camera.cc b/tools/replay/camera.cc index 87afe63a2..72b385dca 100644 --- a/tools/replay/camera.cc +++ b/tools/replay/camera.cc @@ -37,7 +37,8 @@ void CameraServer::startVipcServer() { void CameraServer::cameraThread(Camera &cam) { auto read_frame = [&](FrameReader *fr, int frame_id) { VisionBuf *yuv_buf = vipc_server_->get_buffer(cam.stream_type); - bool ret = fr->get(frame_id, yuv_buf ? (uint8_t *)yuv_buf->addr : nullptr); + assert(yuv_buf); + bool ret = fr->get(frame_id, (uint8_t *)yuv_buf->addr); return ret ? yuv_buf : nullptr; }; @@ -54,9 +55,10 @@ void CameraServer::cameraThread(Camera &cam) { .timestamp_sof = eidx.getTimestampSof(), .timestamp_eof = eidx.getTimestampEof(), }; + yuv->set_frame_id(eidx.getFrameId()); vipc_server_->send(yuv, &extra, false); } else { - rError("camera[%d] failed to get frame:", cam.type, eidx.getSegmentId()); + rError("camera[%d] failed to get frame: %lu", cam.type, eidx.getSegmentId()); } cam.cached_id = id + 1; diff --git a/tools/replay/consoleui.cc b/tools/replay/consoleui.cc index 6b419ca9d..5f165ac31 100644 --- a/tools/replay/consoleui.cc +++ b/tools/replay/consoleui.cc @@ -18,7 +18,10 @@ const std::initializer_list> keyboard_shortc {"space", "Pause/Resume"}, {"e", "Next Engagement"}, {"d", "Next Disengagement"}, - {"t", "Next User Tag"} + {"t", "Next User Tag"}, + {"i", "Next Info"}, + {"w", "Next Warning"}, + {"c", "Next Critical"}, }, { {"enter", "Enter seek request"}, @@ -344,6 +347,12 @@ void ConsoleUI::handleKey(char c) { replay->seekToFlag(FindFlag::nextDisEngagement); } else if (c == 't') { replay->seekToFlag(FindFlag::nextUserFlag); + } else if (c == 'i') { + replay->seekToFlag(FindFlag::nextInfo); + } else if (c == 'w') { + replay->seekToFlag(FindFlag::nextWarning); + } else if (c == 'c') { + replay->seekToFlag(FindFlag::nextCritical); } else if (c == 'm') { replay->seekTo(+60, true); } else if (c == 'M') { diff --git a/tools/replay/framereader.cc b/tools/replay/framereader.cc index ecde9ad5d..a1ff7b9f8 100644 --- a/tools/replay/framereader.cc +++ b/tools/replay/framereader.cc @@ -133,7 +133,7 @@ bool FrameReader::load(const std::byte *data, size_t size, bool no_hw_decoder, s break; } packets.push_back(pkt); - // some stream seems to contian no keyframes + // some stream seems to contain no keyframes key_frames_count_ += pkt->flags & AV_PKT_FLAG_KEY; } valid_ = valid_ && !packets.empty(); diff --git a/tools/replay/logreader.cc b/tools/replay/logreader.cc index 9b7a07a83..e3d507141 100644 --- a/tools/replay/logreader.cc +++ b/tools/replay/logreader.cc @@ -46,7 +46,9 @@ LogReader::~LogReader() { #endif } -bool LogReader::load(const std::string &url, std::atomic *abort, bool local_cache, int chunk_size, int retries) { +bool LogReader::load(const std::string &url, std::atomic *abort, + const std::set &allow, + bool local_cache, int chunk_size, int retries) { raw_ = FileReader(local_cache, chunk_size, retries).read(url, abort); if (raw_.empty()) return false; @@ -54,18 +56,26 @@ bool LogReader::load(const std::string &url, std::atomic *abort, bool loca raw_ = decompressBZ2(raw_, abort); if (raw_.empty()) return false; } - return parse(abort); + return parse(allow, abort); } bool LogReader::load(const std::byte *data, size_t size, std::atomic *abort) { raw_.assign((const char *)data, size); - return parse(abort); + return parse({}, abort); } -bool LogReader::parse(std::atomic *abort) { +bool LogReader::parse(const std::set &allow, std::atomic *abort) { try { kj::ArrayPtr words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word)); while (words.size() > 0 && !(abort && *abort)) { + if (!allow.empty()) { + capnp::FlatArrayMessageReader reader(words); + auto which = reader.getRoot().which(); + if (allow.find(which) == allow.end()) { + words = kj::arrayPtr(reader.getEnd(), words.end()); + continue; + } + } #ifdef HAS_MEMORY_RESOURCE Event *evt = new (mbr_) Event(words); diff --git a/tools/replay/logreader.h b/tools/replay/logreader.h index bd666d0a7..010839af2 100644 --- a/tools/replay/logreader.h +++ b/tools/replay/logreader.h @@ -5,6 +5,8 @@ #include #endif +#include + #include "cereal/gen/cpp/log.capnp.h" #include "system/camerad/cameras/camera_common.h" #include "tools/replay/filereader.h" @@ -50,12 +52,13 @@ class LogReader { public: LogReader(size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE); ~LogReader(); - bool load(const std::string &url, std::atomic *abort = nullptr, bool local_cache = false, int chunk_size = -1, int retries = 0); + bool load(const std::string &url, std::atomic *abort = nullptr, const std::set &allow = {}, + bool local_cache = false, int chunk_size = -1, int retries = 0); bool load(const std::byte *data, size_t size, std::atomic *abort = nullptr); std::vector events; private: - bool parse(std::atomic *abort); + bool parse(const std::set &allow, std::atomic *abort); std::string raw_; #ifdef HAS_MEMORY_RESOURCE std::pmr::monotonic_buffer_resource *mbr_ = nullptr; diff --git a/tools/replay/main.cc b/tools/replay/main.cc index d3d689487..40dace0c9 100644 --- a/tools/replay/main.cc +++ b/tools/replay/main.cc @@ -4,8 +4,6 @@ #include "tools/replay/consoleui.h" #include "tools/replay/replay.h" -const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; - int main(int argc, char *argv[]) { QCoreApplication app(argc, argv); diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 4b983fff8..1337a4ef2 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -19,6 +19,9 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s if ((allow.empty() || allow.contains(it.name)) && !block.contains(it.name)) { uint16_t which = event_struct.getFieldByName(it.name).getProto().getDiscriminantValue(); sockets_[which] = it.name; + if (!allow.empty() || !block.empty()) { + allow_list.insert((cereal::Event::Which)which); + } s.push_back(it.name); } } @@ -81,7 +84,7 @@ void Replay::start(int seconds) { } void Replay::updateEvents(const std::function &lambda) { - // set updating_events to true to force stream thread relase the lock and wait for evnets_udpated. + // set updating_events to true to force stream thread release the lock and wait for evnets_udpated. updating_events_ = true; { std::unique_lock lk(stream_lock_); @@ -91,17 +94,17 @@ void Replay::updateEvents(const std::function &lambda) { stream_cv_.notify_one(); } -void Replay::seekTo(int seconds, bool relative) { +void Replay::seekTo(double seconds, bool relative) { seconds = relative ? seconds + currentSeconds() : seconds; updateEvents([&]() { - seconds = std::max(0, seconds); - int seg = seconds / 60; + seconds = std::max(double(0.0), seconds); + int seg = (int)seconds / 60; if (segments_.find(seg) == segments_.end()) { rWarning("can't seek to %d s segment %d is invalid", seconds, seg); return true; } - rInfo("seeking to %d s, segment %d", seconds, seg); + rInfo("seeking to %d s, segment %d", (int)seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; return isSegmentMerged(seg); @@ -122,7 +125,9 @@ void Replay::buildTimeline() { for (int i = 0; i < segments_.size() && !exit_; ++i) { LogReader log; - if (!log.load(route_->at(i).qlog.toStdString(), &exit_, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; + if (!log.load(route_->at(i).qlog.toStdString(), &exit_, + {cereal::Event::Which::CONTROLS_STATE, cereal::Event::Which::USER_FLAG}, + !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; for (const Event *e : log.events) { if (e->which == cereal::Event::Which::CONTROLS_STATE) { @@ -166,8 +171,11 @@ std::optional Replay::find(FindFlag flag) { } else if (flag == FindFlag::nextDisEngagement && end_ts > cur_ts) { return end_ts; } - } else if (type == TimelineType::UserFlag) { - if (flag == FindFlag::nextUserFlag && start_ts > cur_ts) { + } else if (start_ts > cur_ts) { + if ((flag == FindFlag::nextUserFlag && type == TimelineType::UserFlag) || + (flag == FindFlag::nextInfo && type == TimelineType::AlertInfo) || + (flag == FindFlag::nextWarning && type == TimelineType::AlertWarning) || + (flag == FindFlag::nextCritical && type == TimelineType::AlertCritical)) { return start_ts; } } @@ -203,7 +211,7 @@ void Replay::queueSegment() { SegmentMap::iterator cur, end; cur = end = segments_.lower_bound(std::min(current_segment_.load(), segments_.rbegin()->first)); - for (int i = 0; end != segments_.end() && i <= FORWARD_SEGS; ++i) { + for (int i = 0; end != segments_.end() && i <= segment_cache_limit + FORWARD_FETCH_SEGS; ++i) { ++end; } // load one segment at a time @@ -212,7 +220,7 @@ void Replay::queueSegment() { if ((seg && !seg->isLoaded()) || !seg) { if (!seg) { rDebug("loading segment %d...", n); - seg = std::make_unique(n, route_->at(n), flags_); + seg = std::make_unique(n, route_->at(n), flags_, allow_list); QObject::connect(seg.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished); } break; @@ -242,7 +250,7 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap:: // merge 3 segments in sequence. std::vector segments_need_merge; size_t new_events_size = 0; - for (auto it = begin; it != end && it->second && it->second->isLoaded() && segments_need_merge.size() < 3; ++it) { + for (auto it = begin; it != end && it->second && it->second->isLoaded() && segments_need_merge.size() < segment_cache_limit; ++it) { segments_need_merge.push_back(it->first); new_events_size += it->second->log->events.size(); } @@ -267,6 +275,9 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap:: segments_merged_ = segments_need_merge; return true; }); + if (stream_thread_) { + emit segmentsMerged(); + } } } @@ -287,6 +298,7 @@ void Replay::startStream(const Segment *cur_segment) { auto words = capnp::messageToFlatArray(builder); auto bytes = words.asBytes(); Params().put("CarParams", (const char *)bytes.begin(), bytes.size()); + Params().put("CarParamsPersistent", (const char *)bytes.begin(), bytes.size()); } else { rWarning("failed to read CarParams from current segment"); } @@ -302,6 +314,7 @@ void Replay::startStream(const Segment *cur_segment) { camera_server_ = std::make_unique(camera_size); } + emit segmentsMerged(); // start stream thread stream_thread_ = new QThread(); QObject::connect(stream_thread_, &QThread::started, [=]() { stream(); }); @@ -312,6 +325,8 @@ void Replay::startStream(const Segment *cur_segment) { } void Replay::publishMessage(const Event *e) { + if (event_filter && event_filter(e, filter_opaque)) return; + if (sm == nullptr) { auto bytes = e->bytes(); int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); @@ -378,7 +393,7 @@ void Replay::stream() { if (cur_which < sockets_.size() && sockets_[cur_which] != nullptr) { // keep time - long etime = cur_mono_time_ - evt_start_ts; + long etime = (cur_mono_time_ - evt_start_ts) / speed_; long rtime = nanos_since_boot() - loop_start_ts; long behind_ns = etime - rtime; // if behind_ns is greater than 1 second, it means that an invalid segemnt is skipped by seeking/replaying diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 86d609683..88c285125 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -7,8 +7,10 @@ #include "tools/replay/camera.h" #include "tools/replay/route.h" +const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; + // one segment uses about 100M of memory -constexpr int FORWARD_SEGS = 5; +constexpr int FORWARD_FETCH_SEGS = 3; enum REPLAY_FLAGS { REPLAY_FLAG_NONE = 0x0000, @@ -26,9 +28,13 @@ enum class FindFlag { nextEngagement, nextDisEngagement, nextUserFlag, + nextInfo, + nextWarning, + nextCritical }; enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag }; +typedef bool (*replayEventFilter)(const Event *, void *); class Replay : public QObject { Q_OBJECT @@ -42,15 +48,28 @@ public: void stop(); void pause(bool pause); void seekToFlag(FindFlag flag); - void seekTo(int seconds, bool relative); + void seekTo(double seconds, bool relative); inline bool isPaused() const { return paused_; } + // the filter is called in streaming thread.try to return quickly from it to avoid blocking streaming. + // the filter function must return true if the event should be filtered. + // otherwise it must return false. + inline void installEventFilter(replayEventFilter filter, void *opaque) { + filter_opaque = opaque; + event_filter = filter; + } + inline int segmentCacheLimit() const { return segment_cache_limit; } + inline void setSegmentCacheLimit(int n) { segment_cache_limit = std::max(3, n); } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } inline const Route* route() const { return route_.get(); } - inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + inline double currentSeconds() const { return double(cur_mono_time_ - route_start_ts_) / 1e9; } + inline uint64_t routeStartTime() const { return route_start_ts_; } inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } inline int totalSeconds() const { return segments_.size() * 60; } + 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::string &carFingerprint() const { return car_fingerprint_; } inline const std::vector> getTimeline() { std::lock_guard lk(timeline_lock); @@ -59,9 +78,10 @@ public: signals: void streamStarted(); + void segmentsMerged(); protected slots: - void segmentLoadFinished(bool sucess); + void segmentLoadFinished(bool success); protected: typedef std::map> SegmentMap; @@ -92,7 +112,7 @@ protected: bool paused_ = false; bool events_updated_ = false; uint64_t route_start_ts_ = 0; - uint64_t cur_mono_time_ = 0; + std::atomic cur_mono_time_ = 0; std::unique_ptr> events_; std::unique_ptr> new_events_; std::vector segments_merged_; @@ -108,5 +128,10 @@ protected: std::mutex timeline_lock; QFuture timeline_future; std::vector> timeline; + std::set allow_list; std::string car_fingerprint_; + float speed_ = 1.0; + replayEventFilter event_filter = nullptr; + void *filter_opaque = nullptr; + int segment_cache_limit = 3; }; diff --git a/tools/replay/route.cc b/tools/replay/route.cc index c91b27ae8..f0d6ec5a1 100644 --- a/tools/replay/route.cc +++ b/tools/replay/route.cc @@ -99,7 +99,9 @@ void Route::addFileToSegment(int n, const QString &file) { // class Segment -Segment::Segment(int n, const SegmentFile &files, uint32_t flags) : seg_num(n), flags(flags) { +Segment::Segment(int n, const SegmentFile &files, uint32_t flags, + const std::set &allow) + : seg_num(n), flags(flags), allow(allow) { // [RoadCam, DriverCam, WideRoadCam, log]. fallback to qcamera/qlog const std::array file_list = { (flags & REPLAY_FLAG_QCAMERA) || files.road_cam.isEmpty() ? files.qcamera : files.road_cam, @@ -130,7 +132,7 @@ void Segment::loadFile(int id, const std::string file) { success = frames[id]->load(file, flags & REPLAY_FLAG_NO_HW_DECODER, &abort_, local_cache, 20 * 1024 * 1024, 3); } else { log = std::make_unique(); - success = log->load(file, &abort_, local_cache, 0, 3); + success = log->load(file, &abort_, allow, local_cache, 0, 3); } if (!success) { diff --git a/tools/replay/route.h b/tools/replay/route.h index 6ca9c3b88..6b78ebad8 100644 --- a/tools/replay/route.h +++ b/tools/replay/route.h @@ -47,7 +47,7 @@ class Segment : public QObject { Q_OBJECT public: - Segment(int n, const SegmentFile &files, uint32_t flags); + Segment(int n, const SegmentFile &files, uint32_t flags, const std::set &allow = {}); ~Segment(); inline bool isLoaded() const { return !loading_ && !abort_; } @@ -65,4 +65,5 @@ protected: std::atomic loading_ = 0; QFutureSynchronizer synchronizer_; uint32_t flags; + std::set allow; };