diff --git a/.gitignore b/.gitignore index b8d9a39b..915a798c 100644 --- a/.gitignore +++ b/.gitignore @@ -53,6 +53,7 @@ one openpilot notebooks xx +hyperthneed panda_jungle apks openpilot-apks @@ -66,3 +67,4 @@ pandaextra flycheck_* cppcheck_report.txt +comma.sh diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 6a9f715b..45de84bd 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -2,7 +2,7 @@ Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use. -Most open source development activity is coordinated through our [Discord](https://discord.comma.ai). A lot of documentation is available on our [medium](https://medium.com/@comma_ai/). +Most open source development activity is coordinated through our [GitHub Discussions](https://github.com/commaai/openpilot/discussions) and [Discord](https://discord.comma.ai). A lot of documentation is available on our [medium](https://medium.com/@comma_ai/). ## Getting Started @@ -12,23 +12,19 @@ Most open source development activity is coordinated through our [Discord](https ## Testing -### Local Testing - -You can test your changes on your machine by running `run_docker_tests.sh`. This will run some automated tests in docker against your code. - ### Automated Testing -All PRs and commits are automatically checked by Github Actions. Check out `.github/workflows/` for what Github Actions runs. Any new tests should be added to Github Actions. +All PRs and commits are automatically checked by GitHub Actions. Check out `.github/workflows/` for what GitHub Actions runs. Any new tests should be added to GitHub Actions. ### Code Style and Linting -Code is automatically checked for style by Github Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`. +Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`. ## Car Ports (openpilot) We've released a [Model Port guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for porting to Toyota/Lexus models. -If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty. +If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). ## Pull Requests @@ -38,12 +34,10 @@ git clone https://github.com/commaai/openpilot.git --recursive ``` Or alternatively, when on the master branch: ``` -git submodule init -git submodule update +git submodule update --init ``` The reasons for having submodules on a dedicated repository and our new development philosophy can be found in our [post about externalization](https://medium.com/@comma_ai/a-2020-theme-externalization-13b33326d8b3). Modules that are in seperate repositories include: -* apks * cereal * laika * opendbc diff --git a/Jenkinsfile b/Jenkinsfile index 6bd42d13..a9479a11 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -124,7 +124,7 @@ pipeline { stage('Replay Tests') { steps { phone_steps("eon2", [ - ["camerad/modeld replay", "cd selfdrive/test/process_replay && ./camera_replay.py"], + ["camerad/modeld replay", "QCOM_REPLAY=1 scons -j4 && cd selfdrive/test/process_replay && ./camera_replay.py"], ]) } } @@ -132,7 +132,7 @@ pipeline { stage('HW + Unit Tests') { steps { phone_steps("eon", [ - ["build cereal", "SCONS_CACHE=1 scons -j4 cereal/"], + ["build", "SCONS_CACHE=1 scons -j4"], ["test sounds", "nosetests -s selfdrive/test/test_sounds.py"], ["test boardd loopback", "nosetests -s selfdrive/boardd/tests/test_boardd_loopback.py"], ["test loggerd", "CI=1 python selfdrive/loggerd/tests/test_loggerd.py"], diff --git a/README.md b/README.md index 517cde94..4eb640f9 100644 --- a/README.md +++ b/README.md @@ -64,27 +64,27 @@ Supported Cars | Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | | ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------| -| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 25mph | +| Acura | ILX 2016-19 | AcuraWatch Plus | openpilot | 25mph1 | 25mph | | Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 12mph | | Acura | RDX 2020 | All | Stock | 0mph | 3mph | | Honda | Accord 2018-20 | All | Stock | 0mph | 3mph | | Honda | Accord Hybrid 2018-20 | All | Stock | 0mph | 3mph | -| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph | +| Honda | Civic Hatchback 2017-21 | Honda Sensing | Stock | 0mph | 12mph | | Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph | | Honda | Civic Sedan/Coupe 2019-20 | All | Stock | 0mph | 2mph2 | | Honda | CR-V 2015-16 | Touring | openpilot | 25mph1 | 12mph | | Honda | CR-V 2017-20 | Honda Sensing | Stock | 0mph | 12mph | | Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph | | Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph1 | 12mph | -| Honda | HR-V 2019 | Honda Sensing | openpilot | 25mph1 | 12mph | +| Honda | HR-V 2019-20 | Honda Sensing | openpilot | 25mph1 | 12mph | | Honda | Insight 2019-20 | All | Stock | 0mph | 3mph | | Honda | Inspire 2018 | All | Stock | 0mph | 3mph | | Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph1 | 0mph | | Honda | Passport 2019 | All | openpilot | 25mph1 | 12mph | | Honda | Pilot 2016-19 | Honda Sensing | openpilot | 25mph1 | 12mph | | Honda | Ridgeline 2017-20 | Honda Sensing | openpilot | 25mph1 | 12mph | -| Hyundai | Palisade 2020 | All | Stock | 0mph | 0mph | -| Hyundai | Sonata 2020 | All | Stock | 0mph | 0mph | +| Hyundai | Palisade 2020 | All | Stock | 0mph | 0mph | +| Hyundai | Sonata 2020-21 | All | Stock | 0mph | 0mph | | Lexus | CT Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph | | Lexus | ES 2019-20 | All | openpilot | 0mph | 0mph | | Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | @@ -93,11 +93,12 @@ Supported Cars | Lexus | NX 2018 | All | Stock3| 0mph | 0mph | | Lexus | NX Hybrid 2018 | All | Stock3| 0mph | 0mph | | Lexus | RX 2016-18 | All | Stock3| 0mph | 0mph | -| Lexus | RX 2020 | All | openpilot | 0mph | 0mph | +| Lexus | RX 2020-21 | All | openpilot | 0mph | 0mph | | Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph | | Lexus | RX Hybrid 2020 | All | openpilot | 0mph | 0mph | | Toyota | Avalon 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph | +| Toyota | Camry 2021 | All | openpilot | 0mph | 0mph | | Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph4 | 0mph | | Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | | Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | @@ -106,9 +107,9 @@ Supported Cars | Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph | | Toyota | Corolla Hybrid 2020-21 | All | openpilot | 0mph | 0mph | | Toyota | Highlander 2017-19 | All | Stock3| 0mph | 0mph | -| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Highlander 2020-21 | All | openpilot | 0mph | 0mph | | Toyota | Highlander Hybrid 2017-19 | All | Stock3| 0mph | 0mph | -| Toyota | Highlander Hybrid 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Highlander Hybrid 2020-21 | All | openpilot | 0mph | 0mph | | Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph | | Toyota | Prius 2021 | All | openpilot | 0mph | 0mph | | Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph | @@ -143,7 +144,8 @@ Community Maintained Cars and Features | Holden | Astra 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | | Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph | | Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph | -| Hyundai | Ioniq Electric 2019-20 | SCC + LKAS | Stock | 0mph | 32mph | +| Hyundai | Ioniq Electric 2019 | SCC + LKAS | Stock | 0mph | 32mph | +| Hyundai | Ioniq Electric 2020 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Kona EV 2019 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Santa Fe 2019 | All | Stock | 0mph | 0mph | @@ -162,7 +164,7 @@ Community Maintained Cars and Features | Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph | | Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | | Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Forester 2019 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Forester 2019-20 | EyeSight | Stock | 0mph | 0mph | | Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | | Volkswagen| Golf 2015-19 | Driver Assistance | Stock | 0mph | 0mph | diff --git a/RELEASES.md b/RELEASES.md index 367a7e66..c427e60d 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,13 @@ +Version 0.8.1 (2020-12-21) +======================== + * Original EON is deprecated, upgrade to comma two + * Better model performance in heavy rain + * Better lane positioning in turns + * Fixed bug where model would cut turns on empty roads at night + * Fixed issue where some Toyotas would not completely stop thanks to briskspirit! + * Toyota Camry 2021 with TSS2.5 support + * Hyundai Ioniq Electric 2020 support thanks to baldwalker! + Version 0.8.0 (2020-11-30) ======================== * New driving model: fully 3D and improved cut-in detection diff --git a/SConstruct b/SConstruct index 758d494f..ddca0fe5 100644 --- a/SConstruct +++ b/SConstruct @@ -1,12 +1,10 @@ -import Cython -import distutils import os import shutil import subprocess import sys +import sysconfig import platform import numpy as np -from sysconfig import get_paths TICI = os.path.isfile('/TICI') Decider('MD5-timestamp') @@ -19,10 +17,6 @@ AddOption('--asan', action='store_true', help='turn on ASAN') -# Rebuild cython extensions if python, distutils, or cython change -cython_dependencies = [Value(v) for v in (sys.version, distutils.__version__, Cython.__version__)] -Export('cython_dependencies') - real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() if platform.system() == "Darwin": arch = "Darwin" @@ -130,10 +124,6 @@ else: # change pythonpath to this lenv["PYTHONPATH"] = Dir("#").path -#Get the path for Python.h for cython linking -python_path = get_paths()['include'] -numpy_path = np.get_include() - env = Environment( ENV=lenv, CCFLAGS=[ @@ -188,30 +178,27 @@ env = Environment( CXXFLAGS=["-std=c++1z"] + cxxflags, LIBPATH=libpath + [ "#cereal", + "#phonelibs", + "#opendbc/can", "#selfdrive/boardd", "#selfdrive/common", - "#phonelibs", ], CYTHONCFILESUFFIX=".cpp", - tools=["default", "cython"] + COMPILATIONDB_USE_ABSPATH=True, + tools=["default", "cython", "compilation_db"], ) +if GetOption('test'): + env.CompilationDatabase('compile_commands.json') + if os.environ.get('SCONS_CACHE'): cache_dir = '/tmp/scons_cache' - - if os.getenv('CI'): - branch = os.getenv('GIT_BRANCH') - - if QCOM_REPLAY: - cache_dir = '/tmp/scons_cache_qcom_replay' - elif branch is not None and branch != 'master': - cache_dir_branch = '/tmp/scons_cache_' + branch - if not os.path.isdir(cache_dir_branch) and os.path.isdir(cache_dir): - shutil.copytree(cache_dir, cache_dir_branch) - cache_dir = cache_dir_branch - elif TICI: + if TICI: cache_dir = '/data/scons_cache' + if QCOM_REPLAY: + cache_dir = '/tmp/scons_cache_qcom_replay' + CacheDir(cache_dir) node_interval = 5 @@ -235,22 +222,20 @@ def abspath(x): # rpath works elsewhere return x[0].path.rsplit("/", 1)[1][:-3] -#Cython build enviroment +# Cython build enviroment +py_include = sysconfig.get_paths()['include'] envCython = env.Clone() -envCython["CPPPATH"] += [python_path, numpy_path] +envCython["CPPPATH"] += [py_include, np.get_include()] envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-deprecated-declarations"] -python_libs = [] +envCython["LIBS"] = [] if arch == "Darwin": - envCython["LINKFLAGS"]=["-bundle", "-undefined", "dynamic_lookup"] + envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"] elif arch == "aarch64": - envCython["LINKFLAGS"]=["-shared"] - - python_libs.append(os.path.basename(python_path)) + envCython["LINKFLAGS"] = ["-shared"] + envCython["LIBS"] = [os.path.basename(py_include)] else: - envCython["LINKFLAGS"]=["-pthread", "-shared"] - -envCython["LIBS"] = python_libs + envCython["LINKFLAGS"] = ["-pthread", "-shared"] Export('envCython') @@ -311,3 +296,4 @@ if arch != "Darwin": if arch == "x86_64": SConscript(['tools/lib/index_log/SConscript']) + diff --git a/cereal/.gitignore b/cereal/.gitignore index 9e2fec81..40b416ea 100644 --- a/cereal/.gitignore +++ b/cereal/.gitignore @@ -1,6 +1,7 @@ gen node_modules package-lock.json +*.tmp *.pyc __pycache__ .*.swp diff --git a/cereal/SConscript b/cereal/SConscript index c0ade46f..192e26c9 100644 --- a/cereal/SConscript +++ b/cereal/SConscript @@ -1,4 +1,4 @@ -Import('env', 'arch', 'zmq', 'cython_dependencies') +Import('env', 'envCython', 'arch', 'zmq') import shutil @@ -55,11 +55,7 @@ Depends('messaging/bridge.cc', services_h) # different target? #env.Program('messaging/demo', ['messaging/demo.cc'], LIBS=[messaging_lib, 'zmq']) - -env.Command(['messaging/messaging_pyx.so', 'messaging/messaging_pyx.cpp'], - cython_dependencies + [messaging_lib, 'messaging/messaging_pyx_setup.py', 'messaging/messaging_pyx.pyx', 'messaging/messaging.pxd'], - "cd " + messaging_dir.path + " && python3 messaging_pyx_setup.py build_ext --inplace") - +envCython.Program('messaging/messaging_pyx.so', 'messaging/messaging_pyx.pyx', LIBS=envCython["LIBS"]+[messaging_lib, "zmq"]) if GetOption('test'): env.Program('messaging/test_runner', ['messaging/test_runner.cc', 'messaging/msgq_tests.cc'], LIBS=[messaging_lib]) diff --git a/cereal/car.capnp b/cereal/car.capnp index 4e4f9e75..a9f2e903 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -101,6 +101,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { fanMalfunction @91; cameraMalfunction @92; + startupOneplus @82; + gasUnavailableDEPRECATED @3; dataNeededDEPRECATED @16; modelCommIssueDEPRECATED @27; @@ -112,8 +114,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { invalidGiraffeHondaDEPRECATED @49; invalidGiraffeToyotaDEPRECATED @60; whitePandaUnsupportedDEPRECATED @81; - startupGreyPandaDEPRECATED @82; - canErrorPersistentDEPRECATED @83; + commIssueWarningDEPRECATED @83; focusRecoverActiveDEPRECATED @86; neosUpdateRequiredDEPRECATED @88; modelLagWarningDEPRECATED @93; @@ -398,6 +399,9 @@ struct CarParams { steerRateCost @33 :Float32; # Lateral MPC cost on steering rate steerControlType @34 :SteerControlType; radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN + minSpeedCan @51 :Float32; # Minimum vehicle speed from CAN (below this value drops to 0) + stoppingBrakeRate @52 :Float32; # brake_travel/s while trying to stop + startingBrakeRate @53 :Float32; # brake_travel/s while releasing on restart steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control? diff --git a/cereal/log.capnp b/cereal/log.capnp index bbf6a6be..11f49fa1 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -49,6 +49,8 @@ struct InitData { neo @1; chffrAndroid @2; chffrIos @3; + tici @4; + pc @5; } struct AndroidBuildInfo { @@ -629,6 +631,7 @@ struct ModelData { frameDropPerc @13 :Float32; timestampEof @9 :UInt64; modelExecutionTime @14 :Float32; + gpuExecutionTime @16 :Float32; rawPred @15 :Data; path @1 :PathData; @@ -697,6 +700,7 @@ struct ModelDataV2 { frameDropPerc @2 :Float32; timestampEof @3 :UInt64; modelExecutionTime @15 :Float32; + gpuExecutionTime @17 :Float32; rawPred @16 :Data; position @4 :XYZTData; @@ -1952,6 +1956,7 @@ struct OrbKeyFrame { struct DriverState { frameId @0 :UInt32; modelExecutionTime @14 :Float32; + dspExecutionTime @16 :Float32; rawPred @15 :Data; descriptorDEPRECATED @1 :List(Float32); diff --git a/cereal/messaging/messaging_pyx.pyx b/cereal/messaging/messaging_pyx.pyx index 8b3e8474..fa220c43 100644 --- a/cereal/messaging/messaging_pyx.pyx +++ b/cereal/messaging/messaging_pyx.pyx @@ -7,11 +7,11 @@ from libcpp cimport bool from libc cimport errno -from messaging cimport Context as cppContext -from messaging cimport SubSocket as cppSubSocket -from messaging cimport PubSocket as cppPubSocket -from messaging cimport Poller as cppPoller -from messaging cimport Message as cppMessage +from .messaging cimport Context as cppContext +from .messaging cimport SubSocket as cppSubSocket +from .messaging cimport PubSocket as cppPubSocket +from .messaging cimport Poller as cppPoller +from .messaging cimport Message as cppMessage class MessagingError(Exception): @@ -59,12 +59,12 @@ cdef class Poller: cdef int t = timeout with nogil: - result = self.poller.poll(t) + result = self.poller.poll(t) for s in result: - socket = SubSocket() - socket.setPtr(s) - sockets.append(socket) + socket = SubSocket() + socket.setPtr(s) + sockets.append(socket) return sockets diff --git a/cereal/messaging/messaging_pyx_setup.py b/cereal/messaging/messaging_pyx_setup.py deleted file mode 100644 index 43dc9c98..00000000 --- a/cereal/messaging/messaging_pyx_setup.py +++ /dev/null @@ -1,58 +0,0 @@ -import os -import subprocess -import sysconfig -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module - -from Cython.Build import cythonize -from Cython.Distutils import build_ext - -TICI = os.path.isfile('/TICI') - -def get_ext_filename_without_platform_suffix(filename): - name, ext = os.path.splitext(filename) - ext_suffix = sysconfig.get_config_var('EXT_SUFFIX') - - if ext_suffix == ext: - return filename - - ext_suffix = ext_suffix.replace(ext, '') - idx = name.find(ext_suffix) - - if idx == -1: - return filename - else: - return name[:idx] + ext - - -class BuildExtWithoutPlatformSuffix(build_ext): - def get_ext_filename(self, ext_name): - filename = super().get_ext_filename(ext_name) - return get_ext_filename_without_platform_suffix(filename) - - -sourcefiles = ['messaging_pyx.pyx'] -extra_compile_args = ["-std=c++1z", "-Wno-nullability-completeness"] -libraries = ['zmq'] -ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg - -if ARCH == "aarch64" and not TICI: - # android - extra_compile_args += ["-Wno-deprecated-register"] - libraries += ['gnustl_shared'] - -setup(name='messaging', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "messaging_pyx", - language="c++", - sources=sourcefiles, - extra_compile_args=extra_compile_args, - libraries=libraries, - extra_objects=[ - os.path.join(os.path.dirname(os.path.realpath(__file__)), '../', 'libmessaging.a'), - ] - ), - nthreads=4, - ), -) diff --git a/common/basedir.py b/common/basedir.py index a131f0d1..a728eaac 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,7 +1,7 @@ import os BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) -from common.hardware import PC +from selfdrive.hardware import PC if PC: PERSIST = os.path.join(BASEDIR, "persist") else: diff --git a/common/gpio.py b/common/gpio.py index 73603f26..cb032214 100644 --- a/common/gpio.py +++ b/common/gpio.py @@ -1,11 +1,3 @@ -GPIO_HUB_RST_N = 30 -GPIO_UBLOX_RST_N = 32 -GPIO_UBLOX_SAFEBOOT_N = 33 -GPIO_UBLOX_PWR_EN = 34 -GPIO_STM_RST_N = 124 -GPIO_STM_BOOT0 = 134 - - def gpio_init(pin, output): try: with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f: diff --git a/common/hardware_tici.py b/common/hardware_tici.py deleted file mode 100644 index 2212edd4..00000000 --- a/common/hardware_tici.py +++ /dev/null @@ -1,59 +0,0 @@ -import serial - -from common.hardware_base import HardwareBase -from cereal import log -import subprocess - - -NetworkType = log.ThermalData.NetworkType -NetworkStrength = log.ThermalData.NetworkStrength - - -def run_at_command(cmd, timeout=0.1): - with serial.Serial("/dev/ttyUSB2", timeout=timeout) as ser: - ser.write(cmd + b"\r\n") - ser.readline() # Modem echos request - return ser.readline().decode().rstrip() - - -class Tici(HardwareBase): - def get_sound_card_online(self): - return True - - def get_imei(self, slot): - if slot != 0: - return "" - - for _ in range(10): - try: - imei = run_at_command(b"AT+CGSN") - if len(imei) == 15: - return imei - except serial.SerialException: - pass - - raise RuntimeError("Error getting IMEI") - - def get_serial(self): - return self.get_cmdline()['androidboot.serialno'] - - def get_subscriber_info(self): - return "" - - def reboot(self, reason=None): - subprocess.check_output(["sudo", "reboot"]) - - def get_network_type(self): - return NetworkType.wifi - - def get_sim_info(self): - return { - 'sim_id': '', - 'mcc_mnc': None, - 'network_type': ["Unknown"], - 'sim_state': ["ABSENT"], - 'data_connected': False - } - - def get_network_strength(self, network_type): - return NetworkStrength.unknown diff --git a/common/realtime.py b/common/realtime.py index 9f731500..f702cd6d 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -4,21 +4,31 @@ import os import time import multiprocessing -from common.hardware import PC from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error +from selfdrive.hardware import PC, TICI # time step for each process DT_CTRL = 0.01 # controlsd DT_MDL = 0.05 # model -DT_DMON = 0.1 # driver monitoring DT_TRML = 0.5 # thermald and manager +# driver monitoring +if TICI: + DT_DMON = 0.05 +else: + DT_DMON = 0.1 + class Priority: - MIN_REALTIME = 52 # highest android process priority is 51 - CTRL_LOW = MIN_REALTIME - CTRL_HIGH = MIN_REALTIME + 1 + # CORE 2 + # - modeld = 55 + # - camerad = 54 + CTRL_LOW = 51 # plannerd & radard + + # CORE 3 + # - boardd = 55 + CTRL_HIGH = 53 def set_realtime_priority(level): diff --git a/common/transformations/camera.py b/common/transformations/camera.py index e1fbcb54..9c365dc7 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,7 +1,7 @@ import numpy as np import common.transformations.orientation as orient -from common.hardware import TICI +from selfdrive.hardware import TICI ## -- hardcoded hardware params -- eon_f_focal_length = 910.0 diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 482cff9c..8fe3cbf6 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -14,7 +14,12 @@ function tici_init { } function two_init { - # Restrict Android and other system processes to the first two cores + # Wifi scan + wpa_cli IFNAME=wlan0 SCAN + + # *** shield cores 2-3 *** + + # android gets two cores echo 0-1 > /dev/cpuset/background/cpus echo 0-1 > /dev/cpuset/system-background/cpus echo 0-1 > /dev/cpuset/foreground/cpus @@ -24,7 +29,12 @@ function two_init { # openpilot gets all the cores echo 0-3 > /dev/cpuset/app/cpus - # set up governors + # mask off 2-3 from RPS and XPS - Receive/Transmit Packet Steering + echo 3 | tee /sys/class/net/*/queues/*/rps_cpus + echo 3 | tee /sys/class/net/*/queues/*/xps_cpus + + # *** set up governors *** + # +50mW offroad, +500mW onroad for 30% more RAM bandwidth echo "performance" > /sys/class/devfreq/soc:qcom,cpubw/governor echo 1056000 > /sys/class/devfreq/soc:qcom,m4m/max_freq @@ -40,6 +50,8 @@ function two_init { # /sys/class/devfreq/soc:qcom,mincpubw is the only one left at "powersave" # it seems to gain nothing but a wasted 500mW + # *** set up IRQ affinities *** + # Collect RIL and other possibly long-running I/O interrupts onto CPU 1 echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio) echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage) @@ -50,6 +62,24 @@ function two_init { [ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco [ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T + # GPU and camera get cpu 2 + CAM_IRQS="177 178 179 180 181 182 183 184 185 186 192" + for irq in $CAM_IRQS; do + echo 2 > /proc/irq/$irq/smp_affinity_list + done + echo 2 > /proc/irq/193/smp_affinity_list # GPU + + # give GPU threads RT priority + for pid in $(pgrep "kgsl"); do + chrt -f -p 52 $pid + done + + # the flippening! + LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1 + + # disable bluetooth + service call bluetooth_manager 8 + # Check for NEOS update if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then if [ -f "$DIR/scripts/continue.sh" ]; then @@ -81,9 +111,6 @@ function two_init { } function launch { - # Wifi scan - wpa_cli IFNAME=wlan0 SCAN - # Remove orphaned git lock if it exists on boot [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock @@ -129,9 +156,7 @@ function launch { # comma two init if [ -f /EON ]; then two_init - fi - - if [ -f /TICI ]; then + elif [ -f /TICI ]; then tici_init fi diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 52128a89..faef0b3d 100644 Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ diff --git a/opendbc/.gitignore b/opendbc/.gitignore index f2596919..bf8570d8 100644 --- a/opendbc/.gitignore +++ b/opendbc/.gitignore @@ -1,6 +1,7 @@ .mypy_cache/ *.pyc *.os +*.o *.tmp *.dylib .*.swp diff --git a/opendbc/acura_ilx_2016_can_generated.dbc b/opendbc/acura_ilx_2016_can_generated.dbc index b1e2b2cb..361a75ce 100644 --- a/opendbc/acura_ilx_2016_can_generated.dbc +++ b/opendbc/acura_ilx_2016_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "acura_ilx_2016_can.dbc starts here" +CM_ "acura_ilx_2016_can.dbc starts here"; diff --git a/opendbc/acura_rdx_2018_can_generated.dbc b/opendbc/acura_rdx_2018_can_generated.dbc index 1a87ebc5..895e11f0 100644 --- a/opendbc/acura_rdx_2018_can_generated.dbc +++ b/opendbc/acura_rdx_2018_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "acura_rdx_2018_can.dbc starts here" +CM_ "acura_rdx_2018_can.dbc starts here"; diff --git a/opendbc/acura_rdx_2020_can_generated.dbc b/opendbc/acura_rdx_2020_can_generated.dbc index b4adc662..d3c8f7c0 100644 --- a/opendbc/acura_rdx_2020_can_generated.dbc +++ b/opendbc/acura_rdx_2020_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2020.dbc starts here" +CM_ "Imported file _bosch_2020.dbc starts here"; VERSION "" @@ -375,7 +375,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "acura_rdx_2020_can.dbc starts here" +CM_ "acura_rdx_2020_can.dbc starts here"; BO_ 304 GAS_PEDAL_2: 8 PCM diff --git a/opendbc/can/SConscript b/opendbc/can/SConscript index c8d4f347..4e67fc79 100644 --- a/opendbc/can/SConscript +++ b/opendbc/can/SConscript @@ -1,4 +1,4 @@ -Import('env', 'cereal', 'cython_dependencies') +Import('env', 'envCython', 'cereal') import os from opendbc.can.process_dbc import process @@ -13,10 +13,10 @@ for x in sorted(os.listdir('../')): dbc = env.Command(out_fn, in_fn, compile_dbc) dbcs.append(dbc) - libdbc = env.SharedLibrary('libdbc', ["dbc.cc", "parser.cc", "packer.cc", "common.cc"]+dbcs, LIBS=["capnp", "kj"]) # Build packer and parser -env.Command(['packer_pyx.so', 'packer_pyx.cpp', 'parser_pyx.so', 'parser_pyx.cpp'], - cython_dependencies + [libdbc, cereal, 'common_pyx_setup.py', 'common.pxd', 'packer_pyx.pyx', 'parser_pyx.pyx', 'packer.cc', 'parser.cc'], - "cd opendbc/can && python3 common_pyx_setup.py build_ext --inplace") +lenv = envCython.Clone() +lenv["LINKFLAGS"] += [libdbc[0].get_labspath()] +lenv.Program('parser_pyx.so', 'parser_pyx.pyx') +lenv.Program('packer_pyx.so', 'packer_pyx.pyx') diff --git a/opendbc/can/common_pyx_setup.py b/opendbc/can/common_pyx_setup.py deleted file mode 100644 index a7299842..00000000 --- a/opendbc/can/common_pyx_setup.py +++ /dev/null @@ -1,94 +0,0 @@ -import os -import subprocess -import sysconfig -import platform -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module - -from Cython.Build import cythonize -from Cython.Distutils import build_ext - - -ANNOTATE = os.getenv('ANNOTATE') is not None -BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../../")) - - -def get_ext_filename_without_platform_suffix(filename): - name, ext = os.path.splitext(filename) - ext_suffix = sysconfig.get_config_var('EXT_SUFFIX') - - if ext_suffix == ext: - return filename - - ext_suffix = ext_suffix.replace(ext, '') - idx = name.find(ext_suffix) - - if idx == -1: - return filename - else: - return name[:idx] + ext - - -class BuildExtWithoutPlatformSuffix(build_ext): - def get_ext_filename(self, ext_name): - filename = super().get_ext_filename(ext_name) - return get_ext_filename_without_platform_suffix(filename) - - -extra_compile_args = ["-std=c++1z", "-Wno-nullability-completeness"] -ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg -if ARCH == "aarch64": - extra_compile_args += ["-Wno-deprecated-register"] - -if platform.system() == "Darwin": - libdbc = "libdbc.dylib" -else: - libdbc = "libdbc.so" - -extra_link_args = [os.path.join(BASEDIR, 'opendbc', 'can', libdbc)] -include_dirs = [ - BASEDIR, - os.path.join(BASEDIR, 'phonelibs'), -] - -# Build CAN Parser - -setup(name='CAN parser', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "parser_pyx", - language="c++", - sources=['parser_pyx.pyx'], - extra_compile_args=extra_compile_args, - include_dirs=include_dirs, - extra_link_args=extra_link_args, - ), - nthreads=4, - annotate=ANNOTATE - ), -) - -if platform.system() == "Darwin": - os.system("install_name_tool -change opendbc/can/libdbc.dylib " + BASEDIR + "/opendbc/can/libdbc.dylib parser_pyx.so") - - -# Build CAN Packer - -setup(name='CAN packer', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "packer_pyx", - language="c++", - sources=['packer_pyx.pyx'], - extra_compile_args=extra_compile_args, - include_dirs=include_dirs, - extra_link_args=extra_link_args, - ), - nthreads=4, - annotate=ANNOTATE - ), -) - -if platform.system() == "Darwin": - os.system("install_name_tool -change opendbc/can/libdbc.dylib " + BASEDIR + "/opendbc/can/libdbc.dylib packer_pyx.so") diff --git a/opendbc/can/packer_pyx.pyx b/opendbc/can/packer_pyx.pyx index 14e2f81d..22b4b565 100644 --- a/opendbc/can/packer_pyx.pyx +++ b/opendbc/can/packer_pyx.pyx @@ -8,8 +8,8 @@ from libcpp.string cimport string from libcpp cimport bool from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY -from common cimport CANPacker as cpp_CANPacker -from common cimport dbc_lookup, SignalPackValue, DBC +from .common cimport CANPacker as cpp_CANPacker +from .common cimport dbc_lookup, SignalPackValue, DBC cdef class CANPacker: @@ -22,8 +22,8 @@ cdef class CANPacker: def __init__(self, dbc_name): self.dbc = dbc_lookup(dbc_name) if not self.dbc: - raise RuntimeError("Can't lookup" + dbc_name) - + raise RuntimeError(f"Can't lookup {dbc_name}") + self.packer = new cpp_CANPacker(dbc_name) num_msgs = self.dbc[0].num_msgs for i in range(num_msgs): diff --git a/opendbc/can/parser_pyx.pyx b/opendbc/can/parser_pyx.pyx index 913f5823..39f00a57 100644 --- a/opendbc/can/parser_pyx.pyx +++ b/opendbc/can/parser_pyx.pyx @@ -8,8 +8,8 @@ from libc.stdint cimport uint32_t, uint64_t, uint16_t from libcpp.map cimport map from libcpp cimport bool -from common cimport CANParser as cpp_CANParser -from common cimport SignalParseOptions, MessageParseOptions, dbc_lookup, SignalValue, DBC +from .common cimport CANParser as cpp_CANParser +from .common cimport SignalParseOptions, MessageParseOptions, dbc_lookup, SignalValue, DBC import os import numbers diff --git a/opendbc/honda_accord_lx15t_2018_can_generated.dbc b/opendbc/honda_accord_lx15t_2018_can_generated.dbc index 0ce46d5a..25b4c15a 100644 --- a/opendbc/honda_accord_lx15t_2018_can_generated.dbc +++ b/opendbc/honda_accord_lx15t_2018_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2018.dbc starts here" +CM_ "Imported file _bosch_2018.dbc starts here"; VERSION "" @@ -379,7 +379,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "honda_accord_lx15t_2018_can.dbc starts here" +CM_ "honda_accord_lx15t_2018_can.dbc starts here"; BO_ 304 GAS_PEDAL_2: 8 PCM diff --git a/opendbc/honda_accord_s2t_2018_can_generated.dbc b/opendbc/honda_accord_s2t_2018_can_generated.dbc index 8204b3d9..6407b49f 100644 --- a/opendbc/honda_accord_s2t_2018_can_generated.dbc +++ b/opendbc/honda_accord_s2t_2018_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2018.dbc starts here" +CM_ "Imported file _bosch_2018.dbc starts here"; VERSION "" @@ -379,7 +379,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "honda_accord_s2t_2018_can.dbc starts here" +CM_ "honda_accord_s2t_2018_can.dbc starts here"; BO_ 304 GAS_PEDAL_2: 8 PCM diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc index d9c69c6b..0443f2d5 100644 --- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc +++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2018.dbc starts here" +CM_ "Imported file _bosch_2018.dbc starts here"; VERSION "" @@ -379,7 +379,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "honda_civic_hatchback_ex_2017_can.dbc starts here" +CM_ "honda_civic_hatchback_ex_2017_can.dbc starts here"; BO_ 304 GAS_PEDAL_2: 8 PCM diff --git a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc index 885de713..fecfc588 100644 --- a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc +++ b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2018.dbc starts here" +CM_ "Imported file _bosch_2018.dbc starts here"; VERSION "" @@ -379,7 +379,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "honda_civic_sedan_16_diesel_2019_can.dbc starts here" +CM_ "honda_civic_sedan_16_diesel_2019_can.dbc starts here"; BO_ 316 GAS_PEDAL_2: 8 XXX diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc index 26173859..0d666332 100644 --- a/opendbc/honda_civic_touring_2016_can_generated.dbc +++ b/opendbc/honda_civic_touring_2016_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_civic_touring_2016_can.dbc starts here" +CM_ "honda_civic_touring_2016_can.dbc starts here"; diff --git a/opendbc/honda_crv_ex_2017_body_generated.dbc b/opendbc/honda_crv_ex_2017_body_generated.dbc index 9bf77b3f..4d154e53 100644 --- a/opendbc/honda_crv_ex_2017_body_generated.dbc +++ b/opendbc/honda_crv_ex_2017_body_generated.dbc @@ -1,6 +1,6 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "honda_crv_ex_2017_body.dbc starts here" +CM_ "honda_crv_ex_2017_body.dbc starts here"; BO_ 318291879 BSM_STATUS_RIGHT: 8 XXX SG_ BSM_ALERT : 4|1@0+ (1,0) [0|1] "" XXX SG_ BSM_MODE : 6|2@0+ (1,0) [0|3] "" XXX diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc index f9696552..2e6fbf8b 100644 --- a/opendbc/honda_crv_ex_2017_can_generated.dbc +++ b/opendbc/honda_crv_ex_2017_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2018.dbc starts here" +CM_ "Imported file _bosch_2018.dbc starts here"; VERSION "" @@ -379,7 +379,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "honda_crv_ex_2017_can.dbc starts here" +CM_ "honda_crv_ex_2017_can.dbc starts here"; BO_ 304 GAS_PEDAL_2: 8 PCM diff --git a/opendbc/honda_crv_executive_2016_can_generated.dbc b/opendbc/honda_crv_executive_2016_can_generated.dbc index ce838416..7a7334cd 100644 --- a/opendbc/honda_crv_executive_2016_can_generated.dbc +++ b/opendbc/honda_crv_executive_2016_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_crv_executive_2016_can.dbc starts here" +CM_ "honda_crv_executive_2016_can.dbc starts here"; diff --git a/opendbc/honda_crv_hybrid_2019_can_generated.dbc b/opendbc/honda_crv_hybrid_2019_can_generated.dbc index 3902cf24..e29d0320 100644 --- a/opendbc/honda_crv_hybrid_2019_can_generated.dbc +++ b/opendbc/honda_crv_hybrid_2019_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2018.dbc starts here" +CM_ "Imported file _bosch_2018.dbc starts here"; VERSION "" @@ -379,7 +379,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "honda_crv_hybrid_2019_can.dbc starts here" +CM_ "honda_crv_hybrid_2019_can.dbc starts here"; BO_ 304 GAS_PEDAL_2: 8 PCM diff --git a/opendbc/honda_crv_touring_2016_can_generated.dbc b/opendbc/honda_crv_touring_2016_can_generated.dbc index 8730e035..a3d21d3b 100644 --- a/opendbc/honda_crv_touring_2016_can_generated.dbc +++ b/opendbc/honda_crv_touring_2016_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_crv_touring_2016_can.dbc starts here" +CM_ "honda_crv_touring_2016_can.dbc starts here"; diff --git a/opendbc/honda_fit_ex_2018_can_generated.dbc b/opendbc/honda_fit_ex_2018_can_generated.dbc index a9cf3eb9..3f2e2a19 100644 --- a/opendbc/honda_fit_ex_2018_can_generated.dbc +++ b/opendbc/honda_fit_ex_2018_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_fit_ex_2018_can.dbc starts here" +CM_ "honda_fit_ex_2018_can.dbc starts here"; diff --git a/opendbc/honda_hrv_touring_2019_can_generated.dbc b/opendbc/honda_hrv_touring_2019_can_generated.dbc index d5ca7967..5aea74bc 100644 --- a/opendbc/honda_hrv_touring_2019_can_generated.dbc +++ b/opendbc/honda_hrv_touring_2019_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_hrv_touring_2019_can.dbc starts here" +CM_ "honda_hrv_touring_2019_can.dbc starts here"; diff --git a/opendbc/honda_insight_ex_2019_can_generated.dbc b/opendbc/honda_insight_ex_2019_can_generated.dbc index 8d8d034f..7287af23 100644 --- a/opendbc/honda_insight_ex_2019_can_generated.dbc +++ b/opendbc/honda_insight_ex_2019_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _bosch_2018.dbc starts here" +CM_ "Imported file _bosch_2018.dbc starts here"; VERSION "" @@ -379,7 +379,7 @@ CM_ SG_ 577 LINE_DASHED "1 = line is dashed"; CM_ SG_ 577 LINE_SOLID "1 = line is solid"; VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ; -CM_ "honda_insight_ex_2019_can.dbc starts here" +CM_ "honda_insight_ex_2019_can.dbc starts here"; BO_ 304 GAS_PEDAL_2: 8 PCM diff --git a/opendbc/honda_odyssey_exl_2018_generated.dbc b/opendbc/honda_odyssey_exl_2018_generated.dbc index 36af55b1..7c7526f0 100644 --- a/opendbc/honda_odyssey_exl_2018_generated.dbc +++ b/opendbc/honda_odyssey_exl_2018_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_odyssey_exl_2018.dbc starts here" +CM_ "honda_odyssey_exl_2018.dbc starts here"; diff --git a/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc b/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc index 3c5a88a3..b5422c01 100644 --- a/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc +++ b/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_odyssey_extreme_edition_2018_china_can.dbc starts here" +CM_ "honda_odyssey_extreme_edition_2018_china_can.dbc starts here"; diff --git a/opendbc/honda_pilot_touring_2017_can_generated.dbc b/opendbc/honda_pilot_touring_2017_can_generated.dbc index 1f822132..9cca2491 100644 --- a/opendbc/honda_pilot_touring_2017_can_generated.dbc +++ b/opendbc/honda_pilot_touring_2017_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_pilot_touring_2017_can.dbc starts here" +CM_ "honda_pilot_touring_2017_can.dbc starts here"; diff --git a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc index cdfb2310..0c04d917 100644 --- a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc +++ b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR @@ -19,7 +19,7 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; -CM_ "Imported file _honda_2017.dbc starts here" +CM_ "Imported file _honda_2017.dbc starts here"; VERSION "" @@ -251,7 +251,7 @@ VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ; VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; -CM_ "honda_ridgeline_black_edition_2017_can.dbc starts here" +CM_ "honda_ridgeline_black_edition_2017_can.dbc starts here"; diff --git a/opendbc/hyundai_kia_generic.dbc b/opendbc/hyundai_kia_generic.dbc index 958b5c1f..13ee8f3a 100644 --- a/opendbc/hyundai_kia_generic.dbc +++ b/opendbc/hyundai_kia_generic.dbc @@ -1233,8 +1233,8 @@ BO_ 544 ESP12: 8 ESC SG_ YAW_RATE : 40|13@1+ (0.01,-40.95) [-40.95|40.96] "" _4WD,AFLS,IBOX,LCA,LDWS_LKAS,MDPS,PSB,SCC,SPAS,TCU SG_ YAW_RATE_STAT : 53|1@1+ (1.0,0.0) [0.0|1.0] "" _4WD,AFLS,IBOX,LCA,LDWS_LKAS,MDPS,PSB,SCC,SPAS,TCU SG_ YAW_RATE_DIAG : 54|1@1+ (1.0,0.0) [0.0|1.0] "" _4WD,AFLS,IBOX,LCA,LDWS_LKAS,MDPS,PSB,SCC,SPAS,TCU - SG_ ESP12_AliveCounter : 56|4@1+ (1.0,0.0) [0.0|15.0] "" _4WD,EMS,LPI,TCU,TMU - SG_ ESP12_Checksum : 60|4@1+ (1.0,0.0) [0.0|15.0] "" _4WD,EMS,LPI,TCU,TMU + SG_ ESP12_Checksum : 56|4@1+ (1.0,0.0) [0.0|15.0] "" _4WD,EMS,LPI,TCU,TMU + SG_ ESP12_AliveCounter : 60|4@1+ (1.0,0.0) [0.0|15.0] "" _4WD,EMS,LPI,TCU,TMU BO_ 1307 CLU16: 8 CLU SG_ CF_Clu_TirePressUnitNValueSet : 0|3@1+ (1.0,0.0) [0.0|7.0] "" TPMS diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc index 6cc7eba4..54c81278 100644 --- a/opendbc/lexus_ct200h_2018_pt_generated.dbc +++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "lexus_ct200h_2018_pt.dbc starts here" +CM_ "lexus_ct200h_2018_pt.dbc starts here"; diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc index e1e009ef..24aa1113 100644 --- a/opendbc/lexus_is_2018_pt_generated.dbc +++ b/opendbc/lexus_is_2018_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "lexus_is_2018_pt.dbc starts here" +CM_ "lexus_is_2018_pt.dbc starts here"; diff --git a/opendbc/lexus_nx300_2018_pt_generated.dbc b/opendbc/lexus_nx300_2018_pt_generated.dbc index a9546b7f..824d301d 100644 --- a/opendbc/lexus_nx300_2018_pt_generated.dbc +++ b/opendbc/lexus_nx300_2018_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "lexus_nx300_2018_pt.dbc starts here" +CM_ "lexus_nx300_2018_pt.dbc starts here"; diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc index 7e422ad2..94e6818c 100644 --- a/opendbc/lexus_nx300h_2018_pt_generated.dbc +++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "lexus_nx300h_2018_pt.dbc starts here" +CM_ "lexus_nx300h_2018_pt.dbc starts here"; diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc index c8bb8c38..58c21c59 100644 --- a/opendbc/lexus_rx_350_2016_pt_generated.dbc +++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "lexus_rx_350_2016_pt.dbc starts here" +CM_ "lexus_rx_350_2016_pt.dbc starts here"; diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc index 4fe9f00a..e8a358f4 100644 --- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "lexus_rx_hybrid_2017_pt.dbc starts here" +CM_ "lexus_rx_hybrid_2017_pt.dbc starts here"; diff --git a/opendbc/subaru_forester_2017_generated.dbc b/opendbc/subaru_forester_2017_generated.dbc index 96bc0064..c1ef35fb 100644 --- a/opendbc/subaru_forester_2017_generated.dbc +++ b/opendbc/subaru_forester_2017_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _subaru_preglobal_2015.dbc starts here" +CM_ "Imported file _subaru_preglobal_2015.dbc starts here"; VERSION "" @@ -240,7 +240,7 @@ CM_ SG_ 880 Steering_Voltage_Flat "receives later than 371"; VAL_ 328 Gear 0 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D" 14 "R" 15 "P"; VAL_ 1745 Units 0 "Metric" 1 "Imperial"; -CM_ "subaru_forester_2017.dbc starts here" +CM_ "subaru_forester_2017.dbc starts here"; BO_ 355 ES_DashStatus: 8 XXX diff --git a/opendbc/subaru_global_2017_generated.dbc b/opendbc/subaru_global_2017_generated.dbc index 39063a30..f17667e5 100644 --- a/opendbc/subaru_global_2017_generated.dbc +++ b/opendbc/subaru_global_2017_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _subaru_global.dbc starts here" +CM_ "Imported file _subaru_global.dbc starts here"; VERSION "" @@ -244,7 +244,7 @@ CM_ SG_ 940 Highbeam "01 = low beam, 11 = high beam"; CM_ SG_ 940 FOG_LIGHTS2 "yellow fog light in the dash"; CM_ SG_ 1677 Units "AU/EU: 1 = imperial, 3 = metric US: 3 = imperial, 4 = metric"; -CM_ "subaru_global_2017.dbc starts here" +CM_ "subaru_global_2017.dbc starts here"; BO_ 72 Transmission: 8 XXX diff --git a/opendbc/subaru_outback_2015_generated.dbc b/opendbc/subaru_outback_2015_generated.dbc index 62e3e848..c0a60158 100644 --- a/opendbc/subaru_outback_2015_generated.dbc +++ b/opendbc/subaru_outback_2015_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _subaru_preglobal_2015.dbc starts here" +CM_ "Imported file _subaru_preglobal_2015.dbc starts here"; VERSION "" @@ -240,7 +240,7 @@ CM_ SG_ 880 Steering_Voltage_Flat "receives later than 371"; VAL_ 328 Gear 0 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D" 14 "R" 15 "P"; VAL_ 1745 Units 0 "Metric" 1 "Imperial"; -CM_ "subaru_outback_2015.dbc starts here" +CM_ "subaru_outback_2015.dbc starts here"; BO_ 358 ES_DashStatus: 8 XXX diff --git a/opendbc/subaru_outback_2019_generated.dbc b/opendbc/subaru_outback_2019_generated.dbc index 0d5c14bc..00b04248 100644 --- a/opendbc/subaru_outback_2019_generated.dbc +++ b/opendbc/subaru_outback_2019_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _subaru_preglobal_2015.dbc starts here" +CM_ "Imported file _subaru_preglobal_2015.dbc starts here"; VERSION "" @@ -240,7 +240,7 @@ CM_ SG_ 880 Steering_Voltage_Flat "receives later than 371"; VAL_ 328 Gear 0 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D" 14 "R" 15 "P"; VAL_ 1745 Units 0 "Metric" 1 "Imperial"; -CM_ "subaru_outback_2019.dbc starts here" +CM_ "subaru_outback_2019.dbc starts here"; BO_ 358 ES_DashStatus: 8 XXX diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc index 295bb102..fb6a569d 100644 --- a/opendbc/toyota_avalon_2017_pt_generated.dbc +++ b/opendbc/toyota_avalon_2017_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_avalon_2017_pt.dbc starts here" +CM_ "toyota_avalon_2017_pt.dbc starts here"; diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc index 56fb32ff..72f3d8bd 100644 --- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_camry_hybrid_2018_pt.dbc starts here" +CM_ "toyota_camry_hybrid_2018_pt.dbc starts here"; diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc index 7ad41ef3..d76e3cd7 100644 --- a/opendbc/toyota_corolla_2017_pt_generated.dbc +++ b/opendbc/toyota_corolla_2017_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_corolla_2017_pt.dbc starts here" +CM_ "toyota_corolla_2017_pt.dbc starts here"; diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc index 8db51ffd..b81779fe 100644 --- a/opendbc/toyota_highlander_2017_pt_generated.dbc +++ b/opendbc/toyota_highlander_2017_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_highlander_2017_pt.dbc starts here" +CM_ "toyota_highlander_2017_pt.dbc starts here"; diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc index b9dd821e..6e22e0e2 100644 --- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_highlander_hybrid_2018_pt.dbc starts here" +CM_ "toyota_highlander_hybrid_2018_pt.dbc starts here"; diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc index 0569da07..6d77072b 100644 --- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc +++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _toyota_nodsu_bsm.dbc starts here" +CM_ "Imported file _toyota_nodsu_bsm.dbc starts here"; BO_ 1014 BSM: 8 XXX SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX @@ -18,7 +18,7 @@ CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -26,10 +26,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -47,8 +47,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -337,7 +342,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -345,7 +350,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -379,7 +384,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_nodsu_hybrid_pt.dbc starts here" +CM_ "toyota_nodsu_hybrid_pt.dbc starts here"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index ce38bf50..06876689 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _toyota_nodsu_bsm.dbc starts here" +CM_ "Imported file _toyota_nodsu_bsm.dbc starts here"; BO_ 1014 BSM: 8 XXX SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX @@ -18,7 +18,7 @@ CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -26,10 +26,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -47,8 +47,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -337,7 +342,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -345,7 +350,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -379,7 +384,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_nodsu_pt.dbc starts here" +CM_ "toyota_nodsu_pt.dbc starts here"; diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc index aef24541..868381e5 100644 --- a/opendbc/toyota_prius_2017_pt_generated.dbc +++ b/opendbc/toyota_prius_2017_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_prius_2017_pt.dbc starts here" +CM_ "toyota_prius_2017_pt.dbc starts here"; diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc index ece2f15e..19c5271d 100644 --- a/opendbc/toyota_rav4_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_2017_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_rav4_2017_pt.dbc starts here" +CM_ "toyota_rav4_2017_pt.dbc starts here"; diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc index 4822e126..bb98e0d3 100644 --- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_rav4_hybrid_2017_pt.dbc starts here" +CM_ "toyota_rav4_hybrid_2017_pt.dbc starts here"; diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc index 49735696..ad0d1f31 100644 --- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc +++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc @@ -1,7 +1,7 @@ -CM_ "AUTOGENERATED FILE, DO NOT EDIT" +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here" +CM_ "Imported file _comma.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -9,10 +9,10 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX -CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR @@ -30,8 +30,13 @@ CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX -CM_ "Imported file _toyota_2017.dbc starts here" +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + + +CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -320,7 +325,7 @@ CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; -CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; @@ -328,7 +333,7 @@ CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; -CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; CM_ SG_ 1163 TSREQPD "always 1"; @@ -362,7 +367,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; -CM_ "toyota_sienna_xle_2018_pt.dbc starts here" +CM_ "toyota_sienna_xle_2018_pt.dbc starts here"; diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h index d1ca0c83..ca0f54e5 100644 --- a/panda/board/safety/safety_nissan.h +++ b/panda/board/safety/safety_nissan.h @@ -11,19 +11,27 @@ const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = { const int NISSAN_DEG_TO_CAN = 100; -const CanMsg NISSAN_TX_MSGS[] = {{0x169, 0, 8}, {0x2b1, 0, 8}, {0x4cc, 0, 8}, {0x20b, 2, 6}, {0x280, 2, 8}}; +const CanMsg NISSAN_TX_MSGS[] = {{0x169, 0, 8}, {0x2b1, 0, 8}, {0x4cc, 0, 8}, {0x20b, 2, 6}, {0x20b, 1, 6}, {0x280, 2, 8}}; +// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model. AddrCheckStruct nissan_rx_checks[] = { - {.msg = {{0x2, 0, 5, .expected_timestep = 10000U}}}, // STEER_ANGLE_SENSOR (100Hz) - {.msg = {{0x285, 0, 8, .expected_timestep = 20000U}}}, // WHEEL_SPEEDS_REAR (50Hz) - {.msg = {{0x30f, 2, 3, .expected_timestep = 100000U}}}, // CRUISE_STATE (10Hz) + {.msg = {{0x2, 0, 5, .expected_timestep = 10000U}, + {0x2, 1, 5, .expected_timestep = 10000U}}}, // STEER_ANGLE_SENSOR (100Hz) + {.msg = {{0x285, 0, 8, .expected_timestep = 20000U}, + {0x285, 1, 8, .expected_timestep = 20000U}}}, // WHEEL_SPEEDS_REAR (50Hz) + {.msg = {{0x30f, 2, 3, .expected_timestep = 100000U}, + {0x30f, 1, 3, .expected_timestep = 100000U}}}, // CRUISE_STATE (10Hz) {.msg = {{0x15c, 0, 8, .expected_timestep = 20000U}, + {0x15c, 1, 8, .expected_timestep = 20000U}, {0x239, 0, 8, .expected_timestep = 20000U}}}, // GAS_PEDAL (100Hz / 50Hz) {.msg = {{0x454, 0, 8, .expected_timestep = 100000U}, + {0x454, 1, 8, .expected_timestep = 100000U}, {0x1cc, 0, 4, .expected_timestep = 10000U}}}, // DOORS_LIGHTS (10Hz) / BRAKE (100Hz) }; const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]); +// EPS Location. false = V-CAN, true = C-CAN +bool nissan_alt_eps = false; static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { @@ -34,7 +42,7 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); - if (bus == 0) { + if (((bus == 0) && (!nissan_alt_eps)) || ((bus == 1) && (nissan_alt_eps))) { if (addr == 0x2) { // Current steering angle // Factor -0.1, little endian @@ -73,7 +81,7 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // Handle cruise enabled - if ((bus == 2) && (addr == 0x30f)) { + if ((addr == 0x30f) && (((bus == 2) && (!nissan_alt_eps)) || ((bus == 1) && (nissan_alt_eps)))) { bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1; if (cruise_engaged && !cruise_engaged_prev) { @@ -182,8 +190,14 @@ static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return bus_fwd; } +static void nissan_init(int16_t param) { + controls_allowed = 0; + nissan_alt_eps = param ? 1 : 0; + relay_malfunction_reset(); +} + const safety_hooks nissan_hooks = { - .init = nooutput_init, + .init = nissan_init, .rx = nissan_rx_hook, .tx = nissan_tx_hook, .tx_lin = nooutput_tx_lin_hook, diff --git a/phonelibs/opencl/include/CL/cl_ext_qcom.h b/phonelibs/opencl/include/CL/cl_ext_qcom.h new file mode 100644 index 00000000..6328a1cd --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_ext_qcom.h @@ -0,0 +1,255 @@ +/* Copyright (c) 2009-2017 Qualcomm Technologies, Inc. All Rights Reserved. + * Qualcomm Technologies Proprietary and Confidential. + */ + +#ifndef __OPENCL_CL_EXT_QCOM_H +#define __OPENCL_CL_EXT_QCOM_H + +// Needed by cl_khr_egl_event extension +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +/************************************ + * cl_qcom_create_buffer_from_image * + ************************************/ + +#define CL_BUFFER_FROM_IMAGE_ROW_PITCH_QCOM 0x40C0 +#define CL_BUFFER_FROM_IMAGE_SLICE_PITCH_QCOM 0x40C1 + +extern CL_API_ENTRY cl_mem CL_API_CALL +clCreateBufferFromImageQCOM(cl_mem image, + cl_mem_flags flags, + cl_int *errcode_ret); + + +/************************************ + * cl_qcom_limited_printf extension * + ************************************/ + +/* Builtin printf function buffer size in bytes. */ +#define CL_DEVICE_PRINTF_BUFFER_SIZE_QCOM 0x1049 + + +/************************************* + * cl_qcom_extended_images extension * + *************************************/ + +#define CL_CONTEXT_ENABLE_EXTENDED_IMAGES_QCOM 0x40AA +#define CL_DEVICE_EXTENDED_IMAGE2D_MAX_WIDTH_QCOM 0x40AB +#define CL_DEVICE_EXTENDED_IMAGE2D_MAX_HEIGHT_QCOM 0x40AC +#define CL_DEVICE_EXTENDED_IMAGE3D_MAX_WIDTH_QCOM 0x40AD +#define CL_DEVICE_EXTENDED_IMAGE3D_MAX_HEIGHT_QCOM 0x40AE +#define CL_DEVICE_EXTENDED_IMAGE3D_MAX_DEPTH_QCOM 0x40AF + +/************************************* + * cl_qcom_perf_hint extension * + *************************************/ + +typedef cl_uint cl_perf_hint; + +#define CL_CONTEXT_PERF_HINT_QCOM 0x40C2 + +/*cl_perf_hint*/ +#define CL_PERF_HINT_HIGH_QCOM 0x40C3 +#define CL_PERF_HINT_NORMAL_QCOM 0x40C4 +#define CL_PERF_HINT_LOW_QCOM 0x40C5 + +extern CL_API_ENTRY cl_int CL_API_CALL +clSetPerfHintQCOM(cl_context context, + cl_perf_hint perf_hint); + +// This extension is published at Khronos, so its definitions are made in cl_ext.h. +// This duplication is for backward compatibility. + +#ifndef CL_MEM_ANDROID_NATIVE_BUFFER_HOST_PTR_QCOM + +/********************************* +* cl_qcom_android_native_buffer_host_ptr extension +*********************************/ + +#define CL_MEM_ANDROID_NATIVE_BUFFER_HOST_PTR_QCOM 0x40C6 + + +typedef struct _cl_mem_android_native_buffer_host_ptr +{ + // Type of external memory allocation. + // Must be CL_MEM_ANDROID_NATIVE_BUFFER_HOST_PTR_QCOM for Android native buffers. + cl_mem_ext_host_ptr ext_host_ptr; + + // Virtual pointer to the android native buffer + void* anb_ptr; + +} cl_mem_android_native_buffer_host_ptr; + +#endif //#ifndef CL_MEM_ANDROID_NATIVE_BUFFER_HOST_PTR_QCOM + +/*********************************** +* cl_img_egl_image extension * +************************************/ +typedef void* CLeglImageIMG; +typedef void* CLeglDisplayIMG; + +extern CL_API_ENTRY cl_mem CL_API_CALL +clCreateFromEGLImageIMG(cl_context context, + cl_mem_flags flags, + CLeglImageIMG image, + CLeglDisplayIMG display, + cl_int *errcode_ret); + + +/********************************* +* cl_qcom_other_image extension +*********************************/ + +// Extended flag for creating/querying QCOM non-standard images +#define CL_MEM_OTHER_IMAGE_QCOM (1<<25) + +// cl_channel_type +#define CL_QCOM_UNORM_MIPI10 0x4159 +#define CL_QCOM_UNORM_MIPI12 0x415A +#define CL_QCOM_UNSIGNED_MIPI10 0x415B +#define CL_QCOM_UNSIGNED_MIPI12 0x415C +#define CL_QCOM_UNORM_INT10 0x415D +#define CL_QCOM_UNORM_INT12 0x415E +#define CL_QCOM_UNSIGNED_INT16 0x415F + +// cl_channel_order +// Dedicate 0x4130-0x415F range for QCOM extended image formats +// 0x4130 - 0x4132 range is assigned to pixel-oriented compressed format +#define CL_QCOM_BAYER 0x414E + +#define CL_QCOM_NV12 0x4133 +#define CL_QCOM_NV12_Y 0x4134 +#define CL_QCOM_NV12_UV 0x4135 + +#define CL_QCOM_TILED_NV12 0x4136 +#define CL_QCOM_TILED_NV12_Y 0x4137 +#define CL_QCOM_TILED_NV12_UV 0x4138 + +#define CL_QCOM_P010 0x413C +#define CL_QCOM_P010_Y 0x413D +#define CL_QCOM_P010_UV 0x413E + +#define CL_QCOM_TILED_P010 0x413F +#define CL_QCOM_TILED_P010_Y 0x4140 +#define CL_QCOM_TILED_P010_UV 0x4141 + + +#define CL_QCOM_TP10 0x4145 +#define CL_QCOM_TP10_Y 0x4146 +#define CL_QCOM_TP10_UV 0x4147 + +#define CL_QCOM_TILED_TP10 0x4148 +#define CL_QCOM_TILED_TP10_Y 0x4149 +#define CL_QCOM_TILED_TP10_UV 0x414A + +/********************************* +* cl_qcom_compressed_image extension +*********************************/ + +// Extended flag for creating/querying QCOM non-planar compressed images +#define CL_MEM_COMPRESSED_IMAGE_QCOM (1<<27) + +// Extended image format +// cl_channel_order +#define CL_QCOM_COMPRESSED_RGBA 0x4130 +#define CL_QCOM_COMPRESSED_RGBx 0x4131 + +#define CL_QCOM_COMPRESSED_NV12_Y 0x413A +#define CL_QCOM_COMPRESSED_NV12_UV 0x413B + +#define CL_QCOM_COMPRESSED_P010 0x4142 +#define CL_QCOM_COMPRESSED_P010_Y 0x4143 +#define CL_QCOM_COMPRESSED_P010_UV 0x4144 + +#define CL_QCOM_COMPRESSED_TP10 0x414B +#define CL_QCOM_COMPRESSED_TP10_Y 0x414C +#define CL_QCOM_COMPRESSED_TP10_UV 0x414D + +#define CL_QCOM_COMPRESSED_NV12_4R 0x414F +#define CL_QCOM_COMPRESSED_NV12_4R_Y 0x4150 +#define CL_QCOM_COMPRESSED_NV12_4R_UV 0x4151 +/********************************* +* cl_qcom_compressed_yuv_image_read extension +*********************************/ + +// Extended flag for creating/querying QCOM compressed images +#define CL_MEM_COMPRESSED_YUV_IMAGE_QCOM (1<<28) + +// Extended image format +#define CL_QCOM_COMPRESSED_NV12 0x10C4 + +// Extended flag for setting ION buffer allocation type +#define CL_MEM_ION_HOST_PTR_COMPRESSED_YUV_QCOM 0x40CD +#define CL_MEM_ION_HOST_PTR_PROTECTED_COMPRESSED_YUV_QCOM 0x40CE + +/********************************* +* cl_qcom_accelerated_image_ops +*********************************/ +#define CL_MEM_OBJECT_WEIGHT_IMAGE_QCOM 0x4110 +#define CL_DEVICE_HOF_MAX_NUM_PHASES_QCOM 0x4111 +#define CL_DEVICE_HOF_MAX_FILTER_SIZE_X_QCOM 0x4112 +#define CL_DEVICE_HOF_MAX_FILTER_SIZE_Y_QCOM 0x4113 +#define CL_DEVICE_BLOCK_MATCHING_MAX_REGION_SIZE_X_QCOM 0x4114 +#define CL_DEVICE_BLOCK_MATCHING_MAX_REGION_SIZE_Y_QCOM 0x4115 + +//Extended flag for specifying weight image type +#define CL_WEIGHT_IMAGE_SEPARABLE_QCOM (1<<0) + +// Box Filter +typedef struct _cl_box_filter_size_qcom +{ + // Width of box filter on X direction. + float box_filter_width; + + // Height of box filter on Y direction. + float box_filter_height; +} cl_box_filter_size_qcom; + +// HOF Weight Image Desc +typedef struct _cl_weight_desc_qcom +{ + /** Coordinate of the "center" point of the weight image, + based on the weight image's top-left corner as the origin. */ + size_t center_coord_x; + size_t center_coord_y; + cl_bitfield flags; +} cl_weight_desc_qcom; + +typedef struct _cl_weight_image_desc_qcom +{ + cl_image_desc image_desc; + cl_weight_desc_qcom weight_desc; +} cl_weight_image_desc_qcom; + +/************************************* + * cl_qcom_protected_context extension * + *************************************/ + +#define CL_CONTEXT_PROTECTED_QCOM 0x40C7 +#define CL_MEM_ION_HOST_PTR_PROTECTED_QCOM 0x40C8 + +/************************************* + * cl_qcom_priority_hint extension * + *************************************/ +#define CL_PRIORITY_HINT_NONE_QCOM 0 +typedef cl_uint cl_priority_hint; + +#define CL_CONTEXT_PRIORITY_HINT_QCOM 0x40C9 + +/*cl_priority_hint*/ +#define CL_PRIORITY_HINT_HIGH_QCOM 0x40CA +#define CL_PRIORITY_HINT_NORMAL_QCOM 0x40CB +#define CL_PRIORITY_HINT_LOW_QCOM 0x40CC + +#ifdef __cplusplus +} +#endif + +#endif /* __OPENCL_CL_EXT_QCOM_H */ diff --git a/rednose/helpers/feature_handler.py b/rednose/helpers/feature_handler.py index ce778856..f8ac6514 100755 --- a/rednose/helpers/feature_handler.py +++ b/rednose/helpers/feature_handler.py @@ -153,6 +153,6 @@ def generate_orient_error_jac(K): if __name__ == "__main__": - # TODO: get K from argparse + K = int(sys.argv[1].split("_")[-1]) generated_dir = sys.argv[2] - FeatureHandler.generate_code(generated_dir) + FeatureHandler.generate_code(generated_dir, K=K) diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 6f34cbed..27e8b587 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -20,11 +20,11 @@ from websocket import ABNF, WebSocketTimeoutException, create_connection import cereal.messaging as messaging from cereal.services import service_list -from common.hardware import HARDWARE from common.api import Api from common.basedir import PERSIST from common.params import Params from common.realtime import sec_since_boot +from selfdrive.hardware import HARDWARE from selfdrive.loggerd.config import ROOT from selfdrive.swaglog import cloudlog @@ -221,6 +221,11 @@ def getSimInfo(): return HARDWARE.get_sim_info() +@dispatcher.add_method +def getNetworkType(): + return HARDWARE.get_network_type() + + @dispatcher.add_method def takeSnapshot(): from selfdrive.camerad.snapshot.snapshot import snapshot, jpeg_write diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 1033c082..71e450ba 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -474,6 +474,7 @@ void pigeon_thread() { // ubloxRaw = 8042 PubMaster pm({"ubloxRaw"}); + bool ignition_last = false; #ifdef QCOM2 Pigeon * pigeon = Pigeon::connect("/dev/ttyHS0"); @@ -481,19 +482,27 @@ void pigeon_thread() { Pigeon * pigeon = Pigeon::connect(panda); #endif - pigeon->init(); - while (!do_exit && panda->connected) { std::string recv = pigeon->receive(); if (recv.length() > 0) { if (recv[0] == (char)0x00){ - LOGW("received invalid ublox message, resetting panda GPS"); - pigeon->init(); + if (ignition) { + LOGW("received invalid ublox message while onroad, resetting panda GPS"); + pigeon->init(); + } } else { pigeon_publish_raw(pm, recv); } } + // init pigeon on rising ignition edge + // since it was turned off in low power mode + if(ignition && !ignition_last) { + pigeon->init(); + } + + ignition_last = ignition; + // 10ms - 100 Hz usleep(10*1000); } diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index c209cc4f..d89e302f 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -9,18 +9,30 @@ #include "panda.h" +#ifdef QCOM2 +bool is_legacy_panda_reset() { + FILE *file = fopen("/persist/LEGACY_PANDA_RESET", "r"); + if(file) { + fclose(file); + } + return (file != NULL); +} +#endif + void panda_set_power(bool power){ #ifdef QCOM2 int err = 0; + bool is_legacy = is_legacy_panda_reset(); + err += gpio_init(GPIO_STM_RST_N, true); err += gpio_init(GPIO_STM_BOOT0, true); - err += gpio_set(GPIO_STM_RST_N, false); + err += gpio_set(GPIO_STM_RST_N, is_legacy ? false : true); err += gpio_set(GPIO_STM_BOOT0, false); usleep(100*1000); // 100 ms - err += gpio_set(GPIO_STM_RST_N, power); + err += gpio_set(GPIO_STM_RST_N, is_legacy ? power : (!power)); assert(err == 0); #endif } diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index 2d2716a8..feb55a59 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -34,7 +34,7 @@ env.SharedLibrary('snapshot/visionipc', env.Program('camerad', [ 'main.cc', 'cameras/camera_common.cc', - 'transforms/rgb_to_yuv.c', + 'transforms/rgb_to_yuv.cc', 'imgproc/utils.cc', cameras, ], LIBS=libs) diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index c389dc49..273103aa 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -41,9 +41,9 @@ static cl_program build_debayer_program(cl_device_id device_id, cl_context conte b->rgb_width, b->rgb_height, b->rgb_stride, ci->bayer_flip, ci->hdr); #ifdef QCOM2 - return CLU_LOAD_FROM_FILE(context, device_id, "cameras/real_debayer.cl", args); + return cl_program_from_file(context, device_id, "cameras/real_debayer.cl", args); #else - return CLU_LOAD_FROM_FILE(context, device_id, "cameras/debayer.cl", args); + return cl_program_from_file(context, device_id, "cameras/debayer.cl", args); #endif } @@ -53,7 +53,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, camera_state = s; frame_buf_count = frame_cnt; frame_size = ci->frame_height * ci->frame_stride; - + camera_bufs = std::make_unique(frame_buf_count); camera_bufs_metadata = std::make_unique(frame_buf_count); for (int i = 0; i < frame_buf_count; i++) { @@ -103,23 +103,20 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, yuv_bufs[i].v = yuv_bufs[i].u + (yuv_width / 2 * yuv_height / 2); } - int err; if (ci->bayer) { cl_program prg_debayer = build_debayer_program(device_id, context, ci, this); - krnl_debayer = clCreateKernel(prg_debayer, "debayer10", &err); - assert(err == 0); - assert(clReleaseProgram(prg_debayer) == 0); + krnl_debayer = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); + CL_CHECK(clReleaseProgram(prg_debayer)); } rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, yuv_width, yuv_height, rgb_stride); #ifdef __APPLE__ - q = clCreateCommandQueue(context, device_id, 0, &err); + q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); #else const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; - q = clCreateCommandQueueWithProperties(context, device_id, props, &err); + q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); #endif - assert(err == 0); } CameraBuf::~CameraBuf() { @@ -132,8 +129,12 @@ CameraBuf::~CameraBuf() { for (int i = 0; i < YUV_COUNT; i++) { visionbuf_free(&yuv_ion[i]); } - clReleaseKernel(krnl_debayer); - clReleaseCommandQueue(q); + rgb_to_yuv_destroy(&rgb_to_yuv_state); + + if (krnl_debayer) { + CL_CHECK(clReleaseKernel(krnl_debayer)); + } + CL_CHECK(clReleaseCommandQueue(q)); } bool CameraBuf::acquire() { @@ -156,32 +157,34 @@ bool CameraBuf::acquire() { cl_event debayer_event; cl_mem camrabuf_cl = camera_bufs[buf_idx].buf_cl; if (camera_state->ci.bayer) { - assert(clSetKernelArg(krnl_debayer, 0, sizeof(cl_mem), &camrabuf_cl) == 0); - assert(clSetKernelArg(krnl_debayer, 1, sizeof(cl_mem), &cur_rgb_buf->buf_cl) == 0); + CL_CHECK(clSetKernelArg(krnl_debayer, 0, sizeof(cl_mem), &camrabuf_cl)); + CL_CHECK(clSetKernelArg(krnl_debayer, 1, sizeof(cl_mem), &cur_rgb_buf->buf_cl)); #ifdef QCOM2 - assert(clSetKernelArg(krnl_debayer, 2, camera_state->debayer_cl_localMemSize, 0) == 0); - assert(clEnqueueNDRangeKernel(q, krnl_debayer, 2, NULL, - camera_state->debayer_cl_globalWorkSize, camera_state->debayer_cl_localWorkSize, - 0, 0, &debayer_event) == 0); + constexpr int localMemSize = (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * sizeof(float); + const size_t globalWorkSize[] = {size_t(camera_state->ci.frame_width), size_t(camera_state->ci.frame_height)}; + const size_t localWorkSize[] = {DEBAYER_LOCAL_WORKSIZE, DEBAYER_LOCAL_WORKSIZE}; + CL_CHECK(clSetKernelArg(krnl_debayer, 2, localMemSize, 0)); + CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 2, NULL, globalWorkSize, localWorkSize, + 0, 0, &debayer_event)); #else float digital_gain = camera_state->digital_gain; if ((int)digital_gain == 0) { digital_gain = 1.0; } - assert(clSetKernelArg(krnl_debayer, 2, sizeof(float), &digital_gain) == 0); + CL_CHECK(clSetKernelArg(krnl_debayer, 2, sizeof(float), &digital_gain)); const size_t debayer_work_size = rgb_height; // doesn't divide evenly, is this okay? - assert(clEnqueueNDRangeKernel(q, krnl_debayer, 1, NULL, - &debayer_work_size, NULL, 0, 0, &debayer_event) == 0); + CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 1, NULL, + &debayer_work_size, NULL, 0, 0, &debayer_event)); #endif } else { assert(cur_rgb_buf->len >= frame_size); assert(rgb_stride == camera_state->ci.frame_stride); - assert(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, - cur_rgb_buf->len, 0, 0, &debayer_event) == 0); + CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, + cur_rgb_buf->len, 0, 0, &debayer_event)); } clWaitForEvents(1, &debayer_event); - clReleaseEvent(debayer_event); + CL_CHECK(clReleaseEvent(debayer_event)); tbuffer_release(&camera_tb, buf_idx); visionbuf_sync(cur_rgb_buf, VISIONBUF_SYNC_FROM_DEVICE); @@ -195,11 +198,12 @@ bool CameraBuf::acquire() { pool_acquire(&yuv_pool, cur_yuv_idx); pool_push(&yuv_pool, cur_yuv_idx); + tbuffer_dispatch(&ui_tb, cur_rgb_idx); + return true; } void CameraBuf::release() { - tbuffer_dispatch(&ui_tb, cur_rgb_idx); pool_release(&yuv_pool, cur_yuv_idx); } @@ -294,8 +298,9 @@ void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { row_pointer[0] = row; jpeg_write_scanlines(&cinfo, row_pointer, 1); } - free(row); jpeg_finish_compress(&cinfo); + jpeg_destroy_compress(&cinfo); + free(row); MessageBuilder msg; auto thumbnaild = msg.initEvent().initThumbnail(); @@ -306,6 +311,7 @@ void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { if (s->pm != NULL) { s->pm->send("thumbnail", msg); } + free(thumbnail_buffer); } void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { @@ -347,10 +353,8 @@ void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, in extern volatile sig_atomic_t do_exit; void *processing_thread(MultiCameraState *cameras, const char *tname, - CameraState *cs, int priority, process_thread_cb callback) { + CameraState *cs, process_thread_cb callback) { set_thread_name(tname); - int err = set_realtime_priority(priority); - LOG("%s start! setpriority returns %d", tname, err); for (int cnt = 0; !do_exit; cnt++) { if (!cs->buf.acquire()) continue; @@ -363,65 +367,53 @@ void *processing_thread(MultiCameraState *cameras, const char *tname, } std::thread start_process_thread(MultiCameraState *cameras, const char *tname, - CameraState *cs, int priority, process_thread_cb callback) { - return std::thread(processing_thread, cameras, tname, cs, priority, callback); + CameraState *cs, process_thread_cb callback) { + return std::thread(processing_thread, cameras, tname, cs, callback); } void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) { const CameraBuf *b = &c->buf; - static int meteringbox_xmin = 0, meteringbox_xmax = 0; - static int meteringbox_ymin = 0, meteringbox_ymax = 0; + static int x_min = 0, x_max = 0, y_min = 0, y_max = 0; static const bool rhd_front = Params().read_db_bool("IsRHD"); - sm->update(0); - if (sm->updated("driverState")) { - auto state = (*sm)["driverState"].getDriverState(); - float face_prob = state.getFaceProb(); - float face_position[2]; - face_position[0] = state.getFacePosition()[0]; - face_position[1] = state.getFacePosition()[1]; - - // set front camera metering target - if (face_prob > 0.4) { - int x_offset = rhd_front ? 0:b->rgb_width - 0.5 * b->rgb_height; - meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * b->rgb_height) - 72; - meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * b->rgb_height) + 72; - meteringbox_ymin = (face_position[1] + 0.5) * (b->rgb_height) - 72; - meteringbox_ymax = (face_position[1] + 0.5) * (b->rgb_height) + 72; - } else { // use default setting if no face - meteringbox_ymin = b->rgb_height * 1 / 3; - meteringbox_ymax = b->rgb_height * 1; - meteringbox_xmin = rhd_front ? 0:b->rgb_width * 3 / 5; - meteringbox_xmax = rhd_front ? b->rgb_width * 2 / 5:b->rgb_width; - } - } - // auto exposure if (cnt % 3 == 0) { - // use driver face crop for AE - int x_start, x_end, y_start, y_end; - int skip = 1; + if (sm->update(0) > 0 && sm->updated("driverState")) { + auto state = (*sm)["driverState"].getDriverState(); + // set front camera metering target + if (state.getFaceProb() > 0.4) { + auto face_position = state.getFacePosition(); + int x_offset = rhd_front ? 0 : b->rgb_width - (0.5 * b->rgb_height); + x_offset += (face_position[0] * (rhd_front ? -1.0 : 1.0) + 0.5) * (0.5 * b->rgb_height); + const int y_offset = (face_position[1] + 0.5) * b->rgb_height; - if (meteringbox_xmax > 0) { - x_start = std::max(0, meteringbox_xmin); - x_end = std::min(b->rgb_width - 1, meteringbox_xmax); - y_start = std::max(0, meteringbox_ymin); - y_end = std::min(b->rgb_height - 1, meteringbox_ymax); - } else { - y_start = b->rgb_height * 1 / 3; - y_end = b->rgb_height * 1; - x_start = rhd_front ? 0 : b->rgb_width * 3 / 5; - x_end = rhd_front ? b->rgb_width * 2 / 5 : b->rgb_width; + x_min = std::max(0, x_offset - 72); + x_max = std::min(b->rgb_width - 1, x_offset + 72); + y_min = std::max(0, y_offset - 72); + y_max = std::min(b->rgb_height - 1, y_offset + 72); + } else { // use default setting if no face + x_min = x_max = y_min = y_max = 0; + } + } + + int skip = 1; + // use driver face crop for AE + if (x_max == 0) { + // default setting + x_min = rhd_front ? 0 : b->rgb_width * 3 / 5; + x_max = rhd_front ? b->rgb_width * 2 / 5 : b->rgb_width; + y_min = b->rgb_height / 3; + y_max = b->rgb_height; } #ifdef QCOM2 - x_start = 96; - x_end = 1832; - y_start = 242; - y_end = 1148; + x_min = 96; + x_max = 1832; + y_min = 242; + y_max = 1148; skip = 4; #endif - set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x_start, x_end, 2, y_start, y_end, skip); + set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x_min, x_max, 2, y_min, y_max, skip); } MessageBuilder msg; diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index fc5fa229..1eb68422 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -138,5 +138,5 @@ void fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, i void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr); void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, 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, const char *tname, - CameraState *cs, int priority, process_thread_cb callback); + CameraState *cs, process_thread_cb callback); void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt); diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index a2acc018..aab8d72c 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -119,11 +119,14 @@ void cameras_close(MultiCameraState *s) { // called by processing_thread void camera_process_rear(MultiCameraState *s, CameraState *c, int cnt) { - // empty + if (cnt % 100 == 3) { + const CameraBuf *b = &c->buf; + create_thumbnail(s, c, (uint8_t*)b->cur_rgb_buf->addr); + } } void cameras_run(MultiCameraState *s) { - std::thread t = start_process_thread(s, "processing", &s->rear, 51, camera_process_rear); + std::thread t = start_process_thread(s, "processing", &s->rear, camera_process_rear); set_thread_name("frame_streaming"); run_frame_stream(s); cameras_close(s); diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index e0cdf7a2..451829ec 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -248,7 +248,7 @@ cl_program build_conv_program(cl_device_id device_id, cl_context context, int im "-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d", image_w, image_h, 1, filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w); - return CLU_LOAD_FROM_FILE(context, device_id, "imgproc/conv.cl", args); + return cl_program_from_file(context, device_id, "imgproc/conv.cl", args); } void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { @@ -333,39 +333,26 @@ void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { s->front.device = s->device; s->sm_front = new SubMaster({"driverState"}); - s->sm_rear = new SubMaster({"sensorEvents"}); s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"}); - int err; - const int rgb_width = s->rear.buf.rgb_width; - const int rgb_height = s->rear.buf.rgb_height; for (int i = 0; i < FRAME_BUF_COUNT; i++) { // TODO: make lengths correct s->focus_bufs[i] = visionbuf_allocate(0xb80); s->stats_bufs[i] = visionbuf_allocate(0xb80); } - s->prg_rgb_laplacian = build_conv_program(device_id, ctx, rgb_width/NUM_SEGMENTS_X, rgb_height/NUM_SEGMENTS_Y, 3); - s->krnl_rgb_laplacian = clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err); - assert(err == 0); + const int width = s->rear.buf.rgb_width/NUM_SEGMENTS_X; + const int height = s->rear.buf.rgb_height/NUM_SEGMENTS_Y; + s->prg_rgb_laplacian = build_conv_program(device_id, ctx, width, height, 3); + s->krnl_rgb_laplacian = CL_CHECK_ERR(clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err)); // TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter - s->rgb_conv_roi_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, - rgb_width/NUM_SEGMENTS_X * rgb_height/NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), NULL, NULL); - s->rgb_conv_result_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, - rgb_width/NUM_SEGMENTS_X * rgb_height/NUM_SEGMENTS_Y * sizeof(int16_t), NULL, NULL); - s->rgb_conv_filter_cl = clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, - 9 * sizeof(int16_t), (void*)&lapl_conv_krnl, NULL); - s->conv_cl_localMemSize = ( CONV_LOCAL_WORKSIZE + 2 * (3 / 2) ) * ( CONV_LOCAL_WORKSIZE + 2 * (3 / 2) ); - s->conv_cl_localMemSize *= 3 * sizeof(uint8_t); - s->conv_cl_globalWorkSize[0] = rgb_width/NUM_SEGMENTS_X; - s->conv_cl_globalWorkSize[1] = rgb_height/NUM_SEGMENTS_Y; - s->conv_cl_localWorkSize[0] = CONV_LOCAL_WORKSIZE; - s->conv_cl_localWorkSize[1] = CONV_LOCAL_WORKSIZE; + s->rgb_conv_roi_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, + width * height * 3 * sizeof(uint8_t), NULL, &err)); + s->rgb_conv_result_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, + width * height * sizeof(int16_t), NULL, &err)); + s->rgb_conv_filter_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + 9 * sizeof(int16_t), (void*)&lapl_conv_krnl, &err)); - for (int i=0; ilapres); i++) {s->lapres[i] = 16160;} - - const size_t size = (rgb_width/NUM_SEGMENTS_X)*(rgb_height/NUM_SEGMENTS_Y); - s->rgb_roi_buf = std::make_unique(size*3); - s->conv_result = std::make_unique(size); + std::fill_n(s->lapres, std::size(s->lapres), 16160); } static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { @@ -1792,26 +1779,39 @@ static void parse_autofocus(CameraState *s, uint8_t *d) { s->focus_err = max_focus*1.0; } -static void do_autofocus(CameraState *s) { +static std::optional get_accel_z(SubMaster *sm) { + if (sm->update(0) > 0) { + for (auto event : (*sm)["sensorEvents"].getSensorEvents()) { + if (event.which() == cereal::SensorEventData::ACCELERATION) { + if (auto v = event.getAcceleration().getV(); v.size() >= 3) + return -v[2]; + break; + } + } + } + return std::nullopt; +} + +static void do_autofocus(CameraState *s, SubMaster *sm) { // params for focus PI controller - const float focus_kp = 0.005; - - float err = s->focus_err; - float sag = (s->last_sag_acc_z/9.8) * 128; - const int dac_up = s->device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP; const int dac_down = s->device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN; - float lens_true_pos = s->lens_true_pos; - if (!isnan(err)) { + float lens_true_pos = s->lens_true_pos.load(); + if (!isnan(s->focus_err)) { // learn lens_true_pos - lens_true_pos -= err*focus_kp; + const float focus_kp = 0.005; + lens_true_pos -= s->focus_err*focus_kp; } + if (auto accel_z = get_accel_z(sm)) { + s->last_sag_acc_z = *accel_z; + } + const float sag = (s->last_sag_acc_z / 9.8) * 128; // stay off the walls lens_true_pos = std::clamp(lens_true_pos, float(dac_down), float(dac_up)); int target = std::clamp(lens_true_pos - sag, float(dac_down), float(dac_up)); - s->lens_true_pos = lens_true_pos; + s->lens_true_pos.store(lens_true_pos); /*char debug[4096]; char *pdebug = debug; @@ -2024,12 +2024,12 @@ static void* ops_thread(void* arg) { CameraExpInfo front_op; set_thread_name("camera_settings"); - + SubMaster sm({"sensorEvents"}); while(!do_exit) { rear_op = rear_exp.load(); if (rear_op.op_id != rear_op_id_last) { do_autoexposure(&s->rear, rear_op.grey_frac); - do_autofocus(&s->rear); + do_autofocus(&s->rear, &sm); rear_op_id_last = rear_op.op_id; } @@ -2045,6 +2045,66 @@ static void* ops_thread(void* arg) { return NULL; } +static void update_lapmap(MultiCameraState *s, const CameraBuf *b, const int cnt) { + const size_t width = b->rgb_width / NUM_SEGMENTS_X; + const size_t height = b->rgb_height / NUM_SEGMENTS_Y; + static std::unique_ptr rgb_roi_buf = std::make_unique(width * height * 3); + static std::unique_ptr conv_result = std::make_unique(width * height); + + // sharpness scores + const int roi_id = cnt % std::size(s->lapres); // rolling roi + const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1); + const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1); + + const uint8_t *rgb_addr_offset = (uint8_t *)b->cur_rgb_buf->addr + y_offset * height * FULL_STRIDE_X * 3 + x_offset * width * 3; + for (int i = 0; i < height; ++i) { + memcpy(rgb_roi_buf.get() + i * width * 3, rgb_addr_offset + i * FULL_STRIDE_X * 3, width * 3); + } + + constexpr int conv_cl_localMemSize = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t)); + CL_CHECK(clEnqueueWriteBuffer(b->q, s->rgb_conv_roi_cl, true, 0, width * height * 3 * sizeof(uint8_t), rgb_roi_buf.get(), 0, 0, 0)); + CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 0, sizeof(cl_mem), (void *)&s->rgb_conv_roi_cl)); + CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 1, sizeof(cl_mem), (void *)&s->rgb_conv_result_cl)); + CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 2, sizeof(cl_mem), (void *)&s->rgb_conv_filter_cl)); + CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 3, conv_cl_localMemSize, 0)); + cl_event conv_event; + CL_CHECK(clEnqueueNDRangeKernel(b->q, s->krnl_rgb_laplacian, 2, NULL, + (size_t[]){width, height}, (size_t[]){CONV_LOCAL_WORKSIZE, CONV_LOCAL_WORKSIZE}, 0, 0, &conv_event)); + clWaitForEvents(1, &conv_event); + CL_CHECK(clReleaseEvent(conv_event)); + + CL_CHECK(clEnqueueReadBuffer(b->q, s->rgb_conv_result_cl, true, 0, + width * height * sizeof(int16_t), conv_result.get(), 0, 0, 0)); + + s->lapres[roi_id] = get_lapmap_one(conv_result.get(), width, height); +} + +static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t lapres_size) { + const int dac_down = c->device == DEVICE_LP3 ? LP3_AF_DAC_DOWN : OP3T_AF_DAC_DOWN; + const int dac_up = c->device == DEVICE_LP3 ? LP3_AF_DAC_UP : OP3T_AF_DAC_UP; + const int dac_m = c->device == DEVICE_LP3 ? LP3_AF_DAC_M : OP3T_AF_DAC_M; + const int dac_3sig = c->device == DEVICE_LP3 ? LP3_AF_DAC_3SIG : OP3T_AF_DAC_3SIG; + + const float lens_true_pos = c->lens_true_pos.load(); + int self_recover = c->self_recover.load(); + if (self_recover < 2 && (lens_true_pos < (dac_down + 1) || lens_true_pos > (dac_up - 1)) && is_blur(lapres, lapres_size)) { + // truly stuck, needs help + if (--self_recover < -FOCUS_RECOVER_PATIENCE) { + LOGD("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, self_recover); + // parity determined by which end is stuck at + self_recover = FOCUS_RECOVER_STEPS + (lens_true_pos < dac_m ? 1 : 0); + } + } else if (self_recover < 2 && (lens_true_pos < (dac_m - dac_3sig) || lens_true_pos > (dac_m + dac_3sig))) { + // in suboptimal position with high prob, but may still recover by itself + if (--self_recover < -(FOCUS_RECOVER_PATIENCE * 3)) { + self_recover = FOCUS_RECOVER_STEPS / 2 + (lens_true_pos < dac_m ? 1 : 0); + } + } else if (self_recover < 0) { + self_recover += 1; // reset if fine + } + c->self_recover.store(self_recover); +} + void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) { common_camera_process_front(s->sm_front, s->pm, c, cnt); } @@ -2052,116 +2112,30 @@ void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) { // called by processing_thread void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { const CameraBuf *b = &c->buf; - // cache rgb roi and write to cl + update_lapmap(s, b, cnt); + setup_self_recover(c, &s->lapres[0], std::size(s->lapres)); - // gz compensation - s->sm_rear->update(0); - if (s->sm_rear->updated("sensorEvents")) { - float vals[3] = {0.0}; - bool got_accel = false; - auto sensor_events = (*(s->sm_rear))["sensorEvents"].getSensorEvents(); - for (auto sensor_event : sensor_events) { - if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) { - auto v = sensor_event.getAcceleration().getV(); - if (v.size() < 3) { - continue; - } - for (int j = 0; j < 3; j++) { - vals[j] = v[j]; - } - got_accel = true; - break; - } - } - uint64_t ts = nanos_since_boot(); - if (got_accel && ts - s->rear.last_sag_ts > 10000000) { // 10 ms - s->rear.last_sag_ts = ts; - s->rear.last_sag_acc_z = -vals[2]; - } - } - - // sharpness scores - int roi_id = cnt % ARRAYSIZE(s->lapres); // rolling roi - int roi_x_offset = roi_id % (ROI_X_MAX-ROI_X_MIN+1); - int roi_y_offset = roi_id / (ROI_X_MAX-ROI_X_MIN+1); - - for (int r=0;r<(b->rgb_height/NUM_SEGMENTS_Y);r++) { - memcpy(s->rgb_roi_buf.get() + r * (b->rgb_width/NUM_SEGMENTS_X) * 3, - (uint8_t *) b->cur_rgb_buf->addr + \ - (ROI_Y_MIN + roi_y_offset) * b->rgb_height/NUM_SEGMENTS_Y * FULL_STRIDE_X * 3 + \ - (ROI_X_MIN + roi_x_offset) * b->rgb_width/NUM_SEGMENTS_X * 3 + r * FULL_STRIDE_X * 3, - b->rgb_width/NUM_SEGMENTS_X * 3); - } - - assert(clEnqueueWriteBuffer(b->q, s->rgb_conv_roi_cl, true, 0, - b->rgb_width / NUM_SEGMENTS_X * b->rgb_height / NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), s->rgb_roi_buf.get(), 0, 0, 0) == 0); - assert(clSetKernelArg(s->krnl_rgb_laplacian, 0, sizeof(cl_mem), (void *)&s->rgb_conv_roi_cl) == 0); - assert(clSetKernelArg(s->krnl_rgb_laplacian, 1, sizeof(cl_mem), (void *)&s->rgb_conv_result_cl) == 0); - assert(clSetKernelArg(s->krnl_rgb_laplacian, 2, sizeof(cl_mem), (void *)&s->rgb_conv_filter_cl) == 0); - assert(clSetKernelArg(s->krnl_rgb_laplacian, 3, s->conv_cl_localMemSize, 0) == 0); - cl_event conv_event; - assert(clEnqueueNDRangeKernel(b->q, s->krnl_rgb_laplacian, 2, NULL, - s->conv_cl_globalWorkSize, s->conv_cl_localWorkSize, 0, 0, &conv_event) == 0); - clWaitForEvents(1, &conv_event); - clReleaseEvent(conv_event); - - assert(clEnqueueReadBuffer(b->q, s->rgb_conv_result_cl, true, 0, - b->rgb_width / NUM_SEGMENTS_X * b->rgb_height / NUM_SEGMENTS_Y * sizeof(int16_t), s->conv_result.get(), 0, 0, 0) == 0); - - get_lapmap_one(s->conv_result.get(), &s->lapres[roi_id], b->rgb_width / NUM_SEGMENTS_X, b->rgb_height / NUM_SEGMENTS_Y); - - // setup self recover - const float lens_true_pos = s->rear.lens_true_pos; - std::atomic& self_recover = s->rear.self_recover; - if (is_blur(&s->lapres[0]) && - (lens_true_pos < (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_DOWN : OP3T_AF_DAC_DOWN) + 1 || - lens_true_pos > (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_UP : OP3T_AF_DAC_UP) - 1) && - self_recover < 2) { - // truly stuck, needs help - self_recover -= 1; - if (self_recover < -FOCUS_RECOVER_PATIENCE) { - LOGD("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", - lens_true_pos, self_recover.load()); - self_recover = FOCUS_RECOVER_STEPS + ((lens_true_pos < (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_M : OP3T_AF_DAC_M)) ? 1 : 0); // parity determined by which end is stuck at - } - } else if ((lens_true_pos < (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_M - LP3_AF_DAC_3SIG : OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) || - lens_true_pos > (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_M + LP3_AF_DAC_3SIG : OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) && - self_recover < 2) { - // in suboptimal position with high prob, but may still recover by itself - self_recover -= 1; - if (self_recover < -(FOCUS_RECOVER_PATIENCE * 3)) { - self_recover = FOCUS_RECOVER_STEPS / 2 + ((lens_true_pos < (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_M : OP3T_AF_DAC_M)) ? 1 : 0); - } - } else if (self_recover < 0) { - self_recover += 1; // reset if fine - } - - { - MessageBuilder msg; - auto framed = msg.initEvent().initFrame(); - fill_frame_data(framed, b->cur_frame_data, cnt); - if (env_send_rear) { - fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); - } - framed.setFocusVal(kj::ArrayPtr(&s->rear.focus[0], NUM_FOCUS)); - framed.setFocusConf(kj::ArrayPtr(&s->rear.confidence[0], NUM_FOCUS)); - framed.setSharpnessScore(kj::ArrayPtr(&s->lapres[0], ARRAYSIZE(s->lapres))); - framed.setRecoverState(self_recover); - framed.setTransform(kj::ArrayPtr(&b->yuv_transform.v[0], 9)); - s->pm->send("frame", msg); + MessageBuilder msg; + auto framed = msg.initEvent().initFrame(); + fill_frame_data(framed, b->cur_frame_data, cnt); + if (env_send_rear) { + fill_frame_image(framed, (uint8_t *)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); } + framed.setFocusVal(s->rear.focus); + framed.setFocusConf(s->rear.confidence); + framed.setRecoverState(s->rear.self_recover); + framed.setSharpnessScore(s->lapres); + framed.setTransform(b->yuv_transform.v); + s->pm->send("frame", msg); if (cnt % 100 == 3) { - create_thumbnail(s, c, (uint8_t*)b->cur_rgb_buf->addr); + create_thumbnail(s, c, (uint8_t *)b->cur_rgb_buf->addr); } - const int exposure_x = 290; - const int exposure_y = 322; - const int exposure_width = 560; - const int exposure_height = 314; - const int skip = 1; if (cnt % 3 == 0) { - set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip); + const int x = 290, y = 322, width = 560, height = 314; + const int skip = 1; + set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x, x + width, skip, y, y + height, skip); } } @@ -2173,8 +2147,8 @@ void cameras_run(MultiCameraState *s) { ops_thread, s); assert(err == 0); std::vector threads; - threads.push_back(start_process_thread(s, "processing", &s->rear, 51, camera_process_frame)); - threads.push_back(start_process_thread(s, "frontview", &s->front, 51, camera_process_front)); + threads.push_back(start_process_thread(s, "processing", &s->rear, camera_process_frame)); + threads.push_back(start_process_thread(s, "frontview", &s->front, camera_process_front)); CameraState* cameras[2] = {&s->rear, &s->front}; @@ -2288,13 +2262,12 @@ void cameras_close(MultiCameraState *s) { visionbuf_free(&s->focus_bufs[i]); visionbuf_free(&s->stats_bufs[i]); } - clReleaseMemObject(s->rgb_conv_roi_cl); - clReleaseMemObject(s->rgb_conv_result_cl); - clReleaseMemObject(s->rgb_conv_filter_cl); + CL_CHECK(clReleaseMemObject(s->rgb_conv_roi_cl)); + CL_CHECK(clReleaseMemObject(s->rgb_conv_result_cl)); + CL_CHECK(clReleaseMemObject(s->rgb_conv_filter_cl)); - clReleaseProgram(s->prg_rgb_laplacian); - clReleaseKernel(s->krnl_rgb_laplacian); + CL_CHECK(clReleaseKernel(s->krnl_rgb_laplacian)); + CL_CHECK(clReleaseProgram(s->prg_rgb_laplacian)); delete s->sm_front; - delete s->sm_rear; delete s->pm; } diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 63a42812..7bef442d 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -102,8 +102,7 @@ typedef struct CameraState { uint16_t cur_step_pos; uint16_t cur_lens_pos; - uint64_t last_sag_ts; - float last_sag_acc_z; + std::atomic last_sag_acc_z; std::atomic lens_true_pos; std::atomic self_recover; // af recovery counter, neg is patience, pos is active @@ -132,18 +131,10 @@ typedef struct MultiCameraState { cl_program prg_rgb_laplacian; cl_kernel krnl_rgb_laplacian; - std::unique_ptr rgb_roi_buf; - std::unique_ptr conv_result; - - int conv_cl_localMemSize; - size_t conv_cl_globalWorkSize[2]; - size_t conv_cl_localWorkSize[2]; - CameraState rear; CameraState front; SubMaster *sm_front; - SubMaster *sm_rear; PubMaster *pm; } MultiCameraState; diff --git a/selfdrive/camerad/imgproc/utils.cc b/selfdrive/camerad/imgproc/utils.cc index 0b36d686..4aeffac5 100644 --- a/selfdrive/camerad/imgproc/utils.cc +++ b/selfdrive/camerad/imgproc/utils.cc @@ -1,43 +1,36 @@ #include "utils.h" #include #include - +#include // calculate score based on laplacians in one area -void get_lapmap_one(int16_t *lap, uint16_t *res, int x_pitch, int y_pitch) { - int size = x_pitch * y_pitch; +uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) { + const int size = x_pitch * y_pitch; // avg and max of roi - float fsum = 0; - int16_t mean, max; - max = 0; - - for (int i = 0; i < size; i++) { - int x_offset = i % x_pitch; - int y_offset = i / x_pitch; - fsum += lap[x_offset + y_offset*x_pitch]; - max = std::max(lap[x_offset + y_offset*x_pitch], max); + int16_t max = 0; + int sum = 0; + for (int i = 0; i < size; ++i) { + const int16_t v = lap[i % x_pitch + (i / x_pitch) * x_pitch]; + sum += v; + if (v > max) max = v; } - mean = fsum / size; + const int16_t mean = sum / size; // var of roi - float fvar = 0; - for (int i = 0; i < size; i++) { - int x_offset = i % x_pitch; - int y_offset = i / x_pitch; - fvar += (float)((lap[x_offset + y_offset*x_pitch] - mean) * (lap[x_offset + y_offset*x_pitch] - mean)); + int var = 0; + for (int i = 0; i < size; ++i) { + var += std::pow(lap[i % x_pitch + (i / x_pitch) * x_pitch] - mean, 2); } - fvar = fvar / size; - - *res = std::min(5 * fvar + max, (float)65535); + const float fvar = (float)var / size; + return std::min(5 * fvar + max, (float)65535); } -bool is_blur(uint16_t *lapmap) { - int n_roi = (ROI_X_MAX - ROI_X_MIN + 1) * (ROI_Y_MAX - ROI_Y_MIN + 1); +bool is_blur(const uint16_t *lapmap, const size_t size) { float bad_sum = 0; - for (int i = 0; i < n_roi; i++) { - if (*(lapmap + i) < LM_THRESH) { - bad_sum += 1/(float)n_roi; + for (int i = 0; i < size; i++) { + if (lapmap[i] < LM_THRESH) { + bad_sum += 1 / (float)size; } } return (bad_sum > LM_PREC_THRESH); diff --git a/selfdrive/camerad/imgproc/utils.h b/selfdrive/camerad/imgproc/utils.h index 203ac57a..4928f55a 100644 --- a/selfdrive/camerad/imgproc/utils.h +++ b/selfdrive/camerad/imgproc/utils.h @@ -1,8 +1,7 @@ -#ifndef IMGPROC_UTILS -#define IMGPROC_UTILS +#pragma once #include - +#include #define NUM_SEGMENTS_X 8 #define NUM_SEGMENTS_Y 6 @@ -24,7 +23,5 @@ const int16_t lapl_conv_krnl[9] = {0, 1, 0, 1, -4, 1, 0, 1, 0}; -void get_lapmap_one(int16_t *lap, uint16_t *res, int x_pitch, int y_pitch); -bool is_blur(uint16_t *lapmap); - -#endif +uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch); +bool is_blur(const uint16_t *lapmap, const size_t size); diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 4809315d..b6b948fe 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -304,14 +304,14 @@ void* visionserver_thread(void* arg) { void party(cl_device_id device_id, cl_context context) { VisionState state = {}; VisionState *s = &state; - + cameras_init(&s->cameras, device_id, context); cameras_open(&s->cameras); std::thread server_thread(visionserver_thread, s); - + // priority for cameras - int err = set_realtime_priority(51); + int err = set_realtime_priority(53); LOG("setpriority returns %d", err); cameras_run(&s->cameras); @@ -319,8 +319,12 @@ void party(cl_device_id device_id, cl_context context) { server_thread.join(); } +#ifdef QCOM +#include "CL/cl_ext_qcom.h" +#endif + int main(int argc, char *argv[]) { - set_realtime_priority(51); + set_realtime_priority(53); #if defined(QCOM) set_core_affinity(2); #elif defined(QCOM2) @@ -330,13 +334,17 @@ int main(int argc, char *argv[]) { signal(SIGINT, (sighandler_t)set_do_exit); signal(SIGTERM, (sighandler_t)set_do_exit); - int err; - clu_init(); cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); - cl_context context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); - assert(err == 0); + + // TODO: do this for QCOM2 too +#if defined(QCOM) + const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; + cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); +#else + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); +#endif party(device_id, context); - clReleaseContext(context); + CL_CHECK(clReleaseContext(context)); } diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.c b/selfdrive/camerad/transforms/rgb_to_yuv.cc similarity index 61% rename from selfdrive/camerad/transforms/rgb_to_yuv.c rename to selfdrive/camerad/transforms/rgb_to_yuv.cc index 1a36650b..c0ac1920 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv.c +++ b/selfdrive/camerad/transforms/rgb_to_yuv.cc @@ -6,7 +6,6 @@ #include "rgb_to_yuv.h" void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { - int err = 0; memset(s, 0, sizeof(*s)); printf("width %d, height %d, rgb_stride %d\n", width, height, rgb_stride); assert(width % 2 == 0); @@ -21,34 +20,26 @@ void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, i #endif "-DWIDTH=%d -DHEIGHT=%d -DUV_WIDTH=%d -DUV_HEIGHT=%d -DRGB_STRIDE=%d -DRGB_SIZE=%d", width, height, width/ 2, height / 2, rgb_stride, width * height); - cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/rgb_to_yuv.cl", args); + cl_program prg = cl_program_from_file(ctx, device_id, "transforms/rgb_to_yuv.cl", args); - s->rgb_to_yuv_krnl = clCreateKernel(prg, "rgb_to_yuv", &err); - assert(err == 0); + s->rgb_to_yuv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb_to_yuv", &err)); // done with this - err = clReleaseProgram(prg); - assert(err == 0); + CL_CHECK(clReleaseProgram(prg)); } void rgb_to_yuv_destroy(RGBToYUVState* s) { - int err = 0; - err = clReleaseKernel(s->rgb_to_yuv_krnl); - assert(err == 0); + CL_CHECK(clReleaseKernel(s->rgb_to_yuv_krnl)); } void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) { - int err = 0; - err = clSetKernelArg(s->rgb_to_yuv_krnl, 0, sizeof(cl_mem), &rgb_cl); - assert(err == 0); - err = clSetKernelArg(s->rgb_to_yuv_krnl, 1, sizeof(cl_mem), &yuv_cl); - assert(err == 0); + CL_CHECK(clSetKernelArg(s->rgb_to_yuv_krnl, 0, sizeof(cl_mem), &rgb_cl)); + CL_CHECK(clSetKernelArg(s->rgb_to_yuv_krnl, 1, sizeof(cl_mem), &yuv_cl)); const size_t work_size[2] = { (size_t)(s->width + (s->width % 4 == 0 ? 0 : (4 - s->width % 4))) / 4, (size_t)(s->height + (s->height % 4 == 0 ? 0 : (4 - s->height % 4))) / 4 }; cl_event event; - err = clEnqueueNDRangeKernel(q, s->rgb_to_yuv_krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event); - assert(err == 0); - clWaitForEvents(1, &event); - clReleaseEvent(event); + CL_CHECK(clEnqueueNDRangeKernel(q, s->rgb_to_yuv_krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event)); + CL_CHECK(clWaitForEvents(1, &event)); + CL_CHECK(clReleaseEvent(event)); } diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.h b/selfdrive/camerad/transforms/rgb_to_yuv.h index 79895803..05d5c5ec 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv.h +++ b/selfdrive/camerad/transforms/rgb_to_yuv.h @@ -1,5 +1,4 @@ -#ifndef RGB_TO_YUV_H -#define RGB_TO_YUV_H +#pragma once #include #include @@ -10,10 +9,6 @@ #include #endif -#ifdef __cplusplus -extern "C" { -#endif - typedef struct { int width, height; cl_kernel rgb_to_yuv_krnl; @@ -24,9 +19,3 @@ void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, i void rgb_to_yuv_destroy(RGBToYUVState* s); void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl); - -#ifdef __cplusplus -} -#endif - -#endif // RGB_TO_YUV_H diff --git a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc index 9d68e5b9..a2fd4035 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc +++ b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc @@ -41,9 +41,8 @@ static inline double millis_since_boot() { } void cl_init(cl_device_id &device_id, cl_context &context) { - int err; device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); - context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); + context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); } @@ -102,7 +101,6 @@ bool compare_results(uint8_t *a, uint8_t *b, int len, int stride, int width, int int main(int argc, char** argv) { srand(1337); - clu_init(); cl_device_id device_id; cl_context context; cl_init(device_id, context) ; @@ -137,13 +135,13 @@ int main(int argc, char** argv) { rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, width, height, width * 3); int frame_yuv_buf_size = width * height * 3 / 2; - cl_mem yuv_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, frame_yuv_buf_size, (void*)NULL, &err); + cl_mem yuv_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, frame_yuv_buf_size, (void*)NULL, &err)); uint8_t *frame_yuv_buf = new uint8_t[frame_yuv_buf_size]; uint8_t *frame_yuv_ptr_y = frame_yuv_buf; uint8_t *frame_yuv_ptr_u = frame_yuv_buf + (width * height); uint8_t *frame_yuv_ptr_v = frame_yuv_ptr_u + ((width/2) * (height/2)); - cl_mem rgb_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, width * height * 3, (void*)NULL, &err); + cl_mem rgb_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, width * height * 3, (void*)NULL, &err)); int mismatched = 0; int counter = 0; srand (time(NULL)); diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 2af6df26..77e24b4b 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -5,6 +5,7 @@ from selfdrive.version import comma_remote, tested_branch from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars from selfdrive.car.vin import get_vin, VIN_UNKNOWN from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car +from selfdrive.hardware import EON from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint @@ -23,6 +24,8 @@ def get_startup_event(car_recognized, controller_available): event = EventName.startupNoCar elif car_recognized and not controller_available: event = EventName.startupNoControl + elif EON and "letv" not in open("/proc/cmdline").read(): + event = EventName.startupOneplus return event diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 9dd78e4d..47e6090a 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -35,14 +35,12 @@ 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): - - if apply_brake == 0: - mode = 0x1 - else: + mode = 0x1 + if apply_brake > 0: mode = 0xa + if at_full_stop: + mode = 0xd - 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. #elif near_stop: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 6d474dcd..73dd2ca3 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -16,6 +16,7 @@ A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName + def compute_gb_honda(accel, speed): creep_brake = 0.0 creep_speed = 2.3 diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 5eca73c2..e4e0dd40 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -358,6 +358,7 @@ FW_VERSIONS = { b'37805-5BA-L930\x00\x00', b'37805-5BA-L940\x00\x00', b'37805-5BA-L960\x00\x00', + b'37805-5AG-Q710\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A040\x00\x00', @@ -369,23 +370,27 @@ FW_VERSIONS = { b'28101-5DJ-A040\x00\x00', b'28101-5DJ-A060\x00\x00', b'28101-5DJ-A510\x00\x00', + b'28101-5CG-A320\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TBA-A540\x00\x00', b'57114-TBA-A550\x00\x00', b'57114-TBA-A560\x00\x00', b'57114-TBA-A570\x00\x00', + b'57114-TEA-Q220\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBA,A030\x00\x00', b'39990-TBA-A030\x00\x00', b'39990-TBG-A030\x00\x00', b'39990-TEG-A010\x00\x00', + b'39990-TEA-T020\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TBA-A030\x00\x00', b'77959-TBA-A040\x00\x00', b'77959-TBG-A030\x00\x00', + b'77959-TEA-Q820\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBA-A510\x00\x00', @@ -401,6 +406,7 @@ FW_VERSIONS = { b'78109-TBC-C530\x00\x00', b'78109-TBH-A530\x00\x00', b'78109-TEG-A310\x00\x00', + b'78109-TED-Q510\x00\x00', ], (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TBA-A020\x00\x00', @@ -410,6 +416,7 @@ FW_VERSIONS = { b'36161-TBC-A030\x00\x00', b'36161-TEG-A010\x00\x00', b'36161-TEG-A020\x00\x00', + b'36161-TED-Q320\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TBA-A010\x00\x00', @@ -447,6 +454,7 @@ FW_VERSIONS = { b'37805-5BB-C630\x00\x00', b'37805-5BB-L540\x00\x00', b'37805-5BB-L640\x00\x00', + b'37805-5AZ-G740\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A920\x00\x00', @@ -460,9 +468,11 @@ FW_VERSIONS = { b'28101-5CK-A150\x00\x00', b'28101-5CK-C130\x00\x00', b'28101-5CK-C140\x00\x00', + b'28101-5CK-C150\x00\x00', b'28101-5DJ-A610\x00\x00', b'28101-5DJ-A710\x00\x00', b'28101-5DV-E330\x00\x00', + b'28101-5DV-E610\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TBG-A340\x00\x00', @@ -472,7 +482,6 @@ FW_VERSIONS = { b'57114-TGG-L320\x00\x00', b'57114-TGG-L330\x00\x00', b'57114-TGL-G330\x00\x00', - ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBA-C020\x00\x00', @@ -506,6 +515,7 @@ FW_VERSIONS = { b'78109-TGG-A810\x00\x00', b'78109-TGG-A820\x00\x00', b'78109-TGL-G120\x00\x00', + b'78109-TGL-G130\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TBA-A150\x00\x00', @@ -523,6 +533,7 @@ FW_VERSIONS = { b'36161-TGG-A080\x00\x00', b'36161-TGG-A120\x00\x00', b'36161-TGL-G050\x00\x00', + b'36161-TGL-G070\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TBA-A110\x00\x00', @@ -869,17 +880,43 @@ FW_VERSIONS = { ], }, CAR.ACURA_RDX_3G: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [b'37805-5YF-A420\x00\x00'], - (Ecu.vsa, 0x18da28f1, None): [b'57114-TJB-A040\x00\x00'], - (Ecu.fwdRadar, 0x18dab0f1, None): [b'36802-TJB-A040\x00\x00'], - (Ecu.fwdCamera, 0x18dab5f1, None): [b'36161-TJB-A040\x00\x00'], - (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-TJB-A520\x00\x00'], - (Ecu.transmission, 0x18da1ef1, None): [b'28102-5YK-A700\x00\x00'], - (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-TJB-AB10\x00\x00'], - (Ecu.srs, 0x18da53f1, None): [b'77959-TJB-A040\x00\x00'], - (Ecu.electricBrakeBooster, 0x18da2bf1, None): [b'46114-TJB-A050\x00\x00'], - (Ecu.gateway, 0x18daeff1, None): [b'38897-TJB-A110\x00\x00'], - (Ecu.eps, 0x18da30f1, None): [b'39990-TJB-A030\x00\x00'], + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-5YF-A230\x00\x00', + b'37805-5YF-A420\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TJB-A040\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TJB-A040\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TJB-A040\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TJB-A520\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28102-5YK-A700\x00\x00', + b'28102-5YK-A711\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TJB-AB10\x00\x00', + b'78109-TJB-AF10\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TJB-A040\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TJB-A050\x00\x00', + b'46114-TJB-A060\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TJB-A110\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TJB-A030\x00\x00', + ], }, CAR.RIDGELINE: { (Ecu.eps, 0x18da30f1, None): [ @@ -946,6 +983,7 @@ FW_VERSIONS = { CAR.HRV: { (Ecu.gateway, 0x18daeff1, None): [ b'38897-T7A-A010\x00\x00', + b'38897-T7A-A110\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-THX-A020\x00\x00', @@ -953,6 +991,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-T7A-A140\x00\x00', b'36161-T7A-A240\x00\x00', + b'36161-T7A-C440\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-T7A-A230\x00\x00', @@ -960,8 +999,23 @@ FW_VERSIONS = { (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-THX-A110\x00\x00', b'78109-THX-A210\x00\x00', + b'78109-THX-C220\x00\x00', ], }, + CAR.ACURA_ILX: { + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TX6-A010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-TX6-A030\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TX6-C210\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T3R-A120\x00\x00', + ], + }, } DBC = { diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4a296ab9..4483d7b2 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -80,7 +80,7 @@ class CarController(): self.last_resume_frame = frame # 20 Hz LFA MFA message - if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV]: + if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.IONIQ_EV_2020]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) return can_sends diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 81398c23..9f064edf 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -17,7 +17,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_ActToi"] = steer_req values["CF_Lkas_MsgCount"] = frame % 0x10 - if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV]: + if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.SANTA_FE, CAR.IONIQ_EV_2020]: values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 4300e61d..47c468dd 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -94,15 +94,16 @@ class CarInterface(CarInterfaceBase): 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 in [CAR.IONIQ, CAR.IONIQ_EV_LTD]: + elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020]: 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.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 + 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]] - ret.minSteerSpeed = 32 * CV.MPH_TO_MS + if candidate != CAR.IONIQ_EV_2020: + ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.VELOSTER: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG @@ -178,9 +179,8 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - # these cars require a special panda safety mode due to missing counters and checksums in the messages - if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019, + if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80]: ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy @@ -205,9 +205,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid + ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False events = self.create_common_events(ret) - #TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event + # TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 8f1512c2..94712a0c 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu # Steer torque limits class SteerLimitParams: def __init__(self, CP): - if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70]: + if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020]: self.STEER_MAX = 384 else: self.STEER_MAX = 255 @@ -23,8 +23,9 @@ class CAR: ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017" ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" - IONIQ = "HYUNDAI IONIQ ELECTRIC PREMIUM SE 2020" + IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019" IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019" + IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020" KONA = "HYUNDAI KONA 2020" KONA_EV = "HYUNDAI KONA ELECTRIC 2019" SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" @@ -120,13 +121,13 @@ FINGERPRINTS = { CAR.GENESIS_G90: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1162: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2003: 8, 2004: 8, 2005: 8, 2008: 8, 2011: 8, 2012: 8, 2013: 8 }], + CAR.IONIQ_EV_2020: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 + }], CAR.IONIQ_EV_LTD: [{ 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1507: 8, 1535: 8 }], CAR.IONIQ: [{ - 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 - }, - { 68:8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 8, 576:8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1473: 8, 1476: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 }], CAR.KONA: [{ @@ -164,16 +165,19 @@ FW_VERSIONS = { b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ', b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ', + b'\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ', ], (Ecu.esp, 0x7d1, None): [ b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02', b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100\xf1\xa01.04', ], (Ecu.engine, 0x7e0, None): [ b'HM6M2_0a0_BD0', b'\xf1\x87391162M003\xf1\xa0000F', b'\xf1\x87391162M003\xf1\xa00240', + b'HM6M1_0a0_F00', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103\xf1\xa01.03', @@ -188,6 +192,7 @@ FW_VERSIONS = { (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', ], }, CAR.SANTA_FE: { @@ -310,15 +315,15 @@ FEATURES = { # which message has the gear "use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]), "use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_2019, CAR.VELOSTER]), - "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]), + "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020]), # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 - "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]), + "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]), - "use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA]), + "use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA, CAR.IONIQ_EV_2020]), } -EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV]) +EV_HYBRID = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV]) DBC = { CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), @@ -327,6 +332,7 @@ DBC = { 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_EV_2020: dbc_dict('hyundai_kia_generic', None), CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None), CAR.IONIQ: dbc_dict('hyundai_kia_generic', None), CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index f7078ed2..746dabb9 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,13 +1,15 @@ import os import time +from typing import Dict + from cereal import car from common.kalman.simple_kalman import KF1D from common.realtime import DT_CTRL from selfdrive.car import gen_empty_fingerprint from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName @@ -15,6 +17,7 @@ MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 144 + 4 = 92 mph # generic car and radar interfaces + class CarInterfaceBase(): def __init__(self, CP, CarController, CarState): self.CP = CP @@ -50,7 +53,7 @@ class CarInterfaceBase(): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate - ret.isPandaBlack = True # TODO: deprecate this field + ret.isPandaBlack = True # TODO: deprecate this field # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque @@ -68,6 +71,9 @@ class CarInterfaceBase(): ret.brakeMaxV = [1.] ret.openpilotLongitudinalControl = False ret.startAccel = 0.0 + ret.minSpeedCan = 0.3 + ret.stoppingBrakeRate = 0.2 # brake_travel/s while trying to stop + ret.startingBrakeRate = 0.8 # brake_travel/s while releasing on restart ret.stoppingControl = False ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] @@ -132,6 +138,7 @@ class CarInterfaceBase(): return events + class RadarInterfaceBase(): def __init__(self, CP): self.pts = {} @@ -145,6 +152,7 @@ class RadarInterfaceBase(): time.sleep(self.radar_ts) # radard runs on RI updates return ret + class CarStateBase: def __init__(self, CP): self.CP = CP @@ -175,10 +183,13 @@ class CarStateBase: return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0 @staticmethod - def parse_gear_shifter(gear): - return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral, - 'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive, - 'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake}.get(gear, GearShifter.unknown) + def parse_gear_shifter(gear: str) -> car.CarState.GearShifter: + d: Dict[str, car.CarState.GearShifter] = { + 'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral, + 'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive, + 'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake + } + return d.get(gear, GearShifter.unknown) @staticmethod def get_cam_can_parser(CP): diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fc121945..a7ab0ee0 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -52,7 +52,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] ret.lateralTuning.pid.kf = 0.00006 - # No steer below disable speed ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 9f2ac20e..1a003a88 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -4,6 +4,7 @@ from selfdrive.car.nissan.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase + class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 8012624a..0cfb6fb4 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -62,7 +62,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] if candidate in [CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]: - ret.safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal + ret.safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal ret.mass = 1568 + STD_CARGO_KG ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 937728d8..88c4ec69 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -27,12 +27,8 @@ FINGERPRINTS = { 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8 }], CAR.FORESTER: [{ - # Forester Sport 2019 - 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 552: 8, 557: 8, 576: 8, 577: 8, 722: 8, 808: 8, 811: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1698: 8, 1722: 8, 1743: 8, 1759: 8, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8 - }, - # Forester 2019 - { - 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1722: 8, 1759: 8, 1787: 5, 1788: 8 + # Forester 2019-2020 + 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 811: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 961: 8, 984: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1698: 8, 1722: 8, 1743: 8, 1759: 8, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8 }], CAR.OUTBACK_PREGLOBAL: [{ # OUTBACK PREMIUM 2.5i 2015 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 96e25013..b76b66b3 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -4,7 +4,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ create_fcw_command -from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams +from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, SteerLimitParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -38,7 +38,6 @@ class CarController(): self.last_standstill = False self.standstill_req = False - self.last_fault_frame = -200 self.steer_rate_limited = False self.fake_ecus = set() @@ -73,12 +72,8 @@ class CarController(): apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer - # only cut torque when steer state is a known fault - if CS.steer_state in [9, 25]: - self.last_fault_frame = frame - - # Cut steering for 2s after fault - if not enabled or (frame - self.last_fault_frame < 200): + # Cut steering while we're in a known fault state (2s) + if not enabled or CS.steer_state in [9, 25]: apply_steer = 0 apply_steer_req = 0 else: @@ -89,7 +84,7 @@ class CarController(): pcm_cancel_cmd = 1 # on entering standstill, send standstill request - if CS.out.standstill and not self.last_standstill: + if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 1fa84f91..b2a3c70b 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -8,6 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase EventName = car.CarEvent.EventName + class CarInterface(CarInterfaceBase): @staticmethod def compute_gb(accel, speed): @@ -121,7 +122,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]] ret.lateralTuning.pid.kf = 0.00006 - elif candidate in [CAR.CAMRY, CAR.CAMRYH]: + elif candidate in [CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.82448 @@ -195,6 +196,7 @@ class CarInterface(CarInterfaceBase): elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True + ret.minSpeedCan = 0.375 ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.9 @@ -256,7 +258,7 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.PRIUS_TSS2: stop_and_go = True ret.safetyParam = 73 - ret.wheelbase = 2.70002 #from toyota online sepc. + ret.wheelbase = 2.70002 # from toyota online sepc. ret.steerRatio = 13.4 # True steerRation from older prius tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 924e02ef..6ef056ff 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -25,6 +25,7 @@ class CAR: CHRH = "TOYOTA C-HR HYBRID 2018" CAMRY = "TOYOTA CAMRY 2018" CAMRYH = "TOYOTA CAMRY HYBRID 2018" + CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 HIGHLANDER = "TOYOTA HIGHLANDER 2017" HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020" HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" @@ -175,6 +176,9 @@ FINGERPRINTS = { { 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.CAMRY_TSS2: [{ + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 791: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1593: 8, 1594: 8, 1595: 8, 1649: 8, 1653: 8, 1654: 8, 1655: 8, 1677: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1800: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1841: 8, 1848: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1940: 8, 1941: 8, 1945: 8, 1948: 8, 1949: 8, 1952: 8, 1953: 8, 1956: 8, 1960: 8, 1961: 8, 1964: 8, 1968: 8, 1973: 8, 1976: 8, 1981: 8, 1986: 8, 1988: 8, 1990: 8, 1994: 8, 1996: 8, 1998: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2012: 8, 2016: 8, 2017: 8 + }], CAR.HIGHLANDER: [{ 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8 }, @@ -276,7 +280,8 @@ FINGERPRINTS = { } # Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection -IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, CAR.LEXUS_NX] +IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, + CAR.LEXUS_NX, CAR.CAMRY_TSS2] FW_VERSIONS = { CAR.AVALON: { @@ -433,6 +438,23 @@ FW_VERSIONS = { b'8646F0607100 ', ], }, + CAR.CAMRY_TSS2: { + (Ecu.eps, 0x7a1, None): [ + b'8965B33630\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'\x01F152606400\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x018966306Q5000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 15): [ + b'\x018821F6201200\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 109): [ + b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + ], + }, CAR.CHR: { (Ecu.engine, 0x700, None): [ b'\x01896631017100\x00\x00\x00\x00', @@ -708,25 +730,31 @@ FW_VERSIONS = { CAR.HIGHLANDER_TSS2: { (Ecu.eps, 0x7a1, None): [ b'8965B48241\x00\x00\x00\x00\x00\x00', + b'8965B48310\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'\x01F15260E051\x00\x00\x00\x00\x00\x00', + b'\x01F15260E110\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x01896630E62200\x00\x00\x00\x00', b'\x01896630E64100\x00\x00\x00\x00', b'\x01896630E64200\x00\x00\x00\x00', + b'\x01896630EB2000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201200\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', ], }, CAR.HIGHLANDERH_TSS2: { (Ecu.eps, 0x7a1, None): [ b'8965B48241\x00\x00\x00\x00\x00\x00', + b'8965B48310\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'\x01F15264872300\x00\x00\x00\x00', @@ -734,12 +762,15 @@ FW_VERSIONS = { ], (Ecu.engine, 0x700, None): [ b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201200\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', ], }, CAR.LEXUS_IS: { @@ -747,6 +778,7 @@ FW_VERSIONS = { b'\x018966353M7100\x00\x00\x00\x00', b'\x018966353Q2300\x00\x00\x00\x00', b'\x018966353R1100\x00\x00\x00\x00', + b'\x018966353R7100\x00\x00\x00\x00', b'\x018966353R8100\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ @@ -764,6 +796,7 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B53271\x00\x00\x00\x00\x00\x00', b'8965B53280\x00\x00\x00\x00\x00\x00', + b'8965B53281\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702300\x00\x00\x00\x00', @@ -948,6 +981,7 @@ FW_VERSIONS = { b'\x01896634A19100\x00\x00\x00\x00', b'\x01896634A20000\x00\x00\x00\x00', b'\x01896634A22000\x00\x00\x00\x00', + b'\x01896634A44000\x00\x00\x00\x00', b'\x01896634A45000\x00\x00\x00\x00', b'\x01896634A46000\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', @@ -955,9 +989,9 @@ FW_VERSIONS = { b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00', b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ - b'F152642520\x00\x00\x00\x00\x00\x00', b'\x01F15260R210\x00\x00\x00\x00\x00\x00', b'\x01F15260R220\x00\x00\x00\x00\x00\x00', b'\x01F15260R300\x00\x00\x00\x00\x00\x00', @@ -965,6 +999,7 @@ FW_VERSIONS = { b'\x01F152642561\x00\x00\x00\x00\x00\x00', b'\x01F152642700\x00\x00\x00\x00\x00\x00', b'\x01F152642710\x00\x00\x00\x00\x00\x00', + b'\x01F152642711\x00\x00\x00\x00\x00\x00', b'\x01F152642750\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -987,14 +1022,18 @@ FW_VERSIONS = { b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4203500\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4203700\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', ], }, CAR.RAV4H_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896634A15000\x00\x00\x00\x00', b'\x018966342M5000\x00\x00\x00\x00', - b'\x018966342X6000\x00\x00\x00\x00', b'\x018966342W8000\x00\x00\x00\x00', + b'\0018966342X5000\x00\x00\x00\x00', + b'\x018966342X6000\x00\x00\x00\x00', + b'\x01896634A25000\x00\x00\x00\x00', + b'\x018966342W5000\x00\x00\x00\x00', b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', @@ -1006,6 +1045,7 @@ FW_VERSIONS = { b'F152642331\x00\x00\x00\x00\x00\x00', b'F152642531\x00\x00\x00\x00\x00\x00', b'F152642532\x00\x00\x00\x00\x00\x00', + b'F152642520\x00\x00\x00\x00\x00\x00', b'F152642521\x00\x00\x00\x00\x00\x00', b'F152642540\x00\x00\x00\x00\x00\x00', b'F152642541\x00\x00\x00\x00\x00\x00', @@ -1014,6 +1054,7 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B42170\x00\x00\x00\x00\x00\x00', b'8965B42171\x00\x00\x00\x00\x00\x00', + b'8965B42180\x00\x00\x00\x00\x00\x00', b'8965B42181\x00\x00\x00\x00\x00\x00', b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00', @@ -1030,6 +1071,7 @@ FW_VERSIONS = { b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4203500\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4203700\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', ], }, CAR.LEXUS_ES_TSS2: { @@ -1235,10 +1277,12 @@ FW_VERSIONS = { }, CAR.LEXUS_RX_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x01896634D12000\x00\x00\x00\x00', b'\x01896630EB0000\x00\x00\x00\x00', b'\x01896630EA9000\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ + b'\x01F152648801\x00\x00\x00\x00\x00\x00', b'\x01F15260E031\x00\x00\x00\x00\x00\x00', b'\x01F15260E041\x00\x00\x00\x00\x00\x00', ], @@ -1251,6 +1295,7 @@ FW_VERSIONS = { b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', ], }, @@ -1305,6 +1350,7 @@ DBC = { CAR.CHRH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'), CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_adas'), + CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'), CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'), @@ -1324,6 +1370,12 @@ DBC = { CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), } -NO_DSU_CAR = set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]) -TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2]) -NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2]) # no resume button press required + +# Toyota/Lexus Safety Sense 2.0 and 2.5 +TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, + CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2]) + +NO_DSU_CAR = TSS2_CAR | set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]) + +# no resume button press required +NO_STOP_TIMER_CAR = TSS2_CAR | set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA]) diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 25230632..63da6672 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -6,6 +6,7 @@ from selfdrive.car.interfaces import CarInterfaceBase GEAR = car.CarState.GearShifter EventName = car.CarEvent.EventName + class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index f70abf58..fdca02e8 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -12,14 +12,13 @@ _visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c']) files = [ 'buffering.c', - 'clutil.c', + 'clutil.cc', 'efd.c', 'glutil.c', 'visionimg.cc', ] if arch == "aarch64": - defines = {} files += [ 'framebuffer.cc', 'touch.c', @@ -30,17 +29,15 @@ if arch == "aarch64": files += ['visionbuf_ion.c'] _gpu_libs = ['gui', 'adreno_utils'] elif arch == "larch64": - defines = {"CLU_NO_CACHE": None} files += [ 'visionbuf_ion.c', ] _gpu_libs = ['GL'] else: - defines = {"CLU_NO_CACHE": None} files += [ 'visionbuf_cl.c', ] _gpu_libs = ["GL"] -_gpucommon = fxn('gpucommon', files, CPPDEFINES=defines, LIBS=_gpu_libs) +_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs) Export('_common', '_visionipc', '_gpucommon', '_gpu_libs') diff --git a/selfdrive/common/clutil.c b/selfdrive/common/clutil.c deleted file mode 100644 index dbfc5545..00000000 --- a/selfdrive/common/clutil.c +++ /dev/null @@ -1,459 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef __APPLE__ -#include -#else -#include -#endif - -#include "common/util.h" - -#include "clutil.h" - -typedef struct CLUProgramIndex { - uint64_t index_hash; - const uint8_t* bin_data; - const uint8_t* bin_end; -} CLUProgramIndex; - -#ifdef CLU_NO_SRC -#include "clcache_bins.h" -#else -static const CLUProgramIndex clu_index[] = {}; -#endif - -void clu_init(void) { -#ifndef CLU_NO_SRC - mkdir("/tmp/clcache", 0777); - unlink("/tmp/clcache/index.cli"); -#endif -} - -cl_device_id cl_get_device_id(cl_device_type device_type) { - bool opencl_platform_found = false; - cl_device_id device_id = NULL; - - cl_uint num_platforms = 0; - int err = clGetPlatformIDs(0, NULL, &num_platforms); - assert(err == 0); - cl_platform_id* platform_ids = malloc(sizeof(cl_platform_id) * num_platforms); - err = clGetPlatformIDs(num_platforms, platform_ids, NULL); - assert(err == 0); - - char cBuffer[1024]; - for (size_t i = 0; i < num_platforms; i++) { - err = clGetPlatformInfo(platform_ids[i], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL); - assert(err == 0); - printf("platform[%zu] CL_PLATFORM_NAME: %s\n", i, cBuffer); - - cl_uint num_devices; - err = clGetDeviceIDs(platform_ids[i], device_type, 0, NULL, &num_devices); - if (err != 0 || !num_devices) { - continue; - } - // Get first device - err = clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL); - assert(err == 0); - cl_print_info(platform_ids[i], device_id); - opencl_platform_found = true; - break; - } - free(platform_ids); - - if (!opencl_platform_found) { - printf("No valid openCL platform found\n"); - assert(opencl_platform_found); - } - return device_id; -} - -cl_program cl_create_program_from_file(cl_context ctx, const char* path) { - char* src_buf = read_file(path, NULL); - assert(src_buf); - - int err = 0; - cl_program ret = clCreateProgramWithSource(ctx, 1, (const char**)&src_buf, NULL, &err); - assert(err == 0); - - free(src_buf); - - return ret; -} - -static char* get_version_string(cl_platform_id platform) { - size_t size = 0; - int err; - err = clGetPlatformInfo(platform, CL_PLATFORM_VERSION, 0, NULL, &size); - assert(err == 0); - char *str = malloc(size); - assert(str); - err = clGetPlatformInfo(platform, CL_PLATFORM_VERSION, size, str, NULL); - assert(err == 0); - return str; -} - -void cl_print_info(cl_platform_id platform, cl_device_id device) { - char str[4096]; - - clGetPlatformInfo(platform, CL_PLATFORM_VENDOR, sizeof(str), str, NULL); - printf("vendor: '%s'\n", str); - - char* version = get_version_string(platform); - printf("platform version: '%s'\n", version); - free(version); - - clGetPlatformInfo(platform, CL_PLATFORM_PROFILE, sizeof(str), str, NULL); - printf("profile: '%s'\n", str); - - clGetPlatformInfo(platform, CL_PLATFORM_EXTENSIONS, sizeof(str), str, NULL); - printf("extensions: '%s'\n", str); - - clGetDeviceInfo(device, CL_DEVICE_NAME, sizeof(str), str, NULL); - printf("name: '%s'\n", str); - - clGetDeviceInfo(device, CL_DEVICE_VERSION, sizeof(str), str, NULL); - printf("device version: '%s'\n", str); - - size_t sz; - clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(sz), &sz, NULL); - printf("max work group size: %zu\n", sz); - - cl_device_type type; - clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(type), &type, NULL); - printf("type = 0x%04x = ", (unsigned int)type); - switch(type) { - case CL_DEVICE_TYPE_CPU: - printf("CL_DEVICE_TYPE_CPU\n"); - break; - case CL_DEVICE_TYPE_GPU: - printf("CL_DEVICE_TYPE_GPU\n"); - break; - case CL_DEVICE_TYPE_ACCELERATOR: - printf("CL_DEVICE_TYPE_ACCELERATOR\n"); - break; - default: - printf("Other...\n" ); - break; - } -} - -void cl_print_build_errors(cl_program program, cl_device_id device) { - cl_build_status status; - clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_STATUS, - sizeof(cl_build_status), &status, NULL); - - size_t log_size; - clGetProgramBuildInfo(program, device, - CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size); - - char* log = calloc(log_size+1, 1); - assert(log); - - clGetProgramBuildInfo(program, device, - CL_PROGRAM_BUILD_LOG, log_size+1, log, NULL); - - printf("build failed; status=%d, log:\n%s\n", - status, log); - - free(log); -} - -uint64_t clu_index_hash(const char* s) { - size_t sl = strlen(s); - assert(sl < 128); - uint64_t x = 0; - for (int i=127; i>=0; i--) { - x *= 65599ULL; - x += (uint8_t)s[i> 32); -} - -uint64_t clu_fnv_hash(const uint8_t *data, size_t len) { - /* 64 bit Fowler/Noll/Vo FNV-1a hash code */ - uint64_t hval = 0xcbf29ce484222325ULL; - const uint8_t *dp = data; - const uint8_t *de = data + len; - while (dp < de) { - hval ^= (uint64_t) *dp++; - hval += (hval << 1) + (hval << 4) + (hval << 5) + - (hval << 7) + (hval << 8) + (hval << 40); - } - - return hval; -} - -cl_program cl_cached_program_from_hash(cl_context ctx, cl_device_id device_id, uint64_t hash) { - int err; - - char cache_path[1024]; - snprintf(cache_path, sizeof(cache_path), "/tmp/clcache/%016" PRIx64 ".clb", hash); - - size_t bin_size; - uint8_t *bin = read_file(cache_path, &bin_size); - if (!bin) { - return NULL; - } - - cl_program prg = clCreateProgramWithBinary(ctx, 1, &device_id, &bin_size, (const uint8_t**)&bin, NULL, &err); - assert(err == 0); - - free(bin); - - err = clBuildProgram(prg, 1, &device_id, NULL, NULL, NULL); - assert(err == 0); - - return prg; -} - -#ifndef CLU_NO_CACHE -static uint8_t* get_program_binary(cl_program prg, size_t *out_size) { - int err; - - cl_uint num_devices; - err = clGetProgramInfo(prg, CL_PROGRAM_NUM_DEVICES, sizeof(num_devices), &num_devices, NULL); - assert(err == 0); - assert(num_devices == 1); - - size_t binary_size = 0; - err = clGetProgramInfo(prg, CL_PROGRAM_BINARY_SIZES, sizeof(binary_size), &binary_size, NULL); - assert(err == 0); - assert(binary_size > 0); - - uint8_t *binary_buf = malloc(binary_size); - assert(binary_buf); - - uint8_t* bufs[1] = { binary_buf, }; - - err = clGetProgramInfo(prg, CL_PROGRAM_BINARIES, sizeof(bufs), &bufs, NULL); - assert(err == 0); - - *out_size = binary_size; - return binary_buf; -} -#endif - -cl_program cl_cached_program_from_string(cl_context ctx, cl_device_id device_id, - const char* src, const char* args, - uint64_t *out_hash) { - int err; - - cl_platform_id platform; - err = clGetDeviceInfo(device_id, CL_DEVICE_PLATFORM, sizeof(platform), &platform, NULL); - assert(err == 0); - - const char* platform_version = get_version_string(platform); - - const size_t hash_len = strlen(platform_version)+1+strlen(src)+1+strlen(args)+1; - char* hash_buf = malloc(hash_len); - assert(hash_buf); - memset(hash_buf, 0, hash_len); - snprintf(hash_buf, hash_len, "%s%c%s%c%s", platform_version, 1, src, 1, args); - free((void*)platform_version); - - uint64_t hash = clu_fnv_hash((uint8_t*)hash_buf, hash_len); - free(hash_buf); - - cl_program prg = NULL; -#ifndef CLU_NO_CACHE - prg = cl_cached_program_from_hash(ctx, device_id, hash); -#endif - if (prg == NULL) { - prg = clCreateProgramWithSource(ctx, 1, (const char**)&src, NULL, &err); - assert(err == 0); - - err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); - if (err != 0) { - cl_print_build_errors(prg, device_id); - } - assert(err == 0); - -#ifndef CLU_NO_CACHE - // write program binary to cache - - size_t binary_size; - uint8_t *binary_buf = get_program_binary(prg, &binary_size); - - char cache_path[1024]; - snprintf(cache_path, sizeof(cache_path), "/tmp/clcache/%016" PRIx64 ".clb", hash); - FILE* of = fopen(cache_path, "wb"); - assert(of); - fwrite(binary_buf, 1, binary_size, of); - fclose(of); - - free(binary_buf); -#endif - } - - if (out_hash) *out_hash = hash; - return prg; -} - -cl_program cl_cached_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args, - uint64_t *out_hash) { - char* src_buf = read_file(path, NULL); - assert(src_buf); - cl_program ret = cl_cached_program_from_string(ctx, device_id, src_buf, args, out_hash); - free(src_buf); - return ret; -} - -#ifndef CLU_NO_CACHE -static void add_index(uint64_t index_hash, uint64_t src_hash) { - FILE *f = fopen("/tmp/clcache/index.cli", "a"); - assert(f); - fprintf(f, "%016" PRIx64 " %016" PRIx64 "\n", index_hash, src_hash); - fclose(f); -} -#endif - -cl_program cl_program_from_index(cl_context ctx, cl_device_id device_id, uint64_t index_hash) { - int err; - - int i; - for (i=0; i= ARRAYSIZE(clu_index)) { - assert(false); - } - - size_t bin_size = clu_index[i].bin_end - clu_index[i].bin_data; - const uint8_t *bin_data = clu_index[i].bin_data; - - cl_program prg = clCreateProgramWithBinary(ctx, 1, &device_id, &bin_size, (const uint8_t**)&bin_data, NULL, &err); - assert(err == 0); - - err = clBuildProgram(prg, 1, &device_id, NULL, NULL, NULL); - assert(err == 0); - - return prg; -} - -cl_program cl_index_program_from_string(cl_context ctx, cl_device_id device_id, - const char* src, const char* args, - uint64_t index_hash) { - uint64_t src_hash = 0; - cl_program ret = cl_cached_program_from_string(ctx, device_id, src, args, &src_hash); -#ifndef CLU_NO_CACHE - add_index(index_hash, src_hash); -#endif - return ret; -} - -cl_program cl_index_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args, - uint64_t index_hash) { - uint64_t src_hash = 0; - cl_program ret = cl_cached_program_from_file(ctx, device_id, path, args, &src_hash); -#ifndef CLU_NO_CACHE - add_index(index_hash, src_hash); -#endif - return ret; -} - -/* - * Given a cl code and return a string represenation - */ -const char* cl_get_error_string(int err) { - switch (err) { - case 0: return "CL_SUCCESS"; - case -1: return "CL_DEVICE_NOT_FOUND"; - case -2: return "CL_DEVICE_NOT_AVAILABLE"; - case -3: return "CL_COMPILER_NOT_AVAILABLE"; - case -4: return "CL_MEM_OBJECT_ALLOCATION_FAILURE"; - case -5: return "CL_OUT_OF_RESOURCES"; - case -6: return "CL_OUT_OF_HOST_MEMORY"; - case -7: return "CL_PROFILING_INFO_NOT_AVAILABLE"; - case -8: return "CL_MEM_COPY_OVERLAP"; - case -9: return "CL_IMAGE_FORMAT_MISMATCH"; - case -10: return "CL_IMAGE_FORMAT_NOT_SUPPORTED"; - case -12: return "CL_MAP_FAILURE"; - case -13: return "CL_MISALIGNED_SUB_BUFFER_OFFSET"; - case -14: return "CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST"; - case -15: return "CL_COMPILE_PROGRAM_FAILURE"; - case -16: return "CL_LINKER_NOT_AVAILABLE"; - case -17: return "CL_LINK_PROGRAM_FAILURE"; - case -18: return "CL_DEVICE_PARTITION_FAILED"; - case -19: return "CL_KERNEL_ARG_INFO_NOT_AVAILABLE"; - case -30: return "CL_INVALID_VALUE"; - case -31: return "CL_INVALID_DEVICE_TYPE"; - case -32: return "CL_INVALID_PLATFORM"; - case -33: return "CL_INVALID_DEVICE"; - case -34: return "CL_INVALID_CONTEXT"; - case -35: return "CL_INVALID_QUEUE_PROPERTIES"; - case -36: return "CL_INVALID_COMMAND_QUEUE"; - case -37: return "CL_INVALID_HOST_PTR"; - case -38: return "CL_INVALID_MEM_OBJECT"; - case -39: return "CL_INVALID_IMAGE_FORMAT_DESCRIPTOR"; - case -40: return "CL_INVALID_IMAGE_SIZE"; - case -41: return "CL_INVALID_SAMPLER"; - case -42: return "CL_INVALID_BINARY"; - case -43: return "CL_INVALID_BUILD_OPTIONS"; - case -44: return "CL_INVALID_PROGRAM"; - case -45: return "CL_INVALID_PROGRAM_EXECUTABLE"; - case -46: return "CL_INVALID_KERNEL_NAME"; - case -47: return "CL_INVALID_KERNEL_DEFINITION"; - case -48: return "CL_INVALID_KERNEL"; - case -49: return "CL_INVALID_ARG_INDEX"; - case -50: return "CL_INVALID_ARG_VALUE"; - case -51: return "CL_INVALID_ARG_SIZE"; - case -52: return "CL_INVALID_KERNEL_ARGS"; - case -53: return "CL_INVALID_WORK_DIMENSION"; - case -54: return "CL_INVALID_WORK_GROUP_SIZE"; - case -55: return "CL_INVALID_WORK_ITEM_SIZE"; - case -56: return "CL_INVALID_GLOBAL_OFFSET"; - case -57: return "CL_INVALID_EVENT_WAIT_LIST"; - case -58: return "CL_INVALID_EVENT"; - case -59: return "CL_INVALID_OPERATION"; - case -60: return "CL_INVALID_GL_OBJECT"; - case -61: return "CL_INVALID_BUFFER_SIZE"; - case -62: return "CL_INVALID_MIP_LEVEL"; - case -63: return "CL_INVALID_GLOBAL_WORK_SIZE"; - case -64: return "CL_INVALID_PROPERTY"; - case -65: return "CL_INVALID_IMAGE_DESCRIPTOR"; - case -66: return "CL_INVALID_COMPILER_OPTIONS"; - case -67: return "CL_INVALID_LINKER_OPTIONS"; - case -68: return "CL_INVALID_DEVICE_PARTITION_COUNT"; - case -69: return "CL_INVALID_PIPE_SIZE"; - case -70: return "CL_INVALID_DEVICE_QUEUE"; - case -71: return "CL_INVALID_SPEC_ID"; - case -72: return "CL_MAX_SIZE_RESTRICTION_EXCEEDED"; - case -1002: return "CL_INVALID_D3D10_DEVICE_KHR"; - case -1003: return "CL_INVALID_D3D10_RESOURCE_KHR"; - case -1004: return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR"; - case -1005: return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR"; - case -1006: return "CL_INVALID_D3D11_DEVICE_KHR"; - case -1007: return "CL_INVALID_D3D11_RESOURCE_KHR"; - case -1008: return "CL_D3D11_RESOURCE_ALREADY_ACQUIRED_KHR"; - case -1009: return "CL_D3D11_RESOURCE_NOT_ACQUIRED_KHR"; - case -1010: return "CL_INVALID_DX9_MEDIA_ADAPTER_KHR"; - case -1011: return "CL_INVALID_DX9_MEDIA_SURFACE_KHR"; - case -1012: return "CL_DX9_MEDIA_SURFACE_ALREADY_ACQUIRED_KHR"; - case -1013: return "CL_DX9_MEDIA_SURFACE_NOT_ACQUIRED_KHR"; - case -1093: return "CL_INVALID_EGL_OBJECT_KHR"; - case -1092: return "CL_EGL_RESOURCE_NOT_ACQUIRED_KHR"; - case -1001: return "CL_PLATFORM_NOT_FOUND_KHR"; - case -1057: return "CL_DEVICE_PARTITION_FAILED_EXT"; - case -1058: return "CL_INVALID_PARTITION_COUNT_EXT"; - case -1059: return "CL_INVALID_PARTITION_NAME_EXT"; - case -1094: return "CL_INVALID_ACCELERATOR_INTEL"; - case -1095: return "CL_INVALID_ACCELERATOR_TYPE_INTEL"; - case -1096: return "CL_INVALID_ACCELERATOR_DESCRIPTOR_INTEL"; - case -1097: return "CL_ACCELERATOR_TYPE_NOT_SUPPORTED_INTEL"; - case -1000: return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR"; - case -1098: return "CL_INVALID_VA_API_MEDIA_ADAPTER_INTEL"; - case -1099: return "CL_INVALID_VA_API_MEDIA_SURFACE_INTEL"; - case -1100: return "CL_VA_API_MEDIA_SURFACE_ALREADY_ACQUIRED_INTEL"; - case -1101: return "CL_VA_API_MEDIA_SURFACE_NOT_ACQUIRED_INTEL"; - default: return "CL_UNKNOWN_ERROR"; - } -} diff --git a/selfdrive/common/clutil.cc b/selfdrive/common/clutil.cc new file mode 100644 index 00000000..45767fc5 --- /dev/null +++ b/selfdrive/common/clutil.cc @@ -0,0 +1,185 @@ +#include "clutil.h" +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "utilpp.h" + +namespace { // helper functions + +template +std::string get_info(Func get_info_func, Id id, Name param_name) { + size_t size = 0; + CL_CHECK(get_info_func(id, param_name, 0, NULL, &size)); + std::string info(size, '\0'); + CL_CHECK(get_info_func(id, param_name, size, info.data(), NULL)); + return info; +} +inline std::string get_platform_info(cl_platform_id id, cl_platform_info name) { return get_info(&clGetPlatformInfo, id, name); } +inline std::string get_device_info(cl_device_id id, cl_device_info name) { return get_info(&clGetDeviceInfo, id, name); } + +void cl_print_info(cl_platform_id platform, cl_device_id device) { + size_t work_group_size = 0; + cl_device_type device_type = 0; + clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(work_group_size), &work_group_size, NULL); + clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(device_type), &device_type, NULL); + const char *type_str = "Other..."; + switch (device_type) { + case CL_DEVICE_TYPE_CPU: type_str ="CL_DEVICE_TYPE_CPU"; break; + case CL_DEVICE_TYPE_GPU: type_str = "CL_DEVICE_TYPE_GPU"; break; + case CL_DEVICE_TYPE_ACCELERATOR: type_str = "CL_DEVICE_TYPE_ACCELERATOR"; break; + } + + std::cout << "vendor: " << get_platform_info(platform, CL_PLATFORM_VENDOR) << std::endl + << "platform version: " << get_platform_info(platform, CL_PLATFORM_VERSION) << std::endl + << "profile: " << get_platform_info(platform, CL_PLATFORM_PROFILE) << std::endl + << "extensions: " << get_platform_info(platform, CL_PLATFORM_EXTENSIONS) << std::endl + << "name :" << get_device_info(device, CL_DEVICE_NAME) << std::endl + << "device version :" << get_device_info(device, CL_DEVICE_VERSION) << std::endl + << "max work group size :" << work_group_size << std::endl + << "type = " << device_type << " = " << type_str << std::endl; +} + +void cl_print_build_errors(cl_program program, cl_device_id device) { + cl_build_status status; + clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_STATUS, sizeof(status), &status, NULL); + size_t log_size; + clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size); + std::string log(log_size, '\0'); + clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL); + + std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl; +} + +} // namespace + +cl_device_id cl_get_device_id(cl_device_type device_type) { + cl_uint num_platforms = 0; + CL_CHECK(clGetPlatformIDs(0, NULL, &num_platforms)); + std::unique_ptr platform_ids = std::make_unique(num_platforms); + CL_CHECK(clGetPlatformIDs(num_platforms, &platform_ids[0], NULL)); + + for (size_t i = 0; i < num_platforms; ++i) { + std::cout << "platform[" << i << "] CL_PLATFORM_NAME: " << get_platform_info(platform_ids[i], CL_PLATFORM_NAME) << std::endl; + // Get first device + if (cl_device_id device_id = NULL; clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL) == 0 && device_id) { + cl_print_info(platform_ids[i], device_id); + return device_id; + } + } + std::cout << "No valid openCL platform found" << std::endl; + assert(0); + return nullptr; +} + +cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) { + std::string src = util::read_file(path); + assert(src.length() > 0); + cl_program prg = CL_CHECK_ERR(clCreateProgramWithSource(ctx, 1, (const char*[]){src.c_str()}, NULL, &err)); + if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) { + cl_print_build_errors(prg, device_id); + assert(0); + } + return prg; +} + +// Given a cl code and return a string represenation +#define CL_ERR_TO_STR(err) case err: return #err +const char* cl_get_error_string(int err) { + switch (err) { + CL_ERR_TO_STR(CL_SUCCESS); + CL_ERR_TO_STR(CL_DEVICE_NOT_FOUND); + CL_ERR_TO_STR(CL_DEVICE_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_COMPILER_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_MEM_OBJECT_ALLOCATION_FAILURE); + CL_ERR_TO_STR(CL_OUT_OF_RESOURCES); + CL_ERR_TO_STR(CL_OUT_OF_HOST_MEMORY); + CL_ERR_TO_STR(CL_PROFILING_INFO_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_MEM_COPY_OVERLAP); + CL_ERR_TO_STR(CL_IMAGE_FORMAT_MISMATCH); + CL_ERR_TO_STR(CL_IMAGE_FORMAT_NOT_SUPPORTED); + CL_ERR_TO_STR(CL_MAP_FAILURE); + CL_ERR_TO_STR(CL_MISALIGNED_SUB_BUFFER_OFFSET); + CL_ERR_TO_STR(CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST); + CL_ERR_TO_STR(CL_COMPILE_PROGRAM_FAILURE); + CL_ERR_TO_STR(CL_LINKER_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_LINK_PROGRAM_FAILURE); + CL_ERR_TO_STR(CL_DEVICE_PARTITION_FAILED); + CL_ERR_TO_STR(CL_KERNEL_ARG_INFO_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_INVALID_VALUE); + CL_ERR_TO_STR(CL_INVALID_DEVICE_TYPE); + CL_ERR_TO_STR(CL_INVALID_PLATFORM); + CL_ERR_TO_STR(CL_INVALID_DEVICE); + CL_ERR_TO_STR(CL_INVALID_CONTEXT); + CL_ERR_TO_STR(CL_INVALID_QUEUE_PROPERTIES); + CL_ERR_TO_STR(CL_INVALID_COMMAND_QUEUE); + CL_ERR_TO_STR(CL_INVALID_HOST_PTR); + CL_ERR_TO_STR(CL_INVALID_MEM_OBJECT); + CL_ERR_TO_STR(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR); + CL_ERR_TO_STR(CL_INVALID_IMAGE_SIZE); + CL_ERR_TO_STR(CL_INVALID_SAMPLER); + CL_ERR_TO_STR(CL_INVALID_BINARY); + CL_ERR_TO_STR(CL_INVALID_BUILD_OPTIONS); + CL_ERR_TO_STR(CL_INVALID_PROGRAM); + CL_ERR_TO_STR(CL_INVALID_PROGRAM_EXECUTABLE); + CL_ERR_TO_STR(CL_INVALID_KERNEL_NAME); + CL_ERR_TO_STR(CL_INVALID_KERNEL_DEFINITION); + CL_ERR_TO_STR(CL_INVALID_KERNEL); + CL_ERR_TO_STR(CL_INVALID_ARG_INDEX); + CL_ERR_TO_STR(CL_INVALID_ARG_VALUE); + CL_ERR_TO_STR(CL_INVALID_ARG_SIZE); + CL_ERR_TO_STR(CL_INVALID_KERNEL_ARGS); + CL_ERR_TO_STR(CL_INVALID_WORK_DIMENSION); + CL_ERR_TO_STR(CL_INVALID_WORK_GROUP_SIZE); + CL_ERR_TO_STR(CL_INVALID_WORK_ITEM_SIZE); + CL_ERR_TO_STR(CL_INVALID_GLOBAL_OFFSET); + CL_ERR_TO_STR(CL_INVALID_EVENT_WAIT_LIST); + CL_ERR_TO_STR(CL_INVALID_EVENT); + CL_ERR_TO_STR(CL_INVALID_OPERATION); + CL_ERR_TO_STR(CL_INVALID_GL_OBJECT); + CL_ERR_TO_STR(CL_INVALID_BUFFER_SIZE); + CL_ERR_TO_STR(CL_INVALID_MIP_LEVEL); + CL_ERR_TO_STR(CL_INVALID_GLOBAL_WORK_SIZE); + CL_ERR_TO_STR(CL_INVALID_PROPERTY); + CL_ERR_TO_STR(CL_INVALID_IMAGE_DESCRIPTOR); + CL_ERR_TO_STR(CL_INVALID_COMPILER_OPTIONS); + CL_ERR_TO_STR(CL_INVALID_LINKER_OPTIONS); + CL_ERR_TO_STR(CL_INVALID_DEVICE_PARTITION_COUNT); + case -69: return "CL_INVALID_PIPE_SIZE"; + case -70: return "CL_INVALID_DEVICE_QUEUE"; + case -71: return "CL_INVALID_SPEC_ID"; + case -72: return "CL_MAX_SIZE_RESTRICTION_EXCEEDED"; + case -1002: return "CL_INVALID_D3D10_DEVICE_KHR"; + case -1003: return "CL_INVALID_D3D10_RESOURCE_KHR"; + case -1004: return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR"; + case -1005: return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR"; + case -1006: return "CL_INVALID_D3D11_DEVICE_KHR"; + case -1007: return "CL_INVALID_D3D11_RESOURCE_KHR"; + case -1008: return "CL_D3D11_RESOURCE_ALREADY_ACQUIRED_KHR"; + case -1009: return "CL_D3D11_RESOURCE_NOT_ACQUIRED_KHR"; + case -1010: return "CL_INVALID_DX9_MEDIA_ADAPTER_KHR"; + case -1011: return "CL_INVALID_DX9_MEDIA_SURFACE_KHR"; + case -1012: return "CL_DX9_MEDIA_SURFACE_ALREADY_ACQUIRED_KHR"; + case -1013: return "CL_DX9_MEDIA_SURFACE_NOT_ACQUIRED_KHR"; + case -1093: return "CL_INVALID_EGL_OBJECT_KHR"; + case -1092: return "CL_EGL_RESOURCE_NOT_ACQUIRED_KHR"; + case -1001: return "CL_PLATFORM_NOT_FOUND_KHR"; + case -1057: return "CL_DEVICE_PARTITION_FAILED_EXT"; + case -1058: return "CL_INVALID_PARTITION_COUNT_EXT"; + case -1059: return "CL_INVALID_PARTITION_NAME_EXT"; + case -1094: return "CL_INVALID_ACCELERATOR_INTEL"; + case -1095: return "CL_INVALID_ACCELERATOR_TYPE_INTEL"; + case -1096: return "CL_INVALID_ACCELERATOR_DESCRIPTOR_INTEL"; + case -1097: return "CL_ACCELERATOR_TYPE_NOT_SUPPORTED_INTEL"; + case -1000: return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR"; + case -1098: return "CL_INVALID_VA_API_MEDIA_ADAPTER_INTEL"; + case -1099: return "CL_INVALID_VA_API_MEDIA_SURFACE_INTEL"; + case -1100: return "CL_VA_API_MEDIA_SURFACE_ALREADY_ACQUIRED_INTEL"; + case -1101: return "CL_VA_API_MEDIA_SURFACE_NOT_ACQUIRED_INTEL"; + default: return "CL_UNKNOWN_ERROR"; + } +} diff --git a/selfdrive/common/clutil.h b/selfdrive/common/clutil.h index abbda8c8..793d6e7f 100644 --- a/selfdrive/common/clutil.h +++ b/selfdrive/common/clutil.h @@ -1,5 +1,4 @@ -#ifndef CLUTIL_H -#define CLUTIL_H +#pragma once #include #include @@ -15,74 +14,23 @@ extern "C" { #endif -void clu_init(void); +#define CL_CHECK(_expr) \ + do { \ + assert(CL_SUCCESS == _expr); \ + } while (0) + +#define CL_CHECK_ERR(_expr) \ + ({ \ + cl_int err = CL_INVALID_VALUE; \ + __typeof__(_expr) _ret = _expr; \ + assert(_ret&& err == CL_SUCCESS); \ + _ret; \ + }) cl_device_id cl_get_device_id(cl_device_type device_type); -cl_program cl_create_program_from_file(cl_context ctx, const char* path); -void cl_print_info(cl_platform_id platform, cl_device_id device); -void cl_print_build_errors(cl_program program, cl_device_id device); -void cl_print_build_errors(cl_program program, cl_device_id device); - -cl_program cl_cached_program_from_hash(cl_context ctx, cl_device_id device_id, uint64_t hash); -cl_program cl_cached_program_from_string(cl_context ctx, cl_device_id device_id, - const char* src, const char* args, - uint64_t *out_hash); -cl_program cl_cached_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args, - uint64_t *out_hash); - -cl_program cl_program_from_index(cl_context ctx, cl_device_id device_id, uint64_t index_hash); - -cl_program cl_index_program_from_string(cl_context ctx, cl_device_id device_id, - const char* src, const char* args, - uint64_t index_hash); -cl_program cl_index_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args, - uint64_t index_hash); - -uint64_t clu_index_hash(const char *s); -uint64_t clu_fnv_hash(const uint8_t *data, size_t len); - +cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args); const char* cl_get_error_string(int err); -static inline int cl_check_error(int err) { - if (err != 0) { - fprintf(stderr, "%s\n", cl_get_error_string(err)); - exit(1); - } - return err; -} - - -// // string hash macro. compiler, I'm so sorry. -#define CLU_H1(s,i,x) (x*65599ULL+(uint8_t)s[(i)>32))) - -#define CLU_STRINGIFY(x) #x -#define CLU_STRINGIFY2(x) CLU_STRINGIFY(x) -#define CLU_LINESTR CLU_STRINGIFY2(__LINE__) - -#ifdef CLU_NO_SRC - - #define CLU_LOAD_FROM_STRING(ctx, device_id, src, args) \ - cl_program_from_index(ctx, device_id, CLU_HASH("\1" __FILE__ "\1" CLU_LINESTR) ^ clu_fnv_hash((const uint8_t*)__func__, strlen(__func__)) ^ clu_fnv_hash((const uint8_t*)args, strlen(args))) - #define CLU_LOAD_FROM_FILE(ctx, device_id, path, args) \ - cl_program_from_index(ctx, device_id, CLU_HASH("\2" path) ^ clu_fnv_hash((const uint8_t*)args, strlen(args))) - -#else - - #define CLU_LOAD_FROM_STRING(ctx, device_id, src, args) \ - cl_index_program_from_string(ctx, device_id, src, args, clu_index_hash("\1" __FILE__ "\1" CLU_LINESTR) ^ clu_fnv_hash((const uint8_t*)__func__, strlen(__func__)) ^ clu_fnv_hash((const uint8_t*)args, strlen(args))) - #define CLU_LOAD_FROM_FILE(ctx, device_id, path, args) \ - cl_index_program_from_file(ctx, device_id, path, args, clu_index_hash("\2" path) ^ clu_fnv_hash((const uint8_t*)args, strlen(args))) - -#endif - #ifdef __cplusplus } #endif - -#endif diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index abd47a78..9946b40f 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,10 +1,10 @@ #pragma once -#define MODEL_PATH_DISTANCE 192 -#define TRAJECTORY_SIZE 33 -#define MIN_DRAW_DISTANCE 10.0 -#define MAX_DRAW_DISTANCE 100.0 -#define POLYFIT_DEGREE 4 -#define SPEED_PERCENTILES 10 -#define DESIRE_PRED_SIZE 32 -#define OTHER_META_SIZE 4 +constexpr int MODEL_PATH_DISTANCE = 192; +constexpr int TRAJECTORY_SIZE = 33; +constexpr float MIN_DRAW_DISTANCE = 10.0; +constexpr float MAX_DRAW_DISTANCE = 100.0; +constexpr int POLYFIT_DEGREE = 4; +constexpr int SPEED_PERCENTILES = 10; +constexpr int DESIRE_PRED_SIZE = 32; +constexpr int OTHER_META_SIZE = 4; diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index da799d71..ce079a99 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -25,7 +25,7 @@ std::string getenv_default(const char* env_var, const char * suffix, const char* const char* env_val = getenv(env_var); if (env_val != NULL){ return std::string(env_val) + std::string(suffix); - } else{ + } else { return std::string(default_val); } } diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 1bcd91ec..2958ce25 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.8.0-d56e04c0-2020-11-24T21:53:22" +#define COMMA_VERSION "0.8.1-80393615-2020-12-18T10:57:03" diff --git a/selfdrive/common/visionbuf_cl.c b/selfdrive/common/visionbuf_cl.c index 4c9ba59b..1cf6ad6b 100644 --- a/selfdrive/common/visionbuf_cl.c +++ b/selfdrive/common/visionbuf_cl.c @@ -7,6 +7,7 @@ #include #include #include +#include "common/clutil.h" #define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ @@ -46,8 +47,6 @@ VisionBuf visionbuf_allocate(size_t len) { } VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx) { - int err; - #if __OPENCL_VERSION__ >= 200 void* host_ptr = clSVMAlloc(ctx, CL_MEM_READ_WRITE | CL_MEM_SVM_FINE_GRAIN_BUFFER, len, 0); @@ -56,12 +55,10 @@ VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context c int fd; void* host_ptr = malloc_with_fd(len, &fd); - cl_command_queue q = clCreateCommandQueue(ctx, device_id, 0, &err); - assert(err == 0); + cl_command_queue q = CL_CHECK_ERR(clCreateCommandQueue(ctx, device_id, 0, &err)); #endif - cl_mem mem = clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, len, host_ptr, &err); - assert(err == 0); + cl_mem mem = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, len, host_ptr, &err)); return (VisionBuf){ .len = len, .addr = host_ptr, .handle = 0, .fd = fd, @@ -75,16 +72,14 @@ VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context c } void visionbuf_sync(const VisionBuf* buf, int dir) { - int err = 0; if (!buf->buf_cl) return; #if __OPENCL_VERSION__ < 200 if (dir == VISIONBUF_SYNC_FROM_DEVICE) { - err = clEnqueueReadBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL); + CL_CHECK(clEnqueueReadBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL)); } else { - err = clEnqueueWriteBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL); + CL_CHECK(clEnqueueWriteBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL)); } - assert(err == 0); clFinish(buf->copy_q); #endif } @@ -94,12 +89,11 @@ void visionbuf_free(const VisionBuf* buf) { munmap(buf->addr, buf->len); close(buf->fd); } else { - int err = clReleaseMemObject(buf->buf_cl); - assert(err == 0); + CL_CHECK(clReleaseMemObject(buf->buf_cl)); #if __OPENCL_VERSION__ >= 200 clSVMFree(buf->ctx, buf->addr); #else - clReleaseCommandQueue(buf->copy_q); + CL_CHECK(clReleaseCommandQueue(buf->copy_q)); munmap(buf->addr, buf->len); close(buf->fd); #endif diff --git a/selfdrive/common/visionbuf_ion.c b/selfdrive/common/visionbuf_ion.c index 62eaa337..5c26bea6 100644 --- a/selfdrive/common/visionbuf_ion.c +++ b/selfdrive/common/visionbuf_ion.c @@ -10,7 +10,7 @@ #include #include #include - +#include "common/clutil.h" #include #include "visionbuf.h" @@ -73,7 +73,6 @@ VisionBuf visionbuf_allocate(size_t len) { VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx) { VisionBuf buf = visionbuf_allocate(len); - int err = 0; assert(((uintptr_t)buf.addr % DEVICE_PAGE_SIZE_CL) == 0); @@ -83,11 +82,9 @@ VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context c ion_cl.ion_filedesc = buf.fd; ion_cl.ion_hostptr = buf.addr; - buf.buf_cl = clCreateBuffer(ctx, + buf.buf_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM, - buf.len, &ion_cl, &err); - assert(err == 0); - + buf.len, &ion_cl, &err)); return buf; } @@ -134,7 +131,9 @@ void visionbuf_sync(const VisionBuf* buf, int dir) { } void visionbuf_free(const VisionBuf* buf) { - clReleaseMemObject(buf->buf_cl); + if (buf->buf_cl) { + CL_CHECK(clReleaseMemObject(buf->buf_cl)); + } munmap(buf->addr, buf->mmap_len); close(buf->fd); struct ion_handle_data handle_data = { diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 22e63a79..184077c2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import os from cereal import car, log -from common.hardware import HARDWARE from common.numpy_fast import clip from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL from common.profiler import Profiler @@ -21,6 +20,7 @@ from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibrationd import Calibration +from selfdrive.hardware import HARDWARE LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index d0b10ec8..a8f1273e 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -36,6 +36,7 @@ class ET: # get event name from enum EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} + class Events: def __init__(self): self.events = [] @@ -252,6 +253,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), }, + EventName.startupOneplus: { + ET.PERMANENT: Alert( + "WARNING: Original EON deprecated", + "Device will no longer update", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + }, + EventName.invalidLkasSetting: { ET.PERMANENT: Alert( "Stock LKAS is turned on", @@ -508,7 +517,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo "TAKE CONTROL", "Steering Temporarily Unavailable", AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.), + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 1.), ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable", duration_hud_alert=0.), }, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f31dcb05..2ead3474 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -5,25 +5,23 @@ from selfdrive.controls.lib.pid import PIController LongCtrlState = log.ControlsState.LongControlState STOPPING_EGO_SPEED = 0.5 -MIN_CAN_SPEED = 0.3 # TODO: parametrize this in car interface -STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 +STOPPING_TARGET_SPEED_OFFSET = 0.01 STARTING_TARGET_SPEED = 0.5 BRAKE_THRESHOLD_TO_PID = 0.2 -STOPPING_BRAKE_RATE = 0.2 # brake_travel/s while trying to stop -STARTING_BRAKE_RATE = 0.8 # brake_travel/s while releasing on restart BRAKE_STOPPING_TARGET = 0.5 # apply at least this amount of brake to maintain the vehicle stationary RATE = 100.0 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, - output_gb, brake_pressed, cruise_standstill): + output_gb, brake_pressed, cruise_standstill, min_speed_can): """Update longitudinal control state machine""" + stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ (v_ego < STOPPING_EGO_SPEED and - ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or - brake_pressed)) + ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or + brake_pressed)) starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill @@ -78,9 +76,9 @@ class LongControl(): output_gb = self.last_output_gb self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo, v_target_future, self.v_pid, output_gb, - CS.brakePressed, CS.cruiseState.standstill) + CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan) - v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 if self.long_control_state == LongCtrlState.off or CS.gasPressed: self.reset(v_ego_pid) @@ -106,7 +104,7 @@ class LongControl(): elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET: - output_gb -= STOPPING_BRAKE_RATE / RATE + output_gb -= CP.stoppingBrakeRate / RATE output_gb = clip(output_gb, -brake_max, gas_max) self.reset(CS.vEgo) @@ -114,7 +112,7 @@ class LongControl(): # Intention is to move again, release brake fast before handing control to PID elif self.long_control_state == LongCtrlState.starting: if output_gb < -0.2: - output_gb += STARTING_BRAKE_RATE / RATE + output_gb += CP.startingBrakeRate / RATE self.reset(CS.vEgo) self.last_output_gb = output_gb diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 0412df36..2567b460 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -10,7 +10,7 @@ from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother -from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED +from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX @@ -21,7 +21,7 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] -_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] +_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits @@ -144,7 +144,7 @@ class Planner(): else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) - reset_speed = MIN_CAN_SPEED if starting else v_ego + reset_speed = self.CP.minSpeedCan if starting else v_ego reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index dc3d1f4b..238550dc 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -12,11 +12,12 @@ x_dot = A*x + B*u A depends on longitudinal speed, u [m/s], and vehicle parameters CP """ +from typing import Tuple + import numpy as np from numpy.linalg import solve -from typing import Tuple -from cereal import car +from cereal import car class VehicleModel: def __init__(self, CP: car.CarParams): diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 22d6aa06..3bc000d2 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -3,10 +3,10 @@ import os import sys import threading import capnp -from selfdrive.version import version, dirty +from selfdrive.version import version, dirty, origin, branch +from selfdrive.hardware import PC from selfdrive.swaglog import cloudlog -from common.hardware import PC if os.getenv("NOLOG") or os.getenv("NOCRASH") or PC: def capture_exception(*args, **kwargs): @@ -23,8 +23,14 @@ if os.getenv("NOLOG") or os.getenv("NOCRASH") or PC: else: from raven import Client from raven.transport.http import HTTPTransport + + tags = { + 'dirty': dirty, + 'origin': origin, + 'branch': branch + } client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924', - install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) + install_sys_hook=False, transport=HTTPTransport, release=version, tags=tags) def capture_exception(*args, **kwargs): exc_info = sys.exc_info() diff --git a/selfdrive/debug/fingerprint_from_route.py b/selfdrive/debug/fingerprint_from_route.py new file mode 100755 index 00000000..a06dfe18 --- /dev/null +++ b/selfdrive/debug/fingerprint_from_route.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 + +import sys +from tools.lib.route import Route +from tools.lib.logreader import MultiLogIterator + + +def get_fingerprint(lr): + # TODO: make this a nice tool for car ports. should also work with qlogs for FW + + fw = None + msgs = {} + for msg in lr: + if msg.which() == 'carParams': + fw = msg.carParams.carFw + elif msg.which() == 'can': + for c in msg.can: + # read also msgs sent by EON on CAN bus 0x80 and filter out the + # addr with more than 11 bits + if c.src % 0x80 == 0 and c.address < 0x800: + msgs[c.address] = len(c.dat) + + # show CAN fingerprint + fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) + print(f"\nfound {len(msgs)} messages. CAN fingerprint:\n") + print(fingerprint) + + # TODO: also print the fw fingerprint merged with the existing ones + # show FW fingerprint + print("\nFW fingerprint:\n") + for f in fw: + print(f" (Ecu.{f.ecu}, {hex(f.address)}, {None if f.subAddress == 0 else f.subAddress}): [") + print(f" {f.fwVersion},") + print(" ],") + print() + + +if __name__ == "__main__": + if len(sys.argv) < 2: + print("Usage: ./get_fingerprint_internal.py ") + sys.exit(1) + + route = Route(sys.argv[1]) + lr = MultiLogIterator(route.log_paths()[:5], wraparound=False) + get_fingerprint(lr) diff --git a/selfdrive/debug/toyota_eps_factor.py b/selfdrive/debug/toyota_eps_factor.py index b948bcc1..e63122b6 100755 --- a/selfdrive/debug/toyota_eps_factor.py +++ b/selfdrive/debug/toyota_eps_factor.py @@ -2,22 +2,24 @@ import sys import numpy as np import matplotlib.pyplot as plt -from sklearn import linear_model # pylint: disable=import-error +from sklearn import linear_model # pylint: disable=import-error +from selfdrive.car.toyota.values import STEER_THRESHOLD from tools.lib.route import Route from tools.lib.logreader import MultiLogIterator +MIN_SAMPLES = 30 * 100 -MIN_SAMPLES = 30*100 def to_signed(n, bits): if n >= (1 << max((bits - 1), 0)): n = n - (1 << max(bits, 0)) return n -def get_eps_factor(lr, plot=False): +def get_eps_factor(lr, plot=False): engaged = False + steering_pressed = False torque_cmd, eps_torque = None, None cmds, eps = [], [] @@ -31,8 +33,9 @@ def get_eps_factor(lr, plot=False): torque_cmd = to_signed((m.dat[1] << 8) | m.dat[2], 16) elif m.address == 0x260 and m.src == 0: eps_torque = to_signed((m.dat[5] << 8) | m.dat[6], 16) + steering_pressed = abs(to_signed((m.dat[1] << 8) | m.dat[2], 16)) > STEER_THRESHOLD - if engaged and torque_cmd is not None and eps_torque is not None: + if engaged and torque_cmd is not None and eps_torque is not None and not steering_pressed: cmds.append(torque_cmd) eps.append(eps_torque) else: @@ -45,14 +48,15 @@ def get_eps_factor(lr, plot=False): lm = linear_model.LinearRegression(fit_intercept=False) lm.fit(np.array(cmds).reshape(-1, 1), eps) - scale_factor = 1./lm.coef_[0] + scale_factor = 1. / lm.coef_[0] if plot: - plt.plot(np.array(eps)*scale_factor) + plt.plot(np.array(eps) * scale_factor) plt.plot(cmds) plt.show() return scale_factor + if __name__ == "__main__": r = Route(sys.argv[1]) lr = MultiLogIterator(r.log_paths(), wraparound=False) diff --git a/selfdrive/hardware/__init__.py b/selfdrive/hardware/__init__.py new file mode 100644 index 00000000..3babf1bb --- /dev/null +++ b/selfdrive/hardware/__init__.py @@ -0,0 +1,19 @@ +import os +from typing import cast + +from selfdrive.hardware.base import HardwareBase +from selfdrive.hardware.eon.hardware import Android +from selfdrive.hardware.tici.hardware import Tici +from selfdrive.hardware.pc.hardware import Pc + +EON = os.path.isfile('/EON') +TICI = os.path.isfile('/TICI') +PC = not (EON or TICI) + + +if EON: + HARDWARE = cast(HardwareBase, Android()) +elif TICI: + HARDWARE = cast(HardwareBase, Tici()) +else: + HARDWARE = cast(HardwareBase, Pc()) diff --git a/common/hardware_base.py b/selfdrive/hardware/base.py similarity index 50% rename from common/hardware_base.py rename to selfdrive/hardware/base.py index 24bb52a5..24163a12 100644 --- a/common/hardware_base.py +++ b/selfdrive/hardware/base.py @@ -8,6 +8,22 @@ class HardwareBase: cmdline = f.read() return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2} + @staticmethod + def read_param_file(path, parser, default=0): + try: + with open(path) as f: + return parser(f.read()) + except Exception: + return default + + @abstractmethod + def reboot(self, reason=None): + pass + + @abstractmethod + def uninstall(self): + pass + @abstractmethod def get_sound_card_online(self): pass @@ -24,10 +40,6 @@ class HardwareBase: def get_subscriber_info(self): pass - @abstractmethod - def reboot(self, reason=None): - pass - @abstractmethod def get_network_type(self): pass @@ -39,3 +51,35 @@ class HardwareBase: @abstractmethod def get_network_strength(self, network_type): pass + + @abstractmethod + def get_battery_capacity(self): + pass + + @abstractmethod + def get_battery_status(self): + pass + + @abstractmethod + def get_battery_current(self): + pass + + @abstractmethod + def get_battery_voltage(self): + pass + + @abstractmethod + def get_battery_charging(self): + pass + + @abstractmethod + def set_battery_charging(self, on): + pass + + @abstractmethod + def get_usb_present(self): + pass + + @abstractmethod + def get_current_power_draw(self): + pass diff --git a/selfdrive/hardware/eon/__init__.py b/selfdrive/hardware/eon/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/common/apk.py b/selfdrive/hardware/eon/apk.py similarity index 100% rename from common/apk.py rename to selfdrive/hardware/eon/apk.py diff --git a/common/hardware_android.py b/selfdrive/hardware/eon/hardware.py similarity index 86% rename from common/hardware_android.py rename to selfdrive/hardware/eon/hardware.py index 91e1d142..231b5ba6 100644 --- a/common/hardware_android.py +++ b/selfdrive/hardware/eon/hardware.py @@ -6,7 +6,7 @@ import struct import subprocess from cereal import log -from common.hardware_base import HardwareBase +from selfdrive.hardware.base import HardwareBase NetworkType = log.ThermalData.NetworkType NetworkStrength = log.ThermalData.NetworkStrength @@ -98,6 +98,12 @@ class Android(HardwareBase): "i32", "1" # wait ]) + def uninstall(self): + with open('/cache/recovery/command', 'w') as f: + f.write('--wipe_data\n') + # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) + self.reboot(reason="recovery") + def get_sim_info(self): # Used for athena # TODO: build using methods from this class @@ -300,3 +306,32 @@ class Android(HardwareBase): network_strength = max(network_strength, ns) return network_strength + + def get_battery_capacity(self): + return self.read_param_file("/sys/class/power_supply/battery/capacity", int, 100) + + def get_battery_status(self): + # This does not correspond with actual charging or not. + # If a USB cable is plugged in, it responds with 'Charging', even when charging is disabled + return self.read_param_file("/sys/class/power_supply/battery/status", lambda x: x.strip(), '') + + def get_battery_current(self): + return self.read_param_file("/sys/class/power_supply/battery/current_now", int) + + def get_battery_voltage(self): + return self.read_param_file("/sys/class/power_supply/battery/voltage_now", int) + + def get_battery_charging(self): + # This does correspond with actually charging + return self.read_param_file("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", True) + + def set_battery_charging(self, on): + with open('/sys/class/power_supply/battery/charging_enabled', 'w') as f: + f.write(f"{1 if on else 0}\n") + + def get_usb_present(self): + return self.read_param_file("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False) + + def get_current_power_draw(self): + # We don't have a good direct way to measure this on android + return None diff --git a/selfdrive/hardware/pc/__init__.py b/selfdrive/hardware/pc/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/common/hardware.py b/selfdrive/hardware/pc/hardware.py similarity index 61% rename from common/hardware.py rename to selfdrive/hardware/pc/hardware.py index 67d0c44d..181de110 100644 --- a/common/hardware.py +++ b/selfdrive/hardware/pc/hardware.py @@ -1,17 +1,7 @@ -import os import random -from typing import cast from cereal import log -from common.hardware_android import Android -from common.hardware_tici import Tici -from common.hardware_base import HardwareBase - -EON = os.path.isfile('/EON') -TICI = os.path.isfile('/TICI') -PC = not (EON or TICI) -ANDROID = EON - +from selfdrive.hardware.base import HardwareBase NetworkType = log.ThermalData.NetworkType NetworkStrength = log.ThermalData.NetworkStrength @@ -21,6 +11,12 @@ class Pc(HardwareBase): def get_sound_card_online(self): return True + def reboot(self, reason=None): + print("REBOOT!") + + def uninstall(self): + print("uninstall") + def get_imei(self, slot): return "%015d" % random.randint(0, 1 << 32) @@ -30,9 +26,6 @@ class Pc(HardwareBase): def get_subscriber_info(self): return "" - def reboot(self, reason=None): - print("REBOOT!") - def get_network_type(self): return NetworkType.wifi @@ -48,10 +41,26 @@ class Pc(HardwareBase): def get_network_strength(self, network_type): return NetworkStrength.unknown + def get_battery_capacity(self): + return 100 -if EON: - HARDWARE = cast(HardwareBase, Android()) -elif TICI: - HARDWARE = cast(HardwareBase, Tici()) -else: - HARDWARE = cast(HardwareBase, Pc()) + def get_battery_status(self): + return "" + + def get_battery_current(self): + return 0 + + def get_battery_voltage(self): + return 0 + + def get_battery_charging(self): + return True + + def set_battery_charging(self, on): + pass + + def get_usb_present(self): + return False + + def get_current_power_draw(self): + return 0 diff --git a/selfdrive/hardware/tici/__init__.py b/selfdrive/hardware/tici/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py new file mode 100644 index 00000000..4c2e7178 --- /dev/null +++ b/selfdrive/hardware/tici/hardware.py @@ -0,0 +1,160 @@ +import subprocess + +from cereal import log +from selfdrive.hardware.base import HardwareBase + +NM = 'org.freedesktop.NetworkManager' +NM_CON_ACT = NM + '.Connection.Active' +NM_DEV_WL = NM + '.Device.Wireless' +NM_AP = NM + '.AccessPoint' +DBUS_PROPS = 'org.freedesktop.DBus.Properties' + +MM = 'org.freedesktop.ModemManager1' +MM_MODEM = MM + ".Modem" +MM_MODEM_SIMPLE = MM + ".Modem.Simple" +MM_SIM = MM + ".Sim" + +MM_MODEM_STATE_CONNECTED = 11 + +NetworkType = log.ThermalData.NetworkType +NetworkStrength = log.ThermalData.NetworkStrength + +# https://developer.gnome.org/ModemManager/unstable/ModemManager-Flags-and-Enumerations.html#MMModemAccessTechnology +MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5 +MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14 + + +class Tici(HardwareBase): + def __init__(self): + import dbus # pylint: disable=import-error + + self.bus = dbus.SystemBus() + self.nm = self.bus.get_object(NM, '/org/freedesktop/NetworkManager') + self.mm = self.bus.get_object(MM, '/org/freedesktop/ModemManager1') + + def get_sound_card_online(self): + return True + + def reboot(self, reason=None): + subprocess.check_output(["sudo", "reboot"]) + + def uninstall(self): + # TODO: implement uninstall. reboot to factory reset? + pass + + def get_serial(self): + return self.get_cmdline()['androidboot.serialno'] + + def get_network_type(self): + primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS) + primary_connection = self.bus.get_object(NM, primary_connection) + tp = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS) + + if tp in ['802-3-ethernet', '802-11-wireless']: + return NetworkType.wifi + elif tp in ['gsm']: + modem = self.get_modem() + access_t = modem.Get(MM_MODEM, 'AccessTechnologies', dbus_interface=DBUS_PROPS) + if access_t >= MM_MODEM_ACCESS_TECHNOLOGY_LTE: + return NetworkType.cell4G + elif access_t >= MM_MODEM_ACCESS_TECHNOLOGY_UMTS: + return NetworkType.cell3G + else: + return NetworkType.cell2G + + return NetworkType.none + + def get_modem(self): + objects = self.mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager") + modem_path = list(objects.keys())[0] + return self.bus.get_object(MM, modem_path) + + def get_wlan(self): + wlan_path = self.nm.GetDeviceByIpIface('wlan0', dbus_interface=NM) + return self.bus.get_object(NM, wlan_path) + + def get_sim_info(self): + modem = self.get_modem() + sim_path = modem.Get(MM_MODEM, 'Sim', dbus_interface=DBUS_PROPS) + + if sim_path == "/": + return { + 'sim_id': '', + 'mcc_mnc': None, + 'network_type': ["Unknown"], + 'sim_state': ["ABSENT"], + 'data_connected': False + } + else: + sim = self.bus.get_object(MM, sim_path) + return { + 'sim_id': str(sim.Get(MM_SIM, 'SimIdentifier', dbus_interface=DBUS_PROPS)), + 'mcc_mnc': str(sim.Get(MM_SIM, 'OperatorIdentifier', dbus_interface=DBUS_PROPS)), + 'network_type': ["Unknown"], + 'sim_state': ["READY"], + 'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS) == MM_MODEM_STATE_CONNECTED, + } + + def get_subscriber_info(self): + return "" + + def get_imei(self, slot): + if slot != 0: + return "" + + return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS)) + + def parse_strength(self, percentage): + if percentage < 25: + return NetworkStrength.poor + elif percentage < 50: + return NetworkStrength.moderate + elif percentage < 75: + return NetworkStrength.good + else: + return NetworkStrength.great + + def get_network_strength(self, network_type): + network_strength = NetworkStrength.unknown + + if network_type == NetworkType.none: + pass + elif network_type == NetworkType.wifi: + wlan = self.get_wlan() + active_ap_path = wlan.Get(NM_DEV_WL, 'ActiveAccessPoint', dbus_interface=DBUS_PROPS) + if active_ap_path != "/": + active_ap = self.bus.get_object(NM, active_ap_path) + strength = int(active_ap.Get(NM_AP, 'Strength', dbus_interface=DBUS_PROPS)) + network_strength = self.parse_strength(strength) + else: # Cellular + modem = self.get_modem() + strength = int(modem.Get(MM_MODEM, 'SignalQuality', dbus_interface=DBUS_PROPS)[0]) + network_strength = self.parse_strength(strength) + + return network_strength + + # We don't have a battery, so let's use some sane constants + def get_battery_capacity(self): + return 100 + + def get_battery_status(self): + return "" + + def get_battery_current(self): + return 0 + + def get_battery_voltage(self): + return 0 + + def get_battery_charging(self): + return True + + def set_battery_charging(self, on): + pass + + def get_usb_present(self): + # Not sure if relevant on tici, but the file exists + return self.read_param_file("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False) + + def get_current_power_draw(self): + return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6) diff --git a/selfdrive/hardware/tici/pins.py b/selfdrive/hardware/tici/pins.py new file mode 100644 index 00000000..7139f5e9 --- /dev/null +++ b/selfdrive/hardware/tici/pins.py @@ -0,0 +1,8 @@ +# TODO: these are also defined in a header +# GPIO pin definitions +GPIO_HUB_RST_N = 30 +GPIO_UBLOX_RST_N = 32 +GPIO_UBLOX_SAFEBOOT_N = 33 +GPIO_UBLOX_PWR_EN = 34 +GPIO_STM_RST_N = 124 +GPIO_STM_BOOT0 = 134 diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 9cda80cf..f9b857fb 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -7,16 +7,18 @@ and the image input into the neural network is not corrected for roll. ''' import os +import capnp import copy import json import numpy as np import cereal.messaging as messaging -from selfdrive.config import Conversions as CV -from selfdrive.swaglog import cloudlog +from cereal import car, log from common.params import Params, put_nonblocking from common.transformations.model import model_height from common.transformations.camera import get_view_frame_from_road_frame from common.transformations.orientation import rot_from_euler, euler_from_rot +from selfdrive.config import Conversions as CV +from selfdrive.swaglog import cloudlog MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS MAX_VEL_ANGLE_STD = np.radians(0.25) @@ -59,18 +61,32 @@ class Calibrator(): self.param_put = param_put # Read saved calibration - calibration_params = Params().get("CalibrationParams") + params = Params() + calibration_params = params.get("CalibrationParams") rpy_init = RPY_INIT valid_blocks = 0 + cached_params = params.get("CarParamsCache") + if cached_params is not None: + CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) + cached_params = car.CarParams.from_bytes(cached_params) + if cached_params.carFingerprint != CP.carFingerprint: + calibration_params = None + if param_put and calibration_params: try: + msg = log.Event.from_bytes(calibration_params) + rpy_init = list(msg.liveCalibration.rpyCalib) + valid_blocks = msg.liveCalibration.validBlocks + except (ValueError, capnp.lib.capnp.KjException): + # TODO: remove this when offroad can read capnp calibration_params = json.loads(calibration_params) rpy_init = calibration_params["calib_radians"] valid_blocks = calibration_params['valid_blocks'] except Exception: cloudlog.exception("CalibrationParams file found but error encountered") + self.reset(rpy_init, valid_blocks) self.update_status() @@ -118,7 +134,7 @@ class Calibrator(): write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: - # TODO: this should use the liveCalibration struct from cereal + # TODO: change to raw bytes when offroad can read capnp cal_params = {"calib_radians": list(self.rpy), "valid_blocks": int(self.valid_blocks)} put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) @@ -138,42 +154,44 @@ class Calibrator(): straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or (self.valid_blocks < INPUTS_NEEDED)) - if straight_and_fast and certain_if_calib: - observed_rpy = np.array([0, - -np.arctan2(trans[2], trans[0]), - np.arctan2(trans[1], trans[0])]) - 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) - self.idx = (self.idx + 1) % BLOCK_SIZE - if self.idx == 0: - self.block_idx += 1 - self.valid_blocks = max(self.block_idx, self.valid_blocks) - self.block_idx = self.block_idx % INPUTS_WANTED - if self.valid_blocks > 0: - self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0) - - - self.update_status() - - return new_rpy - else: + if not (straight_and_fast and certain_if_calib): return None - def send_data(self, pm): + observed_rpy = np.array([0, + -np.arctan2(trans[2], trans[0]), + np.arctan2(trans[1], trans[0])]) + 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) + self.idx = (self.idx + 1) % BLOCK_SIZE + if self.idx == 0: + self.block_idx += 1 + self.valid_blocks = max(self.block_idx, self.valid_blocks) + self.block_idx = self.block_idx % INPUTS_WANTED + if self.valid_blocks > 0: + self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0) + + self.update_status() + + return new_rpy + + def get_msg(self): smooth_rpy = self.get_smooth_rpy() extrinsic_matrix = get_view_frame_from_road_frame(0, smooth_rpy[1], smooth_rpy[2], model_height) - cal_send = messaging.new_message('liveCalibration') - cal_send.liveCalibration.validBlocks = self.valid_blocks - cal_send.liveCalibration.calStatus = self.cal_status - cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) - cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] - cal_send.liveCalibration.rpyCalib = [float(x) for x in smooth_rpy] - cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in self.calib_spread] + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = self.valid_blocks + msg.liveCalibration.calStatus = self.cal_status + msg.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) + msg.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] + msg.liveCalibration.rpyCalib = [float(x) for x in smooth_rpy] + msg.liveCalibration.rpyCalibSpread = [float(x) for x in self.calib_spread] + return msg - pm.send('liveCalibration', cal_send) + def send_data(self, pm): + pm.send('liveCalibration', self.get_msg()) def calibrationd_thread(sm=None, pm=None): diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index ce4e4dca..74d56ed3 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -213,8 +213,7 @@ kj::Array UbloxMsgParser::gen_solution() { std::time_t utc_tt = timegm(&timeinfo); gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06); float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f }; - kj::ArrayPtr ap(&f[0], sizeof(f) / sizeof(f[0])); - gpsLoc.setVNED(ap); + gpsLoc.setVNED(f); gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05); @@ -318,10 +317,8 @@ kj::Array UbloxMsgParser::gen_nav_data() { eph.setTgd(ephem_data.Tgd); eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid); if(ephem_data.ionoCoeffsValid) { - kj::ArrayPtr apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0])); - eph.setIonoAlpha(apa); - kj::ArrayPtr apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0])); - eph.setIonoBeta(apb); + eph.setIonoAlpha(ephem_data.ionoAlpha); + eph.setIonoBeta(ephem_data.ionoBeta); } else { eph.setIonoAlpha(kj::ArrayPtr()); eph.setIonoBeta(kj::ArrayPtr()); diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 5d59e929..f0577ec1 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -5,12 +5,13 @@ libs = ['zmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', 'yuv', 'bz2', common, cereal, messaging, visionipc] -if arch == "aarch64": - src += ['encoder.c', 'raw_logger.cc'] - libs += ['OmxVenc', 'OmxCore', 'cutils'] -elif arch == "larch64": - src += ['encoder.c', 'raw_logger.cc'] - libs += ['OmxVenc', 'OmxCore', 'pthread'] +if arch in ["aarch64", "larch64"]: + src += ['encoder.c'] + libs += ['OmxVenc', 'OmxCore'] + if arch == "aarch64": + libs += ['cutils'] + else: + libs += ['pthread'] else: libs += ['pthread'] diff --git a/selfdrive/loggerd/encoder.c b/selfdrive/loggerd/encoder.c index 714293c7..1ebb1dd3 100644 --- a/selfdrive/loggerd/encoder.c +++ b/selfdrive/loggerd/encoder.c @@ -288,22 +288,26 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, assert(err == OMX_ErrorNone); if (h265) { - #ifndef QCOM2 // setup HEVC + #ifndef QCOM2 OMX_VIDEO_PARAM_HEVCTYPE hecv_type = {0}; + OMX_INDEXTYPE index_type = (OMX_INDEXTYPE) OMX_IndexParamVideoHevc; + #else + OMX_VIDEO_PARAM_PROFILELEVELTYPE hecv_type = {0}; + OMX_INDEXTYPE index_type = OMX_IndexParamVideoProfileLevelCurrent; + #endif hecv_type.nSize = sizeof(hecv_type); hecv_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, + err = OMX_GetParameter(s->handle, index_type, (OMX_PTR) &hecv_type); assert(err == OMX_ErrorNone); hecv_type.eProfile = OMX_VIDEO_HEVCProfileMain; hecv_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; - err = OMX_SetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, + err = OMX_SetParameter(s->handle, index_type, (OMX_PTR) &hecv_type); assert(err == OMX_ErrorNone); - #endif } else { // setup h264 OMX_VIDEO_PARAM_AVCTYPE avc = { 0 }; @@ -395,6 +399,9 @@ static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { } s->codec_config_len = out_buf->nFilledLen; memcpy(s->codec_config, buf_data, out_buf->nFilledLen); +#ifdef QCOM2 + out_buf->nTimeStamp = 0; +#endif } if (s->of) { @@ -442,6 +449,11 @@ static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { } // give omx back the buffer +#ifdef QCOM2 + if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) { + out_buf->nTimeStamp = 0; + } +#endif err = OMX_FillThisBuffer(s->handle, out_buf); assert(err == OMX_ErrorNone); } @@ -513,6 +525,7 @@ int encoder_encode_frame(EncoderState *s, in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME; in_buf->nOffset = 0; in_buf->nTimeStamp = extra->timestamp_eof/1000LL; // OMX_TICKS, in microseconds + s->last_t = in_buf->nTimeStamp; err = OMX_EmptyThisBuffer(s->handle, in_buf); assert(err == OMX_ErrorNone); @@ -555,9 +568,6 @@ void encoder_open(EncoderState *s, const char* path) { avformat_alloc_output_context2(&s->ofmt_ctx, NULL, NULL, s->vid_path); assert(s->ofmt_ctx); -#ifdef QCOM2 - s->ofmt_ctx->oformat->flags = AVFMT_TS_NONSTRICT; -#endif s->out_stream = avformat_new_stream(s->ofmt_ctx, NULL); assert(s->out_stream); @@ -583,7 +593,9 @@ void encoder_open(EncoderState *s, const char* path) { s->of = fopen(s->vid_path, "wb"); assert(s->of); if (s->codec_config_len > 0) { +#ifndef QCOM2 fwrite(s->codec_config, s->codec_config_len, 1, s->of); +#endif } } @@ -612,7 +624,7 @@ void encoder_close(EncoderState *s) { in_buf->nFilledLen = 0; in_buf->nOffset = 0; in_buf->nFlags = OMX_BUFFERFLAG_EOS; - in_buf->nTimeStamp = 0; + in_buf->nTimeStamp = s->last_t + 1000000LL/s->fps; err = OMX_EmptyThisBuffer(s->handle, in_buf); assert(err == OMX_ErrorNone); diff --git a/selfdrive/loggerd/encoder.h b/selfdrive/loggerd/encoder.h index abd9d18c..8036162b 100644 --- a/selfdrive/loggerd/encoder.h +++ b/selfdrive/loggerd/encoder.h @@ -53,6 +53,8 @@ typedef struct EncoderState { int num_out_bufs; OMX_BUFFERHEADERTYPE** out_buf_headers; + uint64_t last_t; + Queue free_in; Queue done_out; diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 6360a555..8dd328df 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -46,7 +47,6 @@ #ifndef DISABLE_ENCODER #include "encoder.h" -#include "raw_logger.h" #endif #define MAIN_BITRATE 5000000 @@ -110,8 +110,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { #define LOG_ROOT "/data/media/0/realdata" -#define RAW_CLIP_LENGTH 100 // 5 seconds at 20fps -#define RAW_CLIP_FREQUENCY (randrange(61, 8*60)) // once every ~4 minutes namespace { @@ -129,6 +127,11 @@ static void set_do_exit(int sig) { do_exit = 1; } +static bool file_exists (const std::string& fn) { + std::ifstream f(fn); + return f.good(); +} + class RotateState { public: SubSocket* fpkt_sock; @@ -196,7 +199,7 @@ struct LoggerdState { LoggerdState s; #ifndef DISABLE_ENCODER -void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { +void encoder_thread(RotateState *rotate_state, int cam_idx) { switch (cam_idx) { case LOG_CAMERA_ID_DCAMERA: { @@ -266,15 +269,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { encoder_inited = true; } - // dont log a raw clip in the first minute - double rawlogger_start_time = seconds_since_boot()+RAW_CLIP_FREQUENCY; - int rawlogger_clip_cnt = 0; - RawLogger *rawlogger = NULL; - - if (raw_clips) { - rawlogger = new RawLogger("prcamera", buf_info.width, buf_info.height, MAIN_FPS); - } - while (!do_exit) { VIPCBufExtra extra; VIPCBuf* buf = visionstream_get(&stream, &extra); @@ -311,9 +305,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { if (has_encoder_alt) { encoder_rotate(&encoder_alt, s.segment_path, s.rotate_segment); } - if (raw_clips) { - rawlogger->Rotate(s.segment_path, s.rotate_segment); - } encoder_segment = s.rotate_segment; if (lh) { lh_close(lh); @@ -393,44 +384,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { } } - if (raw_clips) { - double ts = seconds_since_boot(); - if (ts > rawlogger_start_time) { - // encode raw if in clip - int out_segment = -1; - int out_id = rawlogger->LogFrame(cnt, y, u, v, &out_segment); - - if (rawlogger_clip_cnt == 0) { - LOG("starting raw clip in seg %d", out_segment); - } - - // publish encode index - MessageBuilder msg; - auto eidx = msg.initEvent().initEncodeIdx(); - eidx.setFrameId(extra.frame_id); - eidx.setType(cereal::EncodeIndex::Type::FULL_LOSSLESS_CLIP); - eidx.setEncodeId(cnt); - eidx.setSegmentNum(out_segment); - eidx.setSegmentId(out_id); - - auto bytes = msg.toBytes(); - if (lh) { - lh_log(lh, bytes.begin(), bytes.size(), false); - } - - // close rawlogger if clip ended - rawlogger_clip_cnt++; - if (rawlogger_clip_cnt >= RAW_CLIP_LENGTH) { - rawlogger->Close(); - - rawlogger_clip_cnt = 0; - rawlogger_start_time = ts+RAW_CLIP_FREQUENCY; - - LOG("ending raw clip in seg %d, next in %.1f sec", out_segment, rawlogger_start_time-ts); - } - } - } - cnt++; } @@ -439,11 +392,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { lh = NULL; } - if (raw_clips) { - rawlogger->Close(); - delete rawlogger; - } - visionstream_destroy(&stream); } @@ -474,7 +422,14 @@ kj::Array gen_init_data() { MessageBuilder msg; auto init = msg.initEvent().initInitData(); - init.setDeviceType(cereal::InitData::DeviceType::NEO); + if (file_exists("/EON")) + init.setDeviceType(cereal::InitData::DeviceType::NEO); + else if (file_exists("/TICI")) { + init.setDeviceType(cereal::InitData::DeviceType::TICI); + } else { + init.setDeviceType(cereal::InitData::DeviceType::PC); + } + init.setVersion(capnp::Text::Reader(COMMA_VERSION)); std::ifstream cmdline_stream("/proc/cmdline"); @@ -662,17 +617,17 @@ int main(int argc, char** argv) { pthread_mutex_init(&s.rotate_lock, NULL); #ifndef DISABLE_ENCODER // rear camera - std::thread encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_FCAMERA], false, LOG_CAMERA_ID_FCAMERA); + std::thread encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_FCAMERA], LOG_CAMERA_ID_FCAMERA); s.rotate_state[LOG_CAMERA_ID_FCAMERA].enabled = true; // front camera std::thread front_encoder_thread_handle; if (record_front) { - front_encoder_thread_handle = std::thread(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_DCAMERA], false, LOG_CAMERA_ID_DCAMERA); + front_encoder_thread_handle = std::thread(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_DCAMERA], LOG_CAMERA_ID_DCAMERA); s.rotate_state[LOG_CAMERA_ID_DCAMERA].enabled = true; } #ifdef QCOM2 // wide camera - std::thread wide_encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_ECAMERA], false, LOG_CAMERA_ID_ECAMERA); + std::thread wide_encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_ECAMERA], LOG_CAMERA_ID_ECAMERA); s.rotate_state[LOG_CAMERA_ID_ECAMERA].enabled = true; #endif #endif @@ -798,6 +753,7 @@ int main(int argc, char** argv) { #endif logger_close(&s.logger); + LOGW("logger closed"); for (auto s : socks){ delete s; diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 456712f2..d2cbb94e 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -10,9 +10,9 @@ import time import traceback from cereal import log -from common.hardware import HARDWARE from common.api import Api from common.params import Params +from selfdrive.hardware import HARDWARE from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.loggerd.config import ROOT from selfdrive.swaglog import cloudlog diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 1775c638..e0fd9429 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -1,37 +1,33 @@ #!/usr/bin/env python3 +import datetime +import importlib import os -import time import sys import fcntl import errno import signal import shutil import subprocess -import datetime import textwrap -from typing import Dict, List -from selfdrive.swaglog import cloudlog, add_logentries_handler +import time +import traceback +from multiprocessing import Process +from typing import Dict, List from common.basedir import BASEDIR -from common.hardware import HARDWARE, ANDROID, PC -WEBCAM = os.getenv("WEBCAM") is not None -sys.path.append(os.path.join(BASEDIR, "pyextra")) +from common.spinner import Spinner +from common.text_window import TextWindow +from selfdrive.hardware import HARDWARE, EON, PC +from selfdrive.swaglog import cloudlog, add_logentries_handler + os.environ['BASEDIR'] = BASEDIR +sys.path.append(os.path.join(BASEDIR, "pyextra")) TOTAL_SCONS_NODES = 1040 -prebuilt = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) +WEBCAM = os.getenv("WEBCAM") is not None +PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) -# Create folders needed for msgq -try: - os.mkdir("/dev/shm") -except FileExistsError: - pass -except PermissionError: - print("WARNING: failed to make /dev/shm") - -if ANDROID: - os.chmod("/dev/shm", 0o777) def unblock_stdout(): # get a non-blocking stdout @@ -42,8 +38,7 @@ def unblock_stdout(): signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT)) signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM)) - fcntl.fcntl(sys.stdout, fcntl.F_SETFL, - fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK) + fcntl.fcntl(sys.stdout, fcntl.F_SETFL, fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK) while True: try: @@ -70,12 +65,6 @@ def unblock_stdout(): if __name__ == "__main__": unblock_stdout() -from common.spinner import Spinner -from common.text_window import TextWindow - -import importlib -import traceback -from multiprocessing import Process # Run scons spinner = Spinner() @@ -83,7 +72,7 @@ spinner.update("0") if __name__ != "__main__": spinner.close() -if not prebuilt: +def build(): for retry in [True, False]: # run scons env = os.environ.copy() @@ -91,7 +80,7 @@ if not prebuilt: env['SCONS_CACHE'] = "1" nproc = os.cpu_count() - j_flag = "" if nproc is None else "-j%d" % (nproc - 1) + j_flag = "" if nproc is None else f"-j{nproc - 1}" scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) compile_output = [] @@ -107,8 +96,7 @@ if not prebuilt: prefix = b'progress: ' if line.startswith(prefix): i = int(line[len(prefix):]) - if spinner is not None: - spinner.update("%d" % (70.0 * (i / TOTAL_SCONS_NODES))) + spinner.update("%d" % (70.0 * (i / TOTAL_SCONS_NODES))) elif len(line): compile_output.append(line) print(line.decode('utf8', 'replace')) @@ -141,15 +129,17 @@ if not prebuilt: cloudlog.error("scons build failed\n" + error_s) # Show TextWindow + spinner.close() error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors]) with TextWindow("openpilot failed to build\n \n" + error_s) as t: t.wait_for_exit() - exit(1) else: break -import cereal +if __name__ == "__main__" and not PREBUILT: + build() + import cereal.messaging as messaging from common.params import Params @@ -158,9 +148,8 @@ from selfdrive.registration import register from selfdrive.version import version, dirty from selfdrive.loggerd.config import ROOT from selfdrive.launcher import launcher -from common.apk import update_apks, pm_apply_packages, start_offroad +from selfdrive.hardware.eon.apk import update_apks, pm_apply_packages, start_offroad -ThermalStatus = cereal.log.ThermalData.ThermalStatus # comment out anything you don't want to run managed_processes = { @@ -234,6 +223,7 @@ car_started_processes = [ 'calibrationd', 'paramsd', 'camerad', + 'modeld', 'proclogd', 'locationd', 'clocksd', @@ -245,32 +235,22 @@ driver_view_processes = [ 'dmonitoringmodeld' ] -if WEBCAM: - car_started_processes += [ - 'dmonitoringd', - 'dmonitoringmodeld', - ] - -if not PC: +if not PC or WEBCAM: car_started_processes += [ 'ubloxd', 'dmonitoringd', 'dmonitoringmodeld', ] -if ANDROID: +if EON: car_started_processes += [ 'gpsd', 'rtshield', ] -# starting dmonitoringmodeld when modeld is initializing can sometimes \ -# result in a weird snpe state where dmon constantly uses more cpu than normal. -car_started_processes += ['modeld'] def register_managed_process(name, desc, car_started=False): global managed_processes, car_started_processes, persistent_processes - print("registering %s" % name) managed_processes[name] = desc if car_started: car_started_processes.append(name) @@ -326,22 +306,22 @@ def start_daemon_process(name): params.put(pid_param, str(proc.pid)) -def prepare_managed_process(p): +def prepare_managed_process(p, build=False): proc = managed_processes[p] if isinstance(proc, str): # import this python cloudlog.info("preimporting %s" % proc) importlib.import_module(proc) - elif os.path.isfile(os.path.join(BASEDIR, proc[0], "Makefile")): + elif os.path.isfile(os.path.join(BASEDIR, proc[0], "SConscript")) and build: # build this process cloudlog.info("building %s" % (proc,)) try: - subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + subprocess.check_call(["scons", "u", "-j4", "."], cwd=os.path.join(BASEDIR, proc[0])) except subprocess.CalledProcessError: - # make clean if the build failed - cloudlog.warning("building %s failed, make clean" % (proc, )) - subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0])) - subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + # clean and retry if the build failed + cloudlog.warning("building %s failed, cleaning and retrying" % (proc, )) + subprocess.check_call(["scons", "-u", "-c", "."], cwd=os.path.join(BASEDIR, proc[0])) + subprocess.check_call(["scons", "-u", "-j4", "."], cwd=os.path.join(BASEDIR, proc[0])) def join_process(process, timeout): @@ -373,7 +353,8 @@ def kill_managed_process(name): join_process(running[name], 15) if running[name].exitcode is None: cloudlog.critical("unkillable process %s failed to die!" % name) - os.system("date >> /sdcard/unkillable_reboot") + os.system("date >> /data/unkillable_reboot") + os.sync() HARDWARE.reboot() raise RuntimeError else: @@ -388,7 +369,7 @@ def kill_managed_process(name): def cleanup_all_processes(signal, frame): cloudlog.info("caught ctrl-c %s %s" % (signal, frame)) - if ANDROID: + if EON: pm_apply_packages('disable') for name in list(running.keys()): @@ -407,21 +388,23 @@ def send_managed_process_signal(name, sig): # ****************** run loop ****************** -def manager_init(should_register=True): - if should_register: - reg_res = register() - if reg_res: - dongle_id = reg_res - else: - raise Exception("server registration failed") - else: - dongle_id = "c"*16 +def manager_init(): + # Create folders needed for msgq + try: + os.mkdir("/dev/shm") + except FileExistsError: + pass + except PermissionError: + print("WARNING: failed to make /dev/shm") # set dongle id - cloudlog.info("dongle id is " + dongle_id) + reg_res = register() + if reg_res: + dongle_id = reg_res + else: + raise Exception("server registration failed") os.environ['DONGLE_ID'] = dongle_id - cloudlog.info("dirty is %d" % dirty) if not dirty: os.environ['CLEAN'] = '1' @@ -436,14 +419,13 @@ def manager_init(should_register=True): pass # ensure shared libraries are readable by apks - if ANDROID: + if EON: os.chmod(BASEDIR, 0o755) + os.chmod("/dev/shm", 0o777) os.chmod(os.path.join(BASEDIR, "cereal"), 0o755) os.chmod(os.path.join(BASEDIR, "cereal", "libmessaging_shared.so"), 0o755) def manager_thread(): - # now loop - thermal_sock = messaging.sub_sock('thermal') cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) @@ -451,8 +433,6 @@ def manager_thread(): # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) - params = Params() - # start daemon processes for p in daemon_processes: start_daemon_process(p) @@ -462,7 +442,7 @@ def manager_thread(): start_managed_process(p) # start offroad - if ANDROID: + if EON: pm_apply_packages('enable') start_offroad() @@ -475,6 +455,8 @@ def manager_thread(): started_prev = False logger_dead = False + params = Params() + thermal_sock = messaging.sub_sock('thermal') while 1: msg = messaging.recv_sock(thermal_sock, wait=True) @@ -505,6 +487,7 @@ def manager_thread(): # trigger an update after going offroad if started_prev: + os.sync() send_managed_process_signal("updated", signal.SIGHUP) started_prev = msg.thermal.started @@ -517,33 +500,19 @@ def manager_thread(): if params.get("DoUninstall", encoding='utf8') == "1": break -def manager_prepare(spinner=None): +def manager_prepare(): # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) # Spinner has to start from 70 here - total = 100.0 if prebuilt else 30.0 + total = 100.0 if PREBUILT else 30.0 for i, p in enumerate(managed_processes): - if spinner is not None: - spinner.update("%d" % ((100.0 - total) + total * (i + 1) / len(managed_processes),)) + perc = (100.0 - total) + total * (i + 1) / len(managed_processes) + spinner.update(str(int(perc))) prepare_managed_process(p) -def uninstall(): - cloudlog.warning("uninstalling") - with open('/cache/recovery/command', 'w') as f: - f.write('--wipe_data\n') - # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) - HARDWARE.reboot(reason="recovery") - def main(): - if ANDROID: - # the flippening! - os.system('LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1') - - # disable bluetooth - os.system('service call bluetooth_manager 8') - params = Params() params.manager_start() @@ -568,17 +537,17 @@ def main(): if params.get(k) is None: params.put(k, v) - # is this chffrplus? + # is this dashcam? if os.getenv("PASSIVE") is not None: params.put("Passive", str(int(os.getenv("PASSIVE")))) if params.get("Passive") is None: raise Exception("Passive must be set to continue") - if ANDROID: + if EON: update_apks() manager_init() - manager_prepare(spinner) + manager_prepare() spinner.close() if os.getenv("PREPAREONLY") is not None: @@ -596,7 +565,8 @@ def main(): cleanup_all_processes(None, None) if params.get("DoUninstall", encoding='utf8') == "1": - uninstall() + cloudlog.warning("uninstalling") + HARDWARE.uninstall() if __name__ == "__main__": @@ -608,7 +578,8 @@ if __name__ == "__main__": # Show last 3 lines of traceback error = traceback.format_exc(-3) - error = "Manager failed to start\n \n" + error + error = "Manager failed to start\n\n" + error + spinner.close() with TextWindow(error) as t: t.wait_for_exit() diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 569bcf0e..4fcded10 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -3,27 +3,23 @@ lenv = env.Clone() libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc] -TEST_THNEED = False - common_src = [ "models/commonmodel.cc", "runners/snpemodel.cc", - "transforms/loadyuv.c", + "transforms/loadyuv.cc", "transforms/transform.cc" ] if arch == "aarch64": libs += ['gsl', 'CB', 'gnustl_shared'] - if not TEST_THNEED: - common_src += ["thneed/thneed.cc"] - lenv['CFLAGS'].append("-DUSE_THNEED") - lenv['CXXFLAGS'].append("-DUSE_THNEED") + common_src += ["thneed/thneed.cc"] + lenv['CFLAGS'].append("-DUSE_THNEED") + lenv['CXXFLAGS'].append("-DUSE_THNEED") elif arch == "larch64": libs += ['gsl', 'CB', 'pthread', 'dl'] - if not TEST_THNEED: - common_src += ["thneed/thneed.cc"] - lenv['CFLAGS'].append("-DUSE_THNEED") - lenv['CXXFLAGS'].append("-DUSE_THNEED") + common_src += ["thneed/thneed.cc"] + lenv['CFLAGS'].append("-DUSE_THNEED") + lenv['CXXFLAGS'].append("-DUSE_THNEED") else: libs += ['pthread'] @@ -56,8 +52,3 @@ lenv.Program('_modeld', [ "models/driving.cc", ]+common, LIBS=libs) -if TEST_THNEED: - lenv.Program('thneed/debug/_thneed', [ - "thneed/thneed.cc", "thneed/debug/test.cc" - ]+common, LIBS=libs) - diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 3a9b134e..80af643d 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -26,10 +26,6 @@ int main(int argc, char **argv) { int err; setpriority(PRIO_PROCESS, 0, -15); -#ifdef QCOM2 - set_core_affinity(5); -#endif - signal(SIGINT, (sighandler_t)set_do_exit); signal(SIGTERM, (sighandler_t)set_do_exit); @@ -66,15 +62,11 @@ int main(int argc, char **argv) { double t2 = millis_since_boot(); // send dm packet - dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0); + const float* raw_pred_ptr = send_raw_pred ? (const float *)dmonitoringmodel.output : nullptr; + dmonitoring_publish(pm, extra.frame_id, res, raw_pred_ptr, (t2-t1)/1000.0); LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); last = t1; -#ifdef QCOM2 - // this makes it run at about 2.7Hz on tici CPU to deal with modeld lags - // TODO: DSP needs to be freed (again) - usleep(250000); -#endif } visionstream_destroy(&stream); } diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index ffb3770d..2febb42a 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -24,8 +24,10 @@ pthread_mutex_t transform_lock; void* live_thread(void *arg) { set_thread_name("live"); + set_realtime_priority(50); SubMaster sm({"liveCalibration"}); + /* import numpy as np from common.transformations.model import medmodel_frame_from_road_frame @@ -60,7 +62,7 @@ void* live_thread(void *arg) { }}, db_s); while (!do_exit) { - if (sm.update(10) > 0){ + if (sm.update(100) > 0){ auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix(); Eigen::Matrix extrinsic_matrix_eigen; @@ -91,7 +93,7 @@ void* live_thread(void *arg) { int main(int argc, char **argv) { int err; - set_realtime_priority(51); + set_realtime_priority(54); #ifdef QCOM set_core_affinity(2); @@ -114,23 +116,13 @@ int main(int argc, char **argv) { PubMaster pm({"modelV2", "model", "cameraOdometry"}); SubMaster sm({"pathPlan", "frame"}); -#if defined(QCOM) || defined(QCOM2) - cl_device_type device_type = CL_DEVICE_TYPE_DEFAULT; -#else - cl_device_type device_type = CL_DEVICE_TYPE_CPU; -#endif - // cl init - cl_device_id device_id = cl_get_device_id(device_type); - cl_context context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); - assert(err == 0); - - cl_command_queue q = clCreateCommandQueue(context, device_id, 0, &err); - assert(err == 0); + cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); // init the models ModelState model; - model_init(&model, device_id, context, true); + model_init(&model, device_id, context); LOGW("models loaded, modeld starting"); // loop @@ -191,10 +183,11 @@ int main(int argc, char **argv) { // TODO: don't make copies! memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len); + visionbuf_sync(&yuv_ion, VISIONBUF_SYNC_TO_DEVICE); ModelDataRaw model_buf = - model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height, - model_transform, NULL, vec_desire); + model_eval_frame(&model, yuv_ion.buf_cl, buf_info.width, buf_info.height, + model_transform, vec_desire); mt2 = millis_since_boot(); float model_execution_time = (mt2 - mt1) / 1000.0; @@ -204,9 +197,9 @@ int main(int argc, char **argv) { if (run_count < 10) frames_dropped = 0; // let frame drops warm up float frame_drop_ratio = frames_dropped / (1 + frames_dropped); - model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); - model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); - posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof); + const float *raw_pred_ptr = send_raw_pred ? &model.output[0] : nullptr; + model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time); + posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof); LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); last = mt1; @@ -223,9 +216,7 @@ int main(int argc, char **argv) { LOG("joining live_thread"); err = pthread_join(live_thread_handle, NULL); assert(err == 0); - clReleaseCommandQueue(q); - clReleaseContext(context); - + CL_CHECK(clReleaseContext(context)); pthread_mutex_destroy(&transform_lock); return 0; } diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 62b2a127..cc7d4b15 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -1,40 +1,31 @@ #include #include #include "commonmodel.h" - +#include "common/clutil.h" #include "common/mat.h" #include "common/timing.h" void frame_init(ModelFrame* frame, int width, int height, cl_device_id device_id, cl_context context) { - int err; - transform_init(&frame->transform, context, device_id); frame->transformed_width = width; frame->transformed_height = height; - frame->transformed_y_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, - (size_t)frame->transformed_width*frame->transformed_height, NULL, &err); - assert(err == 0); - frame->transformed_u_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, - (size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err); - assert(err == 0); - frame->transformed_v_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, - (size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err); - assert(err == 0); - + frame->transformed_y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, + (size_t)frame->transformed_width*frame->transformed_height, NULL, &err)); + frame->transformed_u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, + (size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err)); + frame->transformed_v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, + (size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err)); frame->net_input_size = ((width*height*3)/2)*sizeof(float); - frame->net_input = clCreateBuffer(context, CL_MEM_READ_WRITE, - frame->net_input_size, (void*)NULL, &err); - assert(err == 0); - + frame->net_input = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, + frame->net_input_size, (void*)NULL, &err)); loadyuv_init(&frame->loadyuv, context, device_id, frame->transformed_width, frame->transformed_height); } float *frame_prepare(ModelFrame* frame, cl_command_queue q, cl_mem yuv_cl, int width, int height, - mat3 transform) { - int err; + const mat3 &transform) { transform_queue(&frame->transform, q, yuv_cl, width, height, frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl, @@ -43,9 +34,9 @@ float *frame_prepare(ModelFrame* frame, cl_command_queue q, loadyuv_queue(&frame->loadyuv, q, frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl, frame->net_input); - float *net_input_buf = (float *)clEnqueueMapBuffer(q, frame->net_input, CL_TRUE, + float *net_input_buf = (float *)CL_CHECK_ERR(clEnqueueMapBuffer(q, frame->net_input, CL_TRUE, CL_MAP_READ, 0, frame->net_input_size, - 0, NULL, NULL, &err); + 0, NULL, NULL, &err)); clFinish(q); return net_input_buf; } @@ -53,10 +44,10 @@ float *frame_prepare(ModelFrame* frame, cl_command_queue q, void frame_free(ModelFrame* frame) { transform_destroy(&frame->transform); loadyuv_destroy(&frame->loadyuv); - clReleaseMemObject(frame->net_input); - clReleaseMemObject(frame->transformed_v_cl); - clReleaseMemObject(frame->transformed_u_cl); - clReleaseMemObject(frame->transformed_y_cl); + CL_CHECK(clReleaseMemObject(frame->net_input)); + CL_CHECK(clReleaseMemObject(frame->transformed_v_cl)); + CL_CHECK(clReleaseMemObject(frame->transformed_u_cl)); + CL_CHECK(clReleaseMemObject(frame->transformed_y_cl)); } void softmax(const float* input, float* output, size_t len) { diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 06d46b8f..1e1200db 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -1,6 +1,4 @@ -#ifndef COMMONMODEL_H -#define COMMONMODEL_H - +#pragma once #define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ #include @@ -9,13 +7,12 @@ #endif #include +#include #include "common/mat.h" #include "transforms/transform.h" #include "transforms/loadyuv.h" -#ifdef __cplusplus -extern "C" { -#endif +const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; void softmax(const float* input, float* output, size_t len); float softplus(float input); @@ -34,12 +31,5 @@ void frame_init(ModelFrame* frame, int width, int height, cl_device_id device_id, cl_context context); float *frame_prepare(ModelFrame* frame, cl_command_queue q, cl_mem yuv_cl, int width, int height, - mat3 transform); + const mat3 &transform); void frame_free(ModelFrame* frame); - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index fd262801..f13287f0 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -22,11 +22,8 @@ void dmonitoring_init(DMonitoringModelState* s) { #else const char* model_path = "../../models/dmonitoring_model.dlc"; #endif -#ifdef QCOM2 - int runtime = USE_CPU_RUNTIME; -#else + int runtime = USE_DSP_RUNTIME; -#endif s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, runtime); s->is_rhd = Params().read_db_bool("IsRHD"); } @@ -55,7 +52,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ #else const int full_width_tici = 1928; const int full_height_tici = 1208; - const int adapt_width_tici = 808; + const int adapt_width_tici = 636; const int cropped_height = adapt_width_tici / 1.33; const int cropped_width = cropped_height / 2; @@ -163,7 +160,9 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); //fclose(dump_yuv_file2); + double t1 = millis_since_boot(); s->m->execute(net_input_buf, yuv_buf_len); + double t2 = millis_since_boot(); DMonitoringResult ret = {0}; memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation); @@ -181,30 +180,31 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ ret.face_orientation_meta[2] = softplus(ret.face_orientation_meta[2]); ret.face_position_meta[0] = softplus(ret.face_position_meta[0]); ret.face_position_meta[1] = softplus(ret.face_position_meta[1]); + ret.dsp_execution_time = (t2 - t1) / 1000.; return ret; } -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){ +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time){ // make msg MessageBuilder msg; auto framed = msg.initEvent().initDriverState(); framed.setFrameId(frame_id); framed.setModelExecutionTime(execution_time); + framed.setDspExecutionTime(res.dsp_execution_time); - kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); - kj::ArrayPtr face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta)); - kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); - kj::ArrayPtr face_position_std(&res.face_position_meta[0], ARRAYSIZE(res.face_position_meta)); - framed.setFaceOrientation(face_orientation); - framed.setFaceOrientationStd(face_orientation_std); - framed.setFacePosition(face_position); - framed.setFacePositionStd(face_position_std); + framed.setFaceOrientation(res.face_orientation); + framed.setFaceOrientationStd(res.face_orientation_meta); + framed.setFacePosition(res.face_position); + framed.setFacePositionStd(res.face_position_meta); framed.setFaceProb(res.face_prob); framed.setLeftEyeProb(res.left_eye_prob); framed.setRightEyeProb(res.right_eye_prob); framed.setLeftBlinkProb(res.left_blink_prob); framed.setRightBlinkProb(res.right_blink_prob); framed.setSgProb(res.sg_prob); + if (send_raw_pred) { + framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, OUTPUT_SIZE*sizeof(float))); + } pm.send("driverState", msg); } diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 7360af81..7df3355b 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -5,10 +5,6 @@ #include "runners/run.h" #include "messaging.hpp" -#ifdef __cplusplus -extern "C" { -#endif - #define OUTPUT_SIZE 34 typedef struct DMonitoringResult { @@ -22,6 +18,7 @@ typedef struct DMonitoringResult { float left_blink_prob; float right_blink_prob; float sg_prob; + float dsp_execution_time; } DMonitoringResult; typedef struct DMonitoringModelState { @@ -37,10 +34,6 @@ typedef struct DMonitoringModelState { void dmonitoring_init(DMonitoringModelState* s); DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height); -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time); +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time); void dmonitoring_free(DMonitoringModelState* s); -#ifdef __cplusplus -} -#endif - diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index ebc85f06..ae4b7f9e 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -8,25 +8,42 @@ #include "common/timing.h" #include "common/params.h" #include "driving.h" +#include "clutil.h" -#define MIN_VALID_LEN 10.0 -#define TRAJECTORY_SIZE 33 -#define TRAJECTORY_TIME 10.0 -#define TRAJECTORY_DISTANCE 192.0 -#define PLAN_IDX 0 -#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE) -#define LL_PROB_IDX LL_IDX + 4*2*2*33 -#define RE_IDX LL_PROB_IDX + 4 -#define LEAD_IDX RE_IDX + 2*2*2*33 -#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE) -#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3 -#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN -#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE -#define OUTPUT_SIZE POSE_IDX + POSE_SIZE +constexpr int MODEL_WIDTH = 512; +constexpr int MODEL_HEIGHT = 256; +constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; + +constexpr int PLAN_MHP_N = 5; +constexpr int PLAN_MHP_COLUMNS = 30; +constexpr int PLAN_MHP_VALS = 30*33; +constexpr int PLAN_MHP_SELECTION = 1; +constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION); + +constexpr int LEAD_MHP_N = 5; +constexpr int LEAD_MHP_VALS = 4; +constexpr int LEAD_MHP_SELECTION = 3; +constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION); + +constexpr int POSE_SIZE = 12; + +constexpr int MIN_VALID_LEN = 10; +constexpr int TRAJECTORY_TIME = 10; +constexpr float TRAJECTORY_DISTANCE = 192.0; +constexpr int PLAN_IDX = 0; +constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE; +constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33; +constexpr int RE_IDX = LL_PROB_IDX + 4; +constexpr int LEAD_IDX = RE_IDX + 2*2*2*33; +constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE); +constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3; +constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN; +constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE; +constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE; #ifdef TEMPORAL - #define TEMPORAL_SIZE 512 + constexpr int TEMPORAL_SIZE = 512; #else - #define TEMPORAL_SIZE 0 + constexpr int TEMPORAL_SIZE = 0; #endif // #define DUMP_YUV @@ -35,36 +52,26 @@ Eigen::Matrix vander; float X_IDXS[TRAJECTORY_SIZE]; float T_IDXS[TRAJECTORY_SIZE]; -void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) { +void model_init(ModelState* s, cl_device_id device_id, cl_context context) { frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); - s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float)); + s->input_frames = std::make_unique(MODEL_FRAME_SIZE * 2); - const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE; - s->output = (float*)calloc(output_size, sizeof(float)); - - s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME); + constexpr int output_size = OUTPUT_SIZE + TEMPORAL_SIZE; + s->output = std::make_unique(output_size); + s->m = std::make_unique("../../models/supercombo.dlc", &s->output[0], output_size, USE_GPU_RUNTIME); #ifdef TEMPORAL - assert(temporal); s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); #endif #ifdef DESIRE - s->prev_desire = std::make_unique(DESIRE_LEN); - s->pulse_desire = std::make_unique(DESIRE_LEN); - s->m->addDesire(s->pulse_desire.get(), DESIRE_LEN); + s->m->addDesire(s->pulse_desire, DESIRE_LEN); #endif #ifdef TRAFFIC_CONVENTION - s->traffic_convention = std::make_unique(TRAFFIC_CONVENTION_LEN); - s->m->addTrafficConvention(s->traffic_convention.get(), TRAFFIC_CONVENTION_LEN); - - bool is_rhd = Params().read_db_bool("IsRHD"); - if (is_rhd) { - s->traffic_convention[1] = 1.0; - } else { - s->traffic_convention[0] = 1.0; - } + const int idx = Params().read_db_bool("IsRHD") ? 1 : 0; + s->traffic_convention[idx] = 1.0; + s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif // Build Vandermonde matrix @@ -75,12 +82,12 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1); } } + + s->q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); } -ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, - cl_mem yuv_cl, int width, int height, - mat3 transform, void* sock, - float *desire_in) { +ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height, + const mat3 &transform, float *desire_in) { #ifdef DESIRE if (desire_in != NULL) { for (int i = 1; i < DESIRE_LEN; i++) { @@ -98,10 +105,10 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n"); - float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform); + float *new_frame_buf = frame_prepare(&s->frame, s->q, yuv_cl, width, height, transform); memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE); memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE); - s->m->execute(s->input_frames, MODEL_FRAME_SIZE*2); + s->m->execute(&s->input_frames[0], MODEL_FRAME_SIZE*2); #ifdef DUMP_YUV FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb"); @@ -110,7 +117,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, assert(1==2); #endif - clEnqueueUnmapMemObject(q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL); + clEnqueueUnmapMemObject(s->q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL); // net outputs ModelDataRaw net_outputs; @@ -126,10 +133,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, } void model_free(ModelState* s) { - free(s->output); - free(s->input_frames); frame_free(&s->frame); - delete s->m; + CL_CHECK(clReleaseCommandQueue(s->q)); } void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { @@ -157,50 +162,53 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { out[3] = y0; } -void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) { - float points_arr[TRAJECTORY_SIZE]; - float stds_arr[TRAJECTORY_SIZE]; - float poly_arr[POLYFIT_DEGREE]; - float std; - - for (int i=0; i + data[(max_idx + 1) * group_size + offset]) { + max_idx = i; + } } - std = stds_arr[0]; - poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx); - - kj::ArrayPtr poly(&poly_arr[0], ARRAYSIZE(poly_arr)); - path.setPoly(poly); - path.setProb(1.0); - path.setStd(std); - path.setValidLen(valid_len); + return &data[max_idx * group_size]; } -void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) { - float points_arr[TRAJECTORY_SIZE]; - float stds_arr[TRAJECTORY_SIZE]; - float poly_arr[POLYFIT_DEGREE]; - float std; +static const float *get_plan_data(float *plan) { + return get_best_data(plan, PLAN_MHP_N, PLAN_MHP_GROUP_SIZE, -1); +} + +static const float *get_lead_data(const float *lead, int t_offset) { + return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION); +} + +void fill_path(cereal::ModelData::PathData::Builder path, const float *data, const float prob, + float valid_len, int valid_len_idx, int ll_idx) { + float points[TRAJECTORY_SIZE] = {}; + float stds[TRAJECTORY_SIZE] = {}; + float poly[POLYFIT_DEGREE] = {}; for (int i=0; i poly(&poly_arr[0], ARRAYSIZE(poly_arr)); path.setPoly(poly); path.setProb(prob); path.setStd(std); path.setValidLen(valid_len); } -void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) { - lead.setProb(prob); +void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) { + const float *data = get_lead_data(lead_data, t_offset); + lead.setProb(sigmoid(prob[t_offset])); lead.setT(t); float xyva_arr[LEAD_MHP_VALS]; float xyva_stds_arr[LEAD_MHP_VALS]; @@ -208,14 +216,13 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * d xyva_arr[i] = data[LEAD_MHP_VALS + i]; xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]); } - kj::ArrayPtr xyva(xyva_arr, LEAD_MHP_VALS); - kj::ArrayPtr xyva_stds(xyva_stds_arr, LEAD_MHP_VALS); - lead.setXyva(xyva); - lead.setXyvaStd(xyva_stds); + lead.setXyva(xyva_arr); + lead.setXyvaStd(xyva_stds_arr); } -void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) { - lead.setProb(prob); +void fill_lead(cereal::ModelData::LeadData::Builder lead, const float *lead_data, const float *prob, int t_offset) { + const float *data = get_lead_data(lead_data, t_offset); + lead.setProb(sigmoid(prob[t_offset])); lead.setDist(data[0]); lead.setStd(exp(data[LEAD_MHP_VALS])); // TODO make all msgs same format @@ -227,7 +234,8 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, fl lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3])); } -void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { +template +void fill_meta(MetaBuilder meta, const float *meta_data) { float desire_state_softmax[DESIRE_LEN]; float desire_pred_softmax[4*DESIRE_LEN]; softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN); @@ -235,39 +243,19 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN], &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN); } - kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); - meta.setDesireState(desire_state); + meta.setDesireState(desire_state_softmax); meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); - kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); - meta.setDesirePrediction(desire_pred); -} - -void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) { - float desire_state_softmax[DESIRE_LEN]; - float desire_pred_softmax[4*DESIRE_LEN]; - softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN); - for (int i=0; i<4; i++) { - softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN], - &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN); - } - kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); - meta.setDesireState(desire_state); - meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); - meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); - meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); - meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); - kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); - meta.setDesirePrediction(desire_pred); + meta.setDesirePrediction(desire_pred_softmax); } void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, int columns, int column_offset, float * plan_t_arr) { - float x_arr[TRAJECTORY_SIZE]; - float y_arr[TRAJECTORY_SIZE]; - float z_arr[TRAJECTORY_SIZE]; + float x_arr[TRAJECTORY_SIZE] = {}; + float y_arr[TRAJECTORY_SIZE] = {}; + float z_arr[TRAJECTORY_SIZE] = {}; //float x_std_arr[TRAJECTORY_SIZE]; //float y_std_arr[TRAJECTORY_SIZE]; //float z_std_arr[TRAJECTORY_SIZE]; @@ -288,60 +276,30 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, z_arr[i] = data[i*columns + 2 + column_offset]; //z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; } - kj::ArrayPtr x(x_arr, TRAJECTORY_SIZE); - kj::ArrayPtr y(y_arr, TRAJECTORY_SIZE); - kj::ArrayPtr z(z_arr, TRAJECTORY_SIZE); //kj::ArrayPtr x_std(x_std_arr, TRAJECTORY_SIZE); //kj::ArrayPtr y_std(y_std_arr, TRAJECTORY_SIZE); //kj::ArrayPtr z_std(z_std_arr, TRAJECTORY_SIZE); - kj::ArrayPtr t(t_arr, TRAJECTORY_SIZE); - xyzt.setX(x); - xyzt.setY(y); - xyzt.setZ(z); + xyzt.setX(x_arr); + xyzt.setY(y_arr); + xyzt.setZ(z_arr); //xyzt.setXStd(x_std); //xyzt.setYStd(y_std); //xyzt.setZStd(z_std); - xyzt.setT(t); + xyzt.setT(t_arr); } - -void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, - const ModelDataRaw &net_outputs, uint64_t timestamp_eof, - float model_execution_time) { - // make msg - MessageBuilder msg; - auto framed = msg.initEvent().initModelV2(); - uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; - framed.setFrameId(vipc_frame_id); - framed.setFrameAge(frame_age); - framed.setFrameDropPerc(frame_drop * 100); - framed.setTimestampEof(timestamp_eof); - framed.setModelExecutionTime(model_execution_time); - +void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) { // plan - int plan_mhp_max_idx = 0; - for (int i=1; i - net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) { - plan_mhp_max_idx = i; - } - } - - float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)]; + const float *best_plan = get_plan_data(net_outputs.plan); float plan_t_arr[TRAJECTORY_SIZE]; for (int i=0; i lane_line_probs(lane_line_probs_arr, 4); - framed.setLaneLineProbs(lane_line_probs); + framed.setLaneLineProbs(lane_line_probs_arr); framed.setLaneLineStds(lane_line_stds_arr); // road edges @@ -366,56 +323,24 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, framed.setRoadEdgeStds(road_edge_stds_arr); // meta - auto meta = framed.initMeta(); - fill_meta_v2(meta, net_outputs.meta); + fill_meta(framed.initMeta(), net_outputs.meta); // leads auto leads = framed.initLeads(LEAD_MHP_SELECTION); - int mdn_max_idx = 0; float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0}; for (int t_offset=0; t_offset - net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) { - mdn_max_idx = i; - fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], - sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]); - } - } + fill_lead_v2(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]); } - pm.send("modelV2", msg); } -void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, - const ModelDataRaw &net_outputs, uint64_t timestamp_eof, - float model_execution_time) { - - uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; - MessageBuilder msg; - auto framed = msg.initEvent().initModel(); - framed.setFrameId(vipc_frame_id); - framed.setFrameAge(frame_age); - framed.setFrameDropPerc(frame_drop * 100); - framed.setTimestampEof(timestamp_eof); - framed.setModelExecutionTime(model_execution_time); - +void fill_model(cereal::ModelData::Builder &framed, const ModelDataRaw &net_outputs) { // Find the distribution that corresponds to the most probable plan - int plan_mhp_max_idx = 0; - for (int i=1; i - net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) { - plan_mhp_max_idx = i; - } - } - + const float *best_plan = get_plan_data(net_outputs.plan); // x pos at 10s is a good valid_len float valid_len = 0; - float valid_len_candidate; for (int i=1; i= valid_len){ - valid_len = valid_len_candidate; + if (const float len = best_plan[30*i]; len >= valid_len){ + valid_len = len; } } // clamp to 10 and MODEL_PATH_DISTANCE @@ -426,47 +351,39 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, valid_len_idx = i; } } + fill_path(framed.initPath(), best_plan, 1.0, valid_len, valid_len_idx, 0); + fill_path(framed.initLeftLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[1]), valid_len, valid_len_idx, 1); + fill_path(framed.initRightLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[2]), valid_len, valid_len_idx, 2); - auto lpath = framed.initPath(); - fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx); - - auto left_lane = framed.initLeftLane(); - int ll_idx = 1; - fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx, - sigmoid(net_outputs.lane_lines_prob[ll_idx])); - auto right_lane = framed.initRightLane(); - ll_idx = 2; - fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx, - sigmoid(net_outputs.lane_lines_prob[ll_idx])); - - // Find the distribution that corresponds to the current lead - int mdn_max_idx = 0; - int t_offset = 0; - for (int i=1; i - net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) { - mdn_max_idx = i; - } - } - fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset])); - // Find the distribution that corresponds to the lead in 2s - mdn_max_idx = 0; - t_offset = 1; - for (int i=1; i - net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) { - mdn_max_idx = i; - } - } - fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset])); + fill_lead(framed.initLead(), net_outputs.lead, net_outputs.lead_prob, 0); + fill_lead(framed.initLeadFuture(), net_outputs.lead, net_outputs.lead_prob, 1); fill_meta(framed.initMeta(), net_outputs.meta); - - pm.send("model", msg); } -void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop, + const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof, + float model_execution_time) { + const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; + auto do_publish = [&](auto init_model_func, const char *pub_name) { + MessageBuilder msg; + auto framed = (msg.initEvent().*(init_model_func))(); + framed.setFrameId(vipc_frame_id); + framed.setFrameAge(frame_age); + framed.setFrameDropPerc(frame_drop * 100); + framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); + if (send_raw_pred) { + framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float))); + } + fill_model(framed, net_outputs); + pm.send(pub_name, msg); + }; + do_publish(&cereal::Event::Builder::initModel, "model"); + do_publish(&cereal::Event::Builder::initModelV2, "modelV2"); +} + +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { float trans_arr[3]; float trans_std_arr[3]; @@ -483,14 +400,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, MessageBuilder msg; auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry(); - kj::ArrayPtr trans_vs(&trans_arr[0], 3); - posenetd.setTrans(trans_vs); - kj::ArrayPtr rot_vs(&rot_arr[0], 3); - posenetd.setRot(rot_vs); - kj::ArrayPtr trans_std_vs(&trans_std_arr[0], 3); - posenetd.setTransStd(trans_std_vs); - kj::ArrayPtr rot_std_vs(&rot_std_arr[0], 3); - posenetd.setRotStd(rot_std_vs); + posenetd.setTrans(trans_arr); + posenetd.setRot(rot_arr); + posenetd.setTransStd(trans_std_arr); + posenetd.setRotStd(rot_std_arr); posenetd.setTimestampEof(timestamp_eof); posenetd.setFrameId(vipc_frame_id); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 93177105..c5891013 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -1,12 +1,10 @@ -#ifndef MODEL_H -#define MODEL_H +#pragma once // gate this here #define TEMPORAL #define DESIRE #define TRAFFIC_CONVENTION - #include "common/mat.h" #include "common/util.h" #include "common/modeldata.h" @@ -17,29 +15,9 @@ #include #include "messaging.hpp" -#define MODEL_NAME "supercombo_dlc" - -#define MODEL_WIDTH 512 -#define MODEL_HEIGHT 256 -#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2 -#define DESIRE_LEN 8 -#define TRAFFIC_CONVENTION_LEN 2 - -#define PLAN_MHP_N 5 -#define PLAN_MHP_COLUMNS 30 -#define PLAN_MHP_VALS 30*33 -#define PLAN_MHP_SELECTION 1 -#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION) - -#define LEAD_MHP_N 5 -#define LEAD_MHP_VALS 4 -#define LEAD_MHP_SELECTION 3 -#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION) - -#define POSE_SIZE 12 - -#define MODEL_FREQ 20 - +constexpr int DESIRE_LEN = 8; +constexpr int TRAFFIC_CONVENTION_LEN = 2; +constexpr int MODEL_FREQ = 20; struct ModelDataRaw { float *plan; float *lane_lines; @@ -56,33 +34,26 @@ struct ModelDataRaw { typedef struct ModelState { ModelFrame frame; - float *output; - float *input_frames; - RunModel *m; + std::unique_ptr output; + std::unique_ptr input_frames; + std::unique_ptr m; + cl_command_queue q; #ifdef DESIRE - std::unique_ptr prev_desire; - std::unique_ptr pulse_desire; + float prev_desire[DESIRE_LEN] = {}; + float pulse_desire[DESIRE_LEN] = {}; #endif #ifdef TRAFFIC_CONVENTION - std::unique_ptr traffic_convention; + float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; #endif } ModelState; -void model_init(ModelState* s, cl_device_id device_id, - cl_context context, int temporal); -ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, - cl_mem yuv_cl, int width, int height, - mat3 transform, void* sock, float *desire_in); +void model_init(ModelState* s, cl_device_id device_id, cl_context context); +ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height, + const mat3 &transform, float *desire_in); void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); - -void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, - uint64_t timestamp_eof, float model_execution_time); -void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, - uint64_t timestamp_eof, float model_execution_time); -void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, - uint64_t timestamp_eof); -#endif +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop, + const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof, + float model_execution_time); +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof); diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc index e08370a7..44328565 100644 --- a/selfdrive/modeld/thneed/thneed.cc +++ b/selfdrive/modeld/thneed/thneed.cc @@ -7,9 +7,16 @@ #include #include "thneed.h" +//#define SAVE_KERNELS + +//#define RUN_DISASSEMBLER +//#define RUN_OPTIMIZER + Thneed *g_thneed = NULL; int g_fd = -1; map, string> g_args; +map, int> g_args_size; +map g_program_source; static inline uint64_t nanos_since_boot() { struct timespec t; @@ -27,6 +34,8 @@ void hexdump(uint32_t *d, int len) { printf("\n"); } +// *********** ioctl interceptor *********** + extern "C" { int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL; @@ -39,32 +48,33 @@ int ioctl(int filedes, unsigned long request, void *argp) { // 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("creating context with flags 0x%x\n", create->flags); + 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 & 1) { + if (thneed->record & THNEED_RECORD) { thneed->timestamp = cmd->timestamp; thneed->context_id = cmd->context_id; thneed->cmds.push_back(unique_ptr(new CachedCommand(thneed, cmd))); } - if (thneed->record & 2) { - printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx context_id: %u timestamp: %u\n", + if (thneed->record & THNEED_DEBUG) { + 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->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->record & 2) { + if (thneed->record & THNEED_DEBUG) { 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); @@ -72,20 +82,20 @@ int ioctl(int filedes, unsigned long request, void *argp) { printf("\n"); } - if (thneed->record & 1) { + if (thneed->record & THNEED_RECORD) { thneed->syncobjs.push_back(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->record & 2) { + if (thneed->record & THNEED_DEBUG) { 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->record & 2) { + if (thneed->record & THNEED_DEBUG) { 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->record & 4) { + if (thneed->record & THNEED_VERBOSE_DEBUG) { hexdump((uint32_t *)prop->value, prop->sizebytes); if (prop->type == KGSL_PROP_PWR_CONSTRAINT) { struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value; @@ -103,6 +113,8 @@ int ioctl(int filedes, unsigned long request, void *argp) { } +// *********** GPUMalloc *********** + GPUMalloc::GPUMalloc(int size, int fd) { struct kgsl_gpuobj_alloc alloc; memset(&alloc, 0, sizeof(alloc)); @@ -128,30 +140,38 @@ void *GPUMalloc::alloc(int size) { return ret; } +// *********** CachedCommand, at the ioctl layer *********** + CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) { thneed = lthneed; - assert(cmd->numcmds == 2); - assert(cmd->numobjs == 1); assert(cmd->numsyncs == 0); - memcpy(cmds, (void *)cmd->cmdlist, sizeof(struct kgsl_command_object)*2); - memcpy(objs, (void *)cmd->objlist, sizeof(struct kgsl_command_object)*1); - memcpy(&cache, cmd, sizeof(cache)); - cache.cmdlist = (uint64_t)cmds; - cache.objlist = (uint64_t)objs; - 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->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; + } } - 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; + 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(bool wait) { @@ -168,19 +188,33 @@ void CachedCommand::exec(bool wait) { int wret = ioctl(thneed->fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait); uint64_t te = nanos_since_boot(); - if (thneed->record & 2) printf("exec %d wait %d after %lu us\n", ret, wret, (te-tb)/1000); + if (thneed->record & THNEED_DEBUG) printf("exec %d wait %d after %lu us\n", ret, wret, (te-tb)/1000); } else { - if (thneed->record & 2) printf("CachedCommand::exec got %d\n", ret); + if (thneed->record & THNEED_DEBUG) printf("CachedCommand::exec got %d\n", ret); + } + + if (thneed->record & THNEED_VERBOSE_DEBUG) { + for (auto &it : kq) { + it->debug_print(false); + } + #ifdef RUN_DISASSEMBLER + // assuming 2 commands + disassemble(0); + disassemble(1); + #endif } assert(ret == 0); } -Thneed::Thneed() { +// *********** Thneed *********** + +Thneed::Thneed(bool do_clinit) { + if (do_clinit) clinit(); assert(g_fd != -1); fd = g_fd; ram = make_unique(0x40000, fd); - record = 1; + record = THNEED_RECORD; timestamp = -1; g_thneed = this; } @@ -189,29 +223,18 @@ void Thneed::stop() { record = 0; } -//#define SAVE_LOG - void Thneed::execute(float **finputs, float *foutput, bool slow) { + int ret; uint64_t tb, te; - if (record & 2) tb = nanos_since_boot(); - - #ifdef SAVE_LOG - char fn[0x100]; - snprintf(fn, sizeof(fn), "/tmp/thneed_log_%d", timestamp); - FILE *f = fopen(fn, "wb"); - #endif + if (record & THNEED_DEBUG) tb = nanos_since_boot(); // ****** copy inputs for (int idx = 0; idx < inputs.size(); ++idx) { size_t sz; clGetMemObjectInfo(inputs[idx], CL_MEM_SIZE, sizeof(sz), &sz, NULL); - #ifdef SAVE_LOG - fwrite(&sz, 1, sizeof(sz), f); - fwrite(finputs[idx], 1, sz, f); - #endif - - if (record & 2) printf("copying %lu -- %p -> %p\n", sz, finputs[idx], inputs[idx]); + if (record & THNEED_DEBUG) printf("copying %lu -- %p -> %p\n", sz, finputs[idx], inputs[idx]); + // TODO: This shouldn't have to block clEnqueueWriteBuffer(command_queue, inputs[idx], CL_TRUE, 0, sz, finputs[idx], 0, NULL, NULL); } @@ -229,40 +252,38 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) { prop.type = KGSL_PROP_PWR_CONSTRAINT; prop.value = (void*)&constraint; prop.sizebytes = sizeof(constraint); - int ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); + ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); assert(ret == 0); // ****** run commands int i = 0; - for (auto it = cmds.begin(); it != cmds.end(); ++it) { + for (auto &it : cmds) { ++i; - if (record & 2) printf("run %2d: ", i); - (*it)->exec((i == cmds.size()) || slow); + if (record & THNEED_DEBUG) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000); + it->exec((i == cmds.size()) || slow); } // ****** sync objects - for (auto it = syncobjs.begin(); it != syncobjs.end(); ++it) { + for (auto &it : syncobjs) { struct kgsl_gpuobj_sync cmd; - cmd.objs = (uint64_t)it->data(); - cmd.obj_len = it->length(); - cmd.count = it->length() / sizeof(struct kgsl_gpuobj_sync_obj); + cmd.objs = (uint64_t)it.data(); + cmd.obj_len = it.length(); + cmd.count = it.length() / sizeof(struct kgsl_gpuobj_sync_obj); ret = ioctl(fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd); assert(ret == 0); } // ****** copy outputs - size_t sz; - clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - if (record & 2) printf("copying %lu for output %p -> %p\n", sz, output, foutput); - clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL); - - #ifdef SAVE_LOG - fwrite(&sz, 1, sizeof(sz), f); - fwrite(foutput, 1, sz, f); - fclose(f); - #endif + if (output != NULL) { + size_t sz; + clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + if (record & THNEED_DEBUG) 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"); + } // ****** unset power constraint constraint.type = KGSL_CONSTRAINT_NONE; @@ -272,25 +293,59 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) { ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); assert(ret == 0); - if (record & 2) { + if (record & THNEED_DEBUG) { te = nanos_since_boot(); printf("model exec in %lu us\n", (te-tb)/1000); } } -// TODO: with a different way of getting the input and output buffers, we don't have to intercept CL at all +void Thneed::clinit() { + cl_int err; + + cl_platform_id platform_id[2]; + cl_uint num_devices; + cl_uint num_platforms; + + err = clGetPlatformIDs(sizeof(platform_id)/sizeof(cl_platform_id), platform_id, &num_platforms); + assert(err == 0); + + err = clGetDeviceIDs(platform_id[0], CL_DEVICE_TYPE_DEFAULT, 1, &device_id, &num_devices); + assert(err == 0); + + context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); + assert(err == 0); + + //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 = clCreateCommandQueueWithProperties(context, device_id, props, &err); + assert(err == 0); + + 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 & THNEED_RECORD) ckq.push_back(k); + cl_int ret = k->exec(); + assert(ret == CL_SUCCESS); + } + return clFinish(command_queue); +} + +// *********** OpenCL interceptor *********** -cl_int (*my_clSetKernelArg)(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value) = NULL; cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value) { - if (my_clSetKernelArg == NULL) my_clSetKernelArg = reinterpret_cast(dlsym(RTLD_NEXT, "REAL_clSetKernelArg")); + 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 = my_clSetKernelArg(kernel, arg_index, arg_size, arg_value); + cl_int ret = clSetKernelArg(kernel, arg_index, arg_size, arg_value); return ret; } -cl_int (*my_clEnqueueNDRangeKernel)(cl_command_queue, cl_kernel, cl_uint, const size_t *, const size_t *, const size_t *, cl_uint, const cl_event *, cl_event *) = NULL; cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue, cl_kernel kernel, cl_uint work_dim, @@ -301,62 +356,183 @@ cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue, const cl_event *event_wait_list, cl_event *event) { - if (my_clEnqueueNDRangeKernel == NULL) my_clEnqueueNDRangeKernel = reinterpret_cast(dlsym(RTLD_NEXT, "REAL_clEnqueueNDRangeKernel")); 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); - char name[0x100]; - clGetKernelInfo(kernel, CL_KERNEL_FUNCTION_NAME, sizeof(name), name, NULL); + cl_int ret = 0; + if (thneed != NULL && thneed->record & 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); + } - cl_uint num_args; + // 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 & THNEED_RECORD) { + #ifdef RUN_OPTIMIZER + thneed->optimize(); + #endif + 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) { + // TODO: Find dlsym in a better way. Currently this is hand looked up in libdl.so +#if defined QCOM + void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen-0x2d4); +#elif defined QCOM2 + void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen+0x138); +#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); - if (thneed != NULL && thneed->record & 1) { - thneed->command_queue = command_queue; - for (int i = 0; i < num_args; i++) { + // 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)); + 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(); + + for (int j = 0; j < num_args; j++) { char arg_name[0x100]; - clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); - string arg = g_args[make_pair(kernel, i)]; + clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); + arg_names.push_back(string(arg_name)); - if (strcmp(arg_name, "input") == 0 && strcmp(name, "zero_pad_image_float") == 0) { - cl_mem mem; - memcpy(&mem, (void*)arg.data(), sizeof(mem)); - thneed->inputs.push_back(mem); + 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); + } + } + + // save the global inputs/outputs + if (thneed->record & THNEED_RECORD) { + for (int i = 0; i < num_args; i++) { + if (name == "zero_pad_image_float" && arg_names[i] == "input") { + thneed->inputs.push_back(*(cl_mem*)(args[i].data())); } - if (strcmp(arg_name, "output") == 0 && strcmp(name, "image2d_to_buffer_float") == 0) { - cl_mem mem; - memcpy(&mem, (void*)arg.data(), sizeof(mem)); - thneed->output = mem; + if (name == "image2d_to_buffer_float" && arg_names[i] == "output") { + thneed->output = *(cl_mem*)(args[i].data()); } } } - if (thneed != NULL && thneed->record & 2) { - printf("%p %56s -- ", kernel, name); - 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 (thneed->record & THNEED_DEBUG) { + debug_print(thneed->record & THNEED_VERBOSE_DEBUG); } - if (thneed != NULL && thneed->record & 4) { - // extreme debug + + 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++) { char arg_type[0x100]; - char arg_name[0x100]; clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_type), arg_type, NULL); - clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); - string arg = g_args[make_pair(kernel, i)]; - printf(" %s %s", arg_type, arg_name); + string arg = args[i]; + printf(" %s %s", arg_type, arg_names[i].c_str()); void *arg_value = (void*)arg.data(); int arg_size = arg.size(); - if (arg_size == 1) { + 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)); @@ -373,19 +549,24 @@ cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue, if (strcmp("image2d_t", arg_type) == 0 || strcmp("image1d_t", arg_type) == 0) { 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_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL); clGetImageInfo(val, CL_IMAGE_SLICE_PITCH, sizeof(slice_pitch), &slice_pitch, NULL); assert(depth == 0); assert(array_size == 0); assert(slice_pitch == 0); - printf(" image %zu x %zu rp %zu", width, height, row_pitch); + 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); @@ -396,79 +577,5 @@ cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue, printf("\n"); } } - - cl_int ret = my_clEnqueueNDRangeKernel(command_queue, kernel, work_dim, - global_work_offset, global_work_size, local_work_size, - num_events_in_wait_list, event_wait_list, event); - - /*uint64_t tb = nanos_since_boot(); - clWaitForEvents(1, event); - uint64_t te = nanos_since_boot(); - if (thneed != NULL && thneed->record & 2) { - printf(" wait %lu us\n", (te-tb)/1000); - }*/ - - return ret; -} - -//#define SAVE_KERNELS - -#ifdef SAVE_KERNELS -map program_source; - -cl_program (*my_clCreateProgramWithSource)(cl_context context, cl_uint count, const char **strings, const size_t *lengths, cl_int *errcode_ret) = NULL; -cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, const char **strings, const size_t *lengths, cl_int *errcode_ret) { - if (my_clCreateProgramWithSource == NULL) my_clCreateProgramWithSource = reinterpret_cast(dlsym(RTLD_NEXT, "REAL_clCreateProgramWithSource")); - assert(count == 1); - size_t my_lengths[1]; - my_lengths[0] = lengths[0]; - - char fn[0x100]; - snprintf(fn, sizeof(fn), "/tmp/program_%zu.cl", strlen(strings[0])); - FILE *f = fopen(fn, "wb"); - fprintf(f, "%s", strings[0]); - fclose(f); - - char tmp[0x10000]; - memset(tmp, 0, sizeof(tmp)); - snprintf(fn, sizeof(fn), "/tmp/patched_%zu.cl", strlen(strings[0])); - FILE *g = fopen(fn, "rb"); - if (g != NULL) { - printf("LOADING PATCHED PROGRAM %s\n", fn); - fread(tmp, 1, sizeof(tmp), g); - fclose(g); - strings[0] = tmp; - my_lengths[0] = strlen(tmp); - } - - program_source[ret] = strings[0]; - - cl_program ret = my_clCreateProgramWithSource(context, count, strings, my_lengths, errcode_ret); - return ret; -} -#endif - -void *dlsym(void *handle, const char *symbol) { - // TODO: Find dlsym in a better way. Currently this is hand looked up in libdl.so -#if defined QCOM - void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen-0x2d4); -#elif defined QCOM2 - void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen+0x138); -#else - #error "Unsupported platform for thneed" -#endif - if (memcmp("REAL_", symbol, 5) == 0) { - return my_dlsym(handle, symbol+5); - } else if (strcmp("clEnqueueNDRangeKernel", symbol) == 0) { - return (void*)thneed_clEnqueueNDRangeKernel; - } else if (strcmp("clSetKernelArg", symbol) == 0) { - return (void*)thneed_clSetKernelArg; -#ifdef SAVE_KERNELS - } else if (strcmp("clCreateProgramWithSource", symbol) == 0) { - return (void*)thneed_clCreateProgramWithSource; -#endif - } else { - return my_dlsym(handle, symbol); - } } diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h index a145a284..e1039efd 100644 --- a/selfdrive/modeld/thneed/thneed.h +++ b/selfdrive/modeld/thneed/thneed.h @@ -9,10 +9,18 @@ #include "include/msm_kgsl.h" #include #include +#include #include +#define THNEED_RECORD 1 +#define THNEED_DEBUG 2 +#define THNEED_VERBOSE_DEBUG 4 + using namespace std; +namespace json11 { + class Json; +} class Thneed; class GPUMalloc { @@ -25,28 +33,59 @@ class GPUMalloc { int remaining; }; +class CLQueuedKernel { + public: + CLQueuedKernel(Thneed *lthneed) { thneed = lthneed; } + CLQueuedKernel(Thneed *lthneed, + cl_kernel _kernel, + cl_uint _work_dim, + const size_t *_global_work_size, + const size_t *_local_work_size); + cl_int exec(); + void debug_print(bool verbose); + int get_arg_num(const char *search_arg_name); + cl_program program; + string name; + cl_uint num_args; + vector arg_names; + vector args; + vector args_size; + cl_kernel kernel = NULL; + json11::Json to_json() const; + + cl_uint work_dim; + size_t global_work_size[3] = {0}; + size_t local_work_size[3] = {0}; + private: + Thneed *thneed; +}; + class CachedCommand { public: CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd); void exec(bool wait); - void disassemble(); + void disassemble(int cmd_index); private: struct kgsl_gpu_command cache; - struct kgsl_command_object cmds[2]; - struct kgsl_command_object objs[1]; + unique_ptr cmds; + unique_ptr objs; Thneed *thneed; + vector > kq; }; class Thneed { public: - Thneed(); + Thneed(bool do_clinit=false); void stop(); void execute(float **finputs, float *foutput, bool slow=false); + int optimize(); vector inputs; - cl_mem output; + cl_mem output = NULL; + cl_context context = NULL; cl_command_queue command_queue; + cl_device_id device_id; int context_id; // protected? @@ -56,5 +95,19 @@ class Thneed { vector > cmds; vector syncobjs; int fd; + + // all CL kernels + cl_int clexec(); + vector > kq; + + // pending CL kernels + vector > ckq; + + // loading and saving + void load(const char *filename); + void save(const char *filename); + private: + void clinit(); + json11::Json to_json(); }; diff --git a/selfdrive/modeld/transforms/loadyuv.c b/selfdrive/modeld/transforms/loadyuv.c deleted file mode 100644 index 25181451..00000000 --- a/selfdrive/modeld/transforms/loadyuv.c +++ /dev/null @@ -1,82 +0,0 @@ -#include -#include - -#include "clutil.h" - -#include "loadyuv.h" - -void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) { - int err = 0; - memset(s, 0, sizeof(*s)); - - s->width = width; - s->height = height; - - char args[1024]; - snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d", - width, height); - cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/loadyuv.cl", args); - - s->loadys_krnl = clCreateKernel(prg, "loadys", &err); - assert(err == 0); - s->loaduv_krnl = clCreateKernel(prg, "loaduv", &err); - assert(err == 0); - - // done with this - err = clReleaseProgram(prg); - assert(err == 0); -} - -void loadyuv_destroy(LoadYUVState* s) { - int err = 0; - - err = clReleaseKernel(s->loadys_krnl); - assert(err == 0); - err = clReleaseKernel(s->loaduv_krnl); - assert(err == 0); -} - -void loadyuv_queue(LoadYUVState* s, cl_command_queue q, - cl_mem y_cl, cl_mem u_cl, cl_mem v_cl, - cl_mem out_cl) { - int err = 0; - - err = clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl); - assert(err == 0); - err = clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl); - assert(err == 0); - - const size_t loadys_work_size = (s->width*s->height)/8; - err = clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL, - &loadys_work_size, NULL, 0, 0, NULL); - assert(err == 0); - - const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8; - cl_int loaduv_out_off = (s->width*s->height); - - err = clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl); - assert(err == 0); - err = clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl); - assert(err == 0); - err = clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off); - assert(err == 0); - - err = clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL, - &loaduv_work_size, NULL, 0, 0, NULL); - assert(err == 0); - - loaduv_out_off += (s->width/2)*(s->height/2); - - err = clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl); - assert(err == 0); - err = clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl); - assert(err == 0); - err = clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off); - assert(err == 0); - - err = clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL, - &loaduv_work_size, NULL, 0, 0, NULL); - assert(err == 0); -} diff --git a/selfdrive/modeld/transforms/loadyuv.cc b/selfdrive/modeld/transforms/loadyuv.cc new file mode 100644 index 00000000..fe84e6d2 --- /dev/null +++ b/selfdrive/modeld/transforms/loadyuv.cc @@ -0,0 +1,58 @@ +#include +#include +#include "loadyuv.h" + +void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) { + memset(s, 0, sizeof(*s)); + + s->width = width; + s->height = height; + + char args[1024]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d", + width, height); + cl_program prg = cl_program_from_file(ctx, device_id, "transforms/loadyuv.cl", args); + + s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err)); + s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err)); + + // done with this + CL_CHECK(clReleaseProgram(prg)); +} + +void loadyuv_destroy(LoadYUVState* s) { + CL_CHECK(clReleaseKernel(s->loadys_krnl)); + CL_CHECK(clReleaseKernel(s->loaduv_krnl)); +} + +void loadyuv_queue(LoadYUVState* s, cl_command_queue q, + cl_mem y_cl, cl_mem u_cl, cl_mem v_cl, + cl_mem out_cl) { + CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl)); + CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl)); + + const size_t loadys_work_size = (s->width*s->height)/8; + CL_CHECK(clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL, + &loadys_work_size, NULL, 0, 0, NULL)); + + const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8; + cl_int loaduv_out_off = (s->width*s->height); + + CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl)); + CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl)); + CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off)); + + CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL, + &loaduv_work_size, NULL, 0, 0, NULL)); + + loaduv_out_off += (s->width/2)*(s->height/2); + + CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl)); + CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl)); + CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off)); + + CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL, + &loaduv_work_size, NULL, 0, 0, NULL)); +} diff --git a/selfdrive/modeld/transforms/loadyuv.h b/selfdrive/modeld/transforms/loadyuv.h index be7ea212..dbef4614 100644 --- a/selfdrive/modeld/transforms/loadyuv.h +++ b/selfdrive/modeld/transforms/loadyuv.h @@ -1,18 +1,8 @@ -#ifndef LOADYUV_H -#define LOADYUV_H +#pragma once #include #include - -#ifdef __APPLE__ -#include -#else -#include -#endif - -#ifdef __cplusplus -extern "C" { -#endif +#include "clutil.h" typedef struct { int width, height; @@ -26,9 +16,3 @@ void loadyuv_destroy(LoadYUVState* s); void loadyuv_queue(LoadYUVState* s, cl_command_queue q, cl_mem y_cl, cl_mem u_cl, cl_mem v_cl, cl_mem out_cl); - -#ifdef __cplusplus -} -#endif - -#endif // LOADYUV_H diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc index 53e7fc48..3ea79949 100644 --- a/selfdrive/modeld/transforms/transform.cc +++ b/selfdrive/modeld/transforms/transform.cc @@ -6,35 +6,21 @@ #include "transform.h" void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) { - int err = 0; memset(s, 0, sizeof(*s)); - cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/transform.cl", ""); - - s->krnl = clCreateKernel(prg, "warpPerspective", &err); - assert(err == 0); - + cl_program prg = cl_program_from_file(ctx, device_id, "transforms/transform.cl", ""); + s->krnl = CL_CHECK_ERR(clCreateKernel(prg, "warpPerspective", &err)); // done with this - err = clReleaseProgram(prg); - assert(err == 0); + CL_CHECK(clReleaseProgram(prg)); - s->m_y_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err); - assert(err == 0); - - s->m_uv_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err); - assert(err == 0); + s->m_y_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err)); + s->m_uv_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err)); } void transform_destroy(Transform* s) { - int err = 0; - - err = clReleaseMemObject(s->m_y_cl); - assert(err == 0); - err = clReleaseMemObject(s->m_uv_cl); - assert(err == 0); - - err = clReleaseKernel(s->krnl); - assert(err == 0); + CL_CHECK(clReleaseMemObject(s->m_y_cl)); + CL_CHECK(clReleaseMemObject(s->m_uv_cl)); + CL_CHECK(clReleaseKernel(s->krnl)); } void transform_queue(Transform* s, @@ -43,7 +29,6 @@ void transform_queue(Transform* s, cl_mem out_y, cl_mem out_u, cl_mem out_v, int out_width, int out_height, mat3 projection) { - int err = 0; const int zero = 0; // sampled using pixel center origin @@ -54,10 +39,8 @@ void transform_queue(Transform* s, // in and out uv is half the size of y. mat3 projection_uv = transform_scale_buffer(projection, 0.5); - err = clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL); - assert(err == 0); - err = clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL); - assert(err == 0); + CL_CHECK(clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL)); + CL_CHECK(clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL)); const int in_y_width = in_width; const int in_y_height = in_height; @@ -72,78 +55,41 @@ void transform_queue(Transform* s, const int out_uv_width = out_width/2; const int out_uv_height = out_height/2; - err = clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv); - assert(err == 0); - - err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_y_width); - assert(err == 0); - err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_offset); - assert(err == 0); - err = clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_y_height); - assert(err == 0); - err = clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_width); - assert(err == 0); - - err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_y); - assert(err == 0); - - err = clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_y_width); - assert(err == 0); - err = clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero); - assert(err == 0); - err = clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_y_height); - assert(err == 0); - err = clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_width); - assert(err == 0); - - err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl); - assert(err == 0); + CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv)); + CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_y_width)); + CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_offset)); + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_y_height)); + CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_width)); + CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_y)); + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_y_width)); + CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero)); + CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_y_height)); + CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_width)); + CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl)); const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height}; - err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, - (const size_t*)&work_size_y, NULL, 0, 0, NULL); - assert(err == 0); - + CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, + (const size_t*)&work_size_y, NULL, 0, 0, NULL)); const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height}; - err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width); - assert(err == 0); - err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_u_offset); - assert(err == 0); - err = clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_uv_height); - assert(err == 0); - err = clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_width); - assert(err == 0); + CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width)); + CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_u_offset)); + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_uv_height)); + CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_width)); + CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_u)); + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_uv_width)); + CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero)); + CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_uv_height)); + CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_width)); + CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_uv_cl)); + + CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, + (const size_t*)&work_size_uv, NULL, 0, 0, NULL)); + CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_v_offset)); + CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_v)); - err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_u); - assert(err == 0); - - err = clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_uv_width); - assert(err == 0); - err = clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero); - assert(err == 0); - err = clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_uv_height); - assert(err == 0); - err = clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_width); - assert(err == 0); - - err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_uv_cl); - assert(err == 0); - - err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, - (const size_t*)&work_size_uv, NULL, 0, 0, NULL); - assert(err == 0); - - - err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_v_offset); - assert(err == 0); - err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_v); - assert(err == 0); - - - err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, - (const size_t*)&work_size_uv, NULL, 0, 0, NULL); - assert(err == 0); + CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, + (const size_t*)&work_size_uv, NULL, 0, 0, NULL)); } diff --git a/selfdrive/modeld/transforms/transform.h b/selfdrive/modeld/transforms/transform.h index 14f9d236..21933f4d 100644 --- a/selfdrive/modeld/transforms/transform.h +++ b/selfdrive/modeld/transforms/transform.h @@ -1,5 +1,4 @@ -#ifndef TRANSFORM_H -#define TRANSFORM_H +#pragma once #include #include @@ -13,10 +12,6 @@ #include "common/mat.h" -#ifdef __cplusplus -extern "C" { -#endif - typedef struct { cl_kernel krnl; cl_mem m_y_cl, m_uv_cl; @@ -31,9 +26,3 @@ void transform_queue(Transform* s, cl_command_queue q, cl_mem out_y, cl_mem out_u, cl_mem out_v, int out_width, int out_height, mat3 projection); - -#ifdef __cplusplus -} -#endif - -#endif // TRANSFORM_H diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 52444040..cb630eab 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -3,11 +3,14 @@ import os import time -from common.hardware import TICI -from common.gpio import GPIO_HUB_RST_N, GPIO_STM_BOOT0, GPIO_STM_RST_N, gpio_init, gpio_set from panda import BASEDIR, Panda, PandaDFU, build_st +from common.gpio import gpio_init, gpio_set +from selfdrive.hardware import TICI +from selfdrive.hardware.tici.pins import GPIO_HUB_RST_N, GPIO_STM_BOOT0, GPIO_STM_RST_N from selfdrive.swaglog import cloudlog +def is_legacy_panda_reset(): + return os.path.isfile("/persist/LEGACY_PANDA_RESET") def set_panda_power(power=True): if not TICI: @@ -16,12 +19,12 @@ def set_panda_power(power=True): gpio_init(GPIO_STM_RST_N, True) gpio_init(GPIO_STM_BOOT0, True) - gpio_set(GPIO_STM_RST_N, False) + gpio_set(GPIO_STM_RST_N, False if is_legacy_panda_reset() else True) gpio_set(GPIO_HUB_RST_N, True) time.sleep(0.1) - gpio_set(GPIO_STM_RST_N, power) + gpio_set(GPIO_STM_RST_N, power if is_legacy_panda_reset() else (not power)) def get_firmware_fn(): diff --git a/selfdrive/registration.py b/selfdrive/registration.py index de2231a4..07c1e374 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -2,13 +2,14 @@ import os import json from datetime import datetime, timedelta -from selfdrive.swaglog import cloudlog -from selfdrive.version import version, terms_version, training_version, get_git_commit, get_git_branch, get_git_remote -from common.hardware import HARDWARE from common.api import api_get from common.params import Params from common.file_helpers import mkdirs_exists_ok from common.basedir import PERSIST +from selfdrive.hardware import HARDWARE +from selfdrive.swaglog import cloudlog +from selfdrive.version import version, terms_version, training_version, get_git_commit, \ + get_git_branch, get_git_remote def register(): diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index 5eea0bcd..cb78ba0a 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -62,10 +62,8 @@ void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {x, y, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initAcceleration(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index 61e3d9e4..38a2ff42 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -72,10 +72,8 @@ void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {x, y, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initGyroUncalibrated(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index bc742e43..fbe43c20 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -96,10 +96,8 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {x, y, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initMagneticUncalibrated(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index a7253b0a..15a18672 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -53,10 +53,8 @@ void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {y, -x, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initAcceleration(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index 93cb24ea..4c983e54 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -56,10 +56,8 @@ void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {y, -x, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initGyroUncalibrated(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc index 58295679..16b7fc98 100644 --- a/selfdrive/sensord/sensors_qcom.cc +++ b/selfdrive/sensord/sensors_qcom.cc @@ -150,8 +150,7 @@ void sensor_loop() { switch (data.type) { case SENSOR_TYPE_ACCELEROMETER: { auto svec = log_event.initAcceleration(); - kj::ArrayPtr vs(&data.acceleration.v[0], 3); - svec.setV(vs); + svec.setV(data.acceleration.v); svec.setStatus(data.acceleration.status); break; } @@ -164,8 +163,7 @@ void sensor_loop() { } case SENSOR_TYPE_MAGNETIC_FIELD: { auto svec = log_event.initMagnetic(); - kj::ArrayPtr vs(&data.magnetic.v[0], 3); - svec.setV(vs); + svec.setV(data.magnetic.v); svec.setStatus(data.magnetic.status); break; } @@ -178,8 +176,7 @@ void sensor_loop() { } case SENSOR_TYPE_GYROSCOPE: { auto svec = log_event.initGyro(); - kj::ArrayPtr vs(&data.gyro.v[0], 3); - svec.setV(vs); + svec.setV(data.gyro.v); svec.setStatus(data.gyro.status); break; } diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index ff464557..de8b15b3 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -3,8 +3,8 @@ import subprocess from functools import wraps from nose.tools import nottest -from common.hardware import PC -from common.apk import update_apks, start_offroad, pm_apply_packages, android_packages +from selfdrive.hardware.eon.apk import update_apks, start_offroad, pm_apply_packages, android_packages +from selfdrive.hardware import PC from selfdrive.version import training_version, terms_version from selfdrive.manager import start_managed_process, kill_managed_process, get_running diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index f1801e72..d0140366 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -17,6 +17,9 @@ fi # clear scons cache dirs that haven't been written to in one day cd /tmp && find -name 'scons_cache_*' -type d -maxdepth 1 -mtime +1 -exec rm -rf '{}' \; +# this can get really big on the CI devices +rm -rf /data/core + # set up environment cd $SOURCE_DIR git reset --hard diff --git a/selfdrive/test/test_cpu_usage.py b/selfdrive/test/test_cpu_usage.py index a2c5a0a1..b6147d17 100755 --- a/selfdrive/test/test_cpu_usage.py +++ b/selfdrive/test/test_cpu_usage.py @@ -6,7 +6,6 @@ import subprocess import cereal.messaging as messaging from common.basedir import BASEDIR -from common.params import Params from selfdrive.test.helpers import set_params_enabled def cputime_total(ct): @@ -66,20 +65,23 @@ def test_cpu_usage(): cpu_ok = False # start manager + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" manager_path = os.path.join(BASEDIR, "selfdrive/manager.py") manager_proc = subprocess.Popen(["python", manager_path]) try: proc_sock = messaging.sub_sock('procLog', conflate=True, timeout=2000) + cs_sock = messaging.sub_sock('controlsState', conflate=True) # wait until everything's started start_time = time.monotonic() - while time.monotonic() - start_time < 210: - if Params().get("CarParams") is not None: + while time.monotonic() - start_time < 240: + msg = messaging.recv_sock(cs_sock, wait=True) + if msg is not None: break - time.sleep(2) # take first sample - time.sleep(5) + time.sleep(15) first_proc = messaging.recv_sock(proc_sock, wait=True) if first_proc is None: raise Exception("\n\nTEST FAILED: progLog recv timed out\n\n") @@ -97,7 +99,6 @@ def test_cpu_usage(): if __name__ == "__main__": set_params_enabled() - Params().delete("CarParams") passed = False try: diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index a6f2d4c4..5d59cf38 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -4,9 +4,9 @@ import time from statistics import mean from cereal import log -from common.realtime import sec_since_boot from common.params import Params, put_nonblocking -from common.hardware import TICI +from common.realtime import sec_since_boot +from selfdrive.hardware import HARDWARE from selfdrive.swaglog import cloudlog CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) @@ -19,48 +19,6 @@ CAR_CHARGING_RATE_W = 45 VBATT_PAUSE_CHARGING = 11.0 MAX_TIME_OFFROAD_S = 30*3600 -# Parameters -def get_battery_capacity(): - return _read_param("/sys/class/power_supply/battery/capacity", int) - - -def get_battery_status(): - # This does not correspond with actual charging or not. - # If a USB cable is plugged in, it responds with 'Charging', even when charging is disabled - return _read_param("/sys/class/power_supply/battery/status", lambda x: x.strip(), '') - - -def get_battery_current(): - return _read_param("/sys/class/power_supply/battery/current_now", int) - - -def get_battery_voltage(): - return _read_param("/sys/class/power_supply/battery/voltage_now", int) - - -def get_usb_present(): - return _read_param("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False) - - -def get_battery_charging(): - # This does correspond with actually charging - return _read_param("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", True) - - -def set_battery_charging(on): - with open('/sys/class/power_supply/battery/charging_enabled', 'w') as f: - f.write(f"{1 if on else 0}\n") - - -# Helpers -def _read_param(path, parser, default=0): - try: - with open(path) as f: - return parser(f.read()) - except Exception: - return default - - class PowerMonitoring: def __init__(self): self.params = Params() @@ -121,14 +79,13 @@ class PowerMonitoring: # No ignition, we integrate the offroad power used by the device is_uno = health.health.hwType == log.HealthData.HwType.uno # Get current power draw somehow - current_power = 0 - if TICI: - with open("/sys/class/hwmon/hwmon1/power1_input") as f: - current_power = int(f.read()) / 1e6 - elif get_battery_status() == 'Discharging': + current_power = HARDWARE.get_current_power_draw() + if current_power is not None: + pass + elif HARDWARE.get_battery_status() == 'Discharging': # If the battery is discharging, we can use this measurement # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in - current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000)) + current_power = ((HARDWARE.get_battery_voltage() / 1000000) * (HARDWARE.get_battery_current() / 1000000)) elif (self.next_pulsed_measurement_time is not None) and (self.next_pulsed_measurement_time <= now): # TODO: Figure out why this is off by a factor of 3/4??? FUDGE_FACTOR = 1.33 @@ -136,22 +93,22 @@ class PowerMonitoring: # Turn off charging for about 10 sec in a thread that does not get killed on SIGINT, and perform measurement here to avoid blocking thermal def perform_pulse_measurement(now): try: - set_battery_charging(False) + HARDWARE.set_battery_charging(False) time.sleep(5) # Measure for a few sec to get a good average voltages = [] currents = [] for _ in range(6): - voltages.append(get_battery_voltage()) - currents.append(get_battery_current()) + voltages.append(HARDWARE.get_battery_voltage()) + currents.append(HARDWARE.get_battery_current()) time.sleep(1) current_power = ((mean(voltages) / 1000000) * (mean(currents) / 1000000)) self._perform_integration(now, current_power * FUDGE_FACTOR) # Enable charging again - set_battery_charging(True) + HARDWARE.set_battery_charging(True) except Exception: cloudlog.exception("Pulsed power measurement failed") @@ -222,6 +179,6 @@ class PowerMonitoring: should_shutdown = False # Wait until we have shut down charging before powering down should_shutdown |= (not panda_charging and self.should_disable_charging(health, offroad_timestamp)) - should_shutdown |= ((get_battery_capacity() < BATT_PERC_OFF) and (not get_battery_charging()) and ((now - offroad_timestamp) > 60)) + should_shutdown |= ((HARDWARE.get_battery_capacity() < BATT_PERC_OFF) and (not HARDWARE.get_battery_charging()) and ((now - offroad_timestamp) > 60)) should_shutdown &= started_seen return should_shutdown diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index c36e9cdb..843f9fa1 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -11,20 +11,15 @@ from smbus2 import SMBus import cereal.messaging as messaging from cereal import log from common.filter_simple import FirstOrderFilter -from common.hardware import EON, HARDWARE, TICI from common.numpy_fast import clip, interp from common.params import Params from common.realtime import DT_TRML, sec_since_boot from selfdrive.controls.lib.alertmanager import set_offroad_alert +from selfdrive.hardware import EON, HARDWARE, TICI from selfdrive.loggerd.config import get_available_percent from selfdrive.pandad import get_expected_signature from selfdrive.swaglog import cloudlog -from selfdrive.thermald.power_monitoring import (PowerMonitoring, - get_battery_capacity, - get_battery_current, - get_battery_status, - get_battery_voltage, - get_usb_present) +from selfdrive.thermald.power_monitoring import PowerMonitoring from selfdrive.version import get_git_branch, terms_version, training_version ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient']) @@ -257,11 +252,11 @@ def thermald_thread(): msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) msg.thermal.networkType = network_type msg.thermal.networkStrength = network_strength - msg.thermal.batteryPercent = get_battery_capacity() - msg.thermal.batteryStatus = get_battery_status() - msg.thermal.batteryCurrent = get_battery_current() - msg.thermal.batteryVoltage = get_battery_voltage() - msg.thermal.usbOnline = get_usb_present() + msg.thermal.batteryPercent = HARDWARE.get_battery_capacity() + msg.thermal.batteryStatus = HARDWARE.get_battery_status() + msg.thermal.batteryCurrent = HARDWARE.get_battery_current() + msg.thermal.batteryVoltage = HARDWARE.get_battery_voltage() + msg.thermal.usbOnline = HARDWARE.get_usb_present() # Fake battery levels on uno for frame if (not EON) or is_uno: diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 5a208ffc..8ae0a147 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -5,7 +5,7 @@ import time from raven import Client from raven.transport.http import HTTPTransport -from selfdrive.version import version, dirty +from selfdrive.version import version, origin, branch, dirty from selfdrive.swaglog import cloudlog MAX_SIZE = 100000 * 10 # Normal size is 40-100k, allow up to 1M @@ -73,8 +73,14 @@ def report_tombstone(fn, client): def main(): initial_tombstones = set(get_tombstones()) + + tags = { + 'dirty': dirty, + 'origin': origin, + 'branch': branch + } client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', - install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}, string_max_length=10000) + install_sys_hook=False, transport=HTTPTransport, release=version, tags=tags, string_max_length=10000) client.user_context({'id': os.environ.get('DONGLE_ID')}) while True: diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 414f8628..ce39c4fc 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -31,7 +31,7 @@ if arch in ["x86_64", "Darwin", "larch64"]: ] qt_env.Tool('qt') - qt_env['CPPPATH'] += qt_dirs + qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"] qt_flags = [ "-D_REENTRANT", "-DQT_NO_DEBUG", @@ -40,7 +40,7 @@ if arch in ["x86_64", "Darwin", "larch64"]: "-DQT_CORE_LIB" ] qt_env['CXXFLAGS'] += qt_flags - + qt_env['LIBPATH'] += ['#selfdrive/ui'] src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] @@ -71,14 +71,31 @@ else: else: qt_libs += [f"Qt5{m}" for m in qt_modules] - qt_src = ["qt/ui.cc", "qt/window.cc", "qt/qt_sound.cc", "qt//offroad/keyboard.cc", "qt/offroad/input_field.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc", "qt/offroad/wifi.cc", "qt/offroad/wifiManager.cc"] + src + + qt_env.Library("qt_widgets", + ["qt/qt_window.cc", "qt/qt_sound.cc", "qt/widgets/keyboard.cc", "qt/widgets/input_field.cc", + "qt/offroad/wifi.cc", "qt/offroad/wifiManager.cc", "qt/widgets/toggle.cc"], + LIBS=qt_libs) + qt_libs.append("qt_widgets") + + qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc", "qt/widgets/offroad_alerts.cc"] + src qt_env.Program("_ui", qt_src, LIBS=qt_libs + libs) # spinner and text window qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs + libs) qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs + libs) - # setup and installer + # build setup, factory resetter, and installer if "BUILD_SETUP" in os.environ: - qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc"], LIBS=qt_libs + libs + ['curl']) - qt_env.Program("qt/setup/installer", ["qt/setup/installer.cc"], LIBS=qt_libs + libs) + qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs) + qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc"], LIBS=qt_libs + ['curl', 'common']) + + installers = [ + ("openpilot", "master"), + ("openpilot_test", "master"), + #("dashcam", "dashcam"), + #("dashcam_test", "dashcam"), + ] + for name, branch in installers: + flags = qt_env["CXXFLAGS"] + [f"-D{branch}"] + qt_env.Program(f"qt/setup/installer_{name}", ["qt/setup/installer.cc"], LIBS=qt_libs, CXXFLAGS=flags) diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc index 08b734a6..7017bd28 100644 --- a/selfdrive/ui/android/ui.cc +++ b/selfdrive/ui/android/ui.cc @@ -134,7 +134,7 @@ int main(int argc, char* argv[]) { float brightness_b = 0, brightness_m = 0; int result = read_param(&brightness_b, "BRIGHTNESS_B", true); result += read_param(&brightness_m, "BRIGHTNESS_M", true); - if(result != 0) { + if (result != 0) { brightness_b = LEON ? 10.0 : 5.0; brightness_m = LEON ? 2.6 : 1.3; write_param_float(brightness_b, "BRIGHTNESS_B", true); diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 72dae813..5e7008b8 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -22,13 +22,15 @@ extern "C"{ // TODO: this is also hardcoded in common/transformations/camera.py // TODO: choose based on frame input size #ifdef QCOM2 -const float zoom = 1.5; +const float y_offset = 150.0; +const float zoom = 1.1; const mat3 intrinsic_matrix = (mat3){{ 2648.0, 0.0, 1928.0/2, 0.0, 2648.0, 1208.0/2, 0.0, 0.0, 1.0 }}; #else +const float y_offset = 0.0; const float zoom = 2.35; const mat3 intrinsic_matrix = (mat3){{ 910., 0., 1164.0/2, @@ -39,7 +41,7 @@ const mat3 intrinsic_matrix = (mat3){{ // Projects a point in car to space to the corresponding point in full frame // image space. -bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y) { +bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin=0.0) { const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}}; // We'll call the car space point p. // First project into normalized image coordinates with the extrinsics matrix. @@ -53,7 +55,7 @@ bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_ *out_x = KEp.v[0] / KEp.v[2]; *out_y = KEp.v[1] / KEp.v[2]; - return *out_x >= 0 && *out_x <= s->fb_w && *out_y >= 0 && *out_y <= s->fb_h; + return *out_x >= -margin && *out_x <= s->fb_w + margin && *out_y >= -margin && *out_y <= s->fb_h + margin; } @@ -151,7 +153,7 @@ static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::R const float off = 0.5; int max_idx = 0; float lead_d; - if(s->sm->updated("radarState")) { + if (s->sm->updated("radarState")) { lead_d = scene->lead_data[0].getDRel()*2.; } else { lead_d = MAX_DRAW_DISTANCE; @@ -161,12 +163,13 @@ static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::R vertex_data *v = &pvd->v[0]; + const float margin = 500.0f; for (int i = 0; line.getX()[i] <= path_length and i < TRAJECTORY_SIZE; i++) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y); + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y, margin); max_idx = i; } for (int i = max_idx; i >= 0; i--) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y); + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y, margin); } pvd->cnt = v - pvd->v; } @@ -212,12 +215,13 @@ static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Re // TODO check that this doesn't overflow max vertex buffer int max_idx; vertex_data *v = &pvd->v[0]; + const float margin = 500.0f; for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y); + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin); max_idx = i; } for (int i = max_idx - 1; i > 0; i--) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y); + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin); } pvd->cnt = v - pvd->v; } @@ -225,28 +229,29 @@ static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Re static void ui_draw_vision_lane_lines(UIState *s) { const UIScene *scene = &s->scene; + // paint lanelines line_vertices_data *pvd_ll = &s->lane_line_vertices[0]; for (int ll_idx = 0; ll_idx < 4; ll_idx++) { - if(s->sm->updated("modelV2")) { + if (s->sm->updated("modelV2")) { update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance); } NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]); ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr); } - + // paint road edges line_vertices_data *pvd_re = &s->road_edge_vertices[0]; for (int re_idx = 0; re_idx < 2; re_idx++) { - if(s->sm->updated("modelV2")) { + if (s->sm->updated("modelV2")) { update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance); } NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0)); ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr); } - + // paint path - if(s->sm->updated("modelV2")) { + if (s->sm->updated("modelV2")) { update_track_data(s, scene->model.getPosition(), &s->track_vertices); } ui_draw_track(s, &s->track_vertices); @@ -263,7 +268,7 @@ static void ui_draw_world(UIState *s) { // Apply transformation such that video pixel coordinates match video // 1) Put (0, 0) in the middle of the video - nvgTranslate(s->vg, s->video_rect.x + s->video_rect.w / 2, s->video_rect.y + s->video_rect.h / 2); + nvgTranslate(s->vg, s->video_rect.x + s->video_rect.w / 2, s->video_rect.y + s->video_rect.h / 2 + y_offset); // 2) Apply same scaling as video nvgScale(s->vg, zoom, zoom); @@ -440,24 +445,22 @@ static void ui_draw_vision_footer(UIState *s) { ui_draw_vision_face(s); } -void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, UIStatus va_color, - const char* va_text1, const char* va_text2) { +static void ui_draw_vision_alert(UIState *s) { static std::map alert_size_map = { {cereal::ControlsState::AlertSize::NONE, 0}, {cereal::ControlsState::AlertSize::SMALL, 241}, {cereal::ControlsState::AlertSize::MID, 390}, {cereal::ControlsState::AlertSize::FULL, s->fb_h}}; - const UIScene *scene = &s->scene; - bool longAlert1 = strlen(va_text1) > 15; + bool longAlert1 = scene->alert_text1.length() > 15; - NVGcolor color = bg_colors[va_color]; + NVGcolor color = bg_colors[s->status]; color.a *= s->alert_blinking_alpha; - int alr_s = alert_size_map[va_size]; + int alr_s = alert_size_map[scene->alert_size]; const int alr_x = scene->viz_rect.x - bdr_s; const int alr_w = scene->viz_rect.w + (bdr_s*2); - const int alr_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s); + const int alr_h = alr_s+(scene->alert_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s); const int alr_y = s->fb_h-alr_h; ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, color); @@ -469,24 +472,24 @@ void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, nvgFillColor(s->vg, COLOR_WHITE); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - if (va_size == cereal::ControlsState::AlertSize::SMALL) { - ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, va_text1, 40*2.5, COLOR_WHITE, s->font_sans_semibold); - } else if (va_size == cereal::ControlsState::AlertSize::MID) { - ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, va_text1, 48*2.5, COLOR_WHITE, s->font_sans_bold); - ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, va_text2, 36*2.5, COLOR_WHITE, s->font_sans_regular); - } else if (va_size == cereal::ControlsState::AlertSize::FULL) { + if (scene->alert_size == cereal::ControlsState::AlertSize::SMALL) { + ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, scene->alert_text1.c_str(), 40*2.5, COLOR_WHITE, s->font_sans_semibold); + } else if (scene->alert_size == cereal::ControlsState::AlertSize::MID) { + ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, scene->alert_text1.c_str(), 48*2.5, COLOR_WHITE, s->font_sans_bold); + ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, scene->alert_text2.c_str(), 36*2.5, COLOR_WHITE, s->font_sans_regular); + } else if (scene->alert_size == cereal::ControlsState::AlertSize::FULL) { nvgFontSize(s->vg, (longAlert1?72:96)*2.5); nvgFontFaceId(s->vg, s->font_sans_bold); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, va_text1, NULL); + nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, scene->alert_text1.c_str(), NULL); nvgFontSize(s->vg, 48*2.5); nvgFontFaceId(s->vg, s->font_sans_regular); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); - nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, va_text2, NULL); + nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, scene->alert_text2.c_str(), NULL); } } -static void ui_draw_vision(UIState *s) { +static void ui_draw_vision_frame(UIState *s) { const UIScene *scene = &s->scene; const Rect &viz_rect = scene->viz_rect; @@ -498,24 +501,23 @@ static void ui_draw_vision(UIState *s) { glDisable(GL_SCISSOR_TEST); glViewport(0, 0, s->fb_w, s->fb_h); +} - // Draw augmented elements - if (!scene->frontview && scene->world_objects_visible) { - ui_draw_world(s); - } - // Set Speed, Current Speed, Status/Events +static void ui_draw_vision(UIState *s) { + const UIScene *scene = &s->scene; if (!scene->frontview) { + // Draw augmented elements + if (scene->world_objects_visible) { + ui_draw_world(s); + } + // Set Speed, Current Speed, Status/Events ui_draw_vision_header(s); + if (scene->alert_size == cereal::ControlsState::AlertSize::NONE) { + ui_draw_vision_footer(s); + } } else { ui_draw_driver_view(s); } - - if (scene->alert_size != cereal::ControlsState::AlertSize::NONE) { - ui_draw_vision_alert(s, scene->alert_size, s->status, - scene->alert_text1.c_str(), scene->alert_text2.c_str()); - } else if (!scene->frontview) { - ui_draw_vision_footer(s); - } } static void ui_draw_background(UIState *s) { @@ -525,24 +527,34 @@ static void ui_draw_background(UIState *s) { } void ui_draw(UIState *s) { - s->scene.viz_rect = Rect{bdr_s * 3, bdr_s, s->fb_w - 4 * bdr_s, s->fb_h - 2 * bdr_s}; - s->scene.ui_viz_ro = 0; + s->scene.viz_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s}; if (!s->scene.uilayout_sidebarcollapsed) { - s->scene.viz_rect.x = sbr_w + bdr_s; - s->scene.viz_rect.w = s->fb_w - s->scene.viz_rect.x - bdr_s; - s->scene.ui_viz_ro = -(sbr_w - 6 * bdr_s); + s->scene.viz_rect.x += sbr_w; + s->scene.viz_rect.w -= sbr_w; } + const bool draw_vision = s->started && s->status != STATUS_OFFROAD && + s->active_app == cereal::UiLayoutState::App::NONE; + + // GL drawing functions ui_draw_background(s); + if (draw_vision && s->vision_connected) { + ui_draw_vision_frame(s); + } glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glViewport(0, 0, s->fb_w, s->fb_h); + + // NVG drawing functions - should be no GL inside NVG frame nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); ui_draw_sidebar(s); - if (s->started && s->active_app == cereal::UiLayoutState::App::NONE && - s->status != STATUS_OFFROAD && s->vision_connected) { + if (draw_vision && s->vision_connected) { ui_draw_vision(s); } + + if (draw_vision && s->scene.alert_size != cereal::ControlsState::AlertSize::NONE) { + ui_draw_vision_alert(s); + } nvgEndFrame(s->vg); glDisable(GL_BLEND); } @@ -715,13 +727,13 @@ void ui_nvg_init(UIState *s) { glBindVertexArray(0); } - s->video_rect = Rect{bdr_s * 3, bdr_s, s->fb_w - 4 * bdr_s, s->fb_h - 2 * bdr_s}; + s->video_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s}; float zx = zoom * 2 * intrinsic_matrix.v[2] / s->video_rect.w; float zy = zoom * 2 * intrinsic_matrix.v[5] / s->video_rect.h; const mat4 frame_transform = {{ zx, 0.0, 0.0, 0.0, - 0.0, zy, 0.0, 0.0, + 0.0, zy, 0.0, -y_offset / s->video_rect.h * 2, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; diff --git a/selfdrive/ui/paint.hpp b/selfdrive/ui/paint.hpp index e71a2a43..203beef1 100644 --- a/selfdrive/ui/paint.hpp +++ b/selfdrive/ui/paint.hpp @@ -1,9 +1,6 @@ #pragma once #include "ui.hpp" - -void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, int va_color, - const char* va_text1, const char* va_text2); void ui_draw(UIState *s); void ui_draw_image(NVGcontext *vg, float x, float y, float w, float h, int image, float alpha); void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r = 0, int width = 0); diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc new file mode 100644 index 00000000..bc402eec --- /dev/null +++ b/selfdrive/ui/qt/home.cc @@ -0,0 +1,288 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "common/params.h" + +#include "home.hpp" +#include "paint.hpp" +#include "qt_window.hpp" + +#define BACKLIGHT_DT 0.25 +#define BACKLIGHT_TS 2.00 + + +OffroadHome::OffroadHome(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(); + main_layout->setContentsMargins(sbr_w + 50, 50, 50, 50); + + center_layout = new QStackedLayout(); + + // header + QHBoxLayout *header_layout = new QHBoxLayout(); + + date = new QLabel(); + date->setStyleSheet(R"(font-size: 55px;)"); + header_layout->addWidget(date, 0, Qt::AlignTop | Qt::AlignLeft); + + QLabel *version = new QLabel(QString::fromStdString("openpilot v" + Params().get("Version"))); + version->setStyleSheet(R"(font-size: 45px;)"); + header_layout->addWidget(version, 0, Qt::AlignTop | Qt::AlignRight); + + main_layout->addLayout(header_layout); + + alert_notification = new QPushButton(); + QObject::connect(alert_notification, SIGNAL(released()), this, SLOT(openAlerts())); + main_layout->addWidget(alert_notification, 0, Qt::AlignTop | Qt::AlignRight); + + // center + QLabel *drive = new QLabel("Drive me"); + drive->setStyleSheet(R"(font-size: 175px;)"); + center_layout->addWidget(drive); + + alerts_widget = new OffroadAlert(); + QObject::connect(alerts_widget, SIGNAL(closeAlerts()), this, SLOT(closeAlerts())); + center_layout->addWidget(alerts_widget); + center_layout->setAlignment(alerts_widget, Qt::AlignCenter); + + main_layout->addLayout(center_layout, 1); + + // set up refresh timer + timer = new QTimer(this); + QObject::connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); + refresh(); + timer->start(10 * 1000); + + setLayout(main_layout); + setStyleSheet(R"(background-color: none;)"); +} + +void OffroadHome::openAlerts() { + center_layout->setCurrentIndex(1); +} + +void OffroadHome::closeAlerts() { + center_layout->setCurrentIndex(0); +} + +void OffroadHome::refresh() { + bool first_refresh = !date->text().size(); + if (!isVisible() && !first_refresh) { + return; + } + + date->setText(QDateTime::currentDateTime().toString("dddd, MMMM d")); + + // update alerts + + alerts_widget->refresh(); + if (!alerts_widget->alerts.size() && !alerts_widget->updateAvailable) { + alert_notification->setVisible(false); + return; + } + + if (alerts_widget->updateAvailable) { + // There is a new release + alert_notification->setText("UPDATE"); + } else { + int alerts = alerts_widget->alerts.size(); + alert_notification->setText(QString::number(alerts) + " ALERT" + (alerts == 1 ? "" : "S")); + } + + alert_notification->setVisible(true); + + // Red background for alerts, blue for update available + QString style = QString(R"( + padding: 15px; + padding-left: 30px; + padding-right: 30px; + border: 1px solid; + border-radius: 5px; + font-size: 40px; + font-weight: bold; + background-color: red; + )"); + if (alerts_widget->updateAvailable){ + style.replace("red", "blue"); + } + alert_notification->setStyleSheet(style); +} + + +HomeWindow::HomeWindow(QWidget *parent) : QWidget(parent) { + layout = new QGridLayout; + layout->setMargin(0); + + // onroad UI + glWindow = new GLWindow(this); + layout->addWidget(glWindow, 0, 0); + + // draw offroad UI on top of onroad UI + home = new OffroadHome(); + layout->addWidget(home, 0, 0); + QObject::connect(glWindow, SIGNAL(offroadTransition(bool)), this, SLOT(setVisibility(bool))); + QObject::connect(this, SIGNAL(openSettings()), home, SLOT(refresh())); + setLayout(layout); + setStyleSheet(R"( + * { + color: white; + } + )"); +} + +void HomeWindow::setVisibility(bool offroad) { + home->setVisible(offroad); +} + +void HomeWindow::mousePressEvent(QMouseEvent *e) { + UIState *ui_state = glWindow->ui_state; + + glWindow->wake(); + + // Settings button click + if (!ui_state->scene.uilayout_sidebarcollapsed && settings_btn.ptInRect(e->x(), e->y())) { + emit openSettings(); + } + + // Vision click + if (ui_state->started && (e->x() >= ui_state->scene.viz_rect.x - bdr_s)) { + ui_state->scene.uilayout_sidebarcollapsed = !ui_state->scene.uilayout_sidebarcollapsed; + } +} + + +static void handle_display_state(UIState *s, int dt, bool user_input) { + static int awake_timeout = 0; + awake_timeout = std::max(awake_timeout-dt, 0); + + if (user_input || s->ignition || s->started) { + s->awake = true; + awake_timeout = 30*UI_FREQ; + } else if (awake_timeout == 0) { + s->awake = false; + } +} + +static void set_backlight(int brightness) { + std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness"); + if (brightness_control.is_open()) { + brightness_control << brightness << "\n"; + brightness_control.close(); + } +} + + +GLWindow::GLWindow(QWidget *parent) : QOpenGLWidget(parent) { + timer = new QTimer(this); + QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); + + backlight_timer = new QTimer(this); + QObject::connect(backlight_timer, SIGNAL(timeout()), this, SLOT(backlightUpdate())); + + int result = read_param(&brightness_b, "BRIGHTNESS_B", true); + result += read_param(&brightness_m, "BRIGHTNESS_M", true); + if (result != 0) { + brightness_b = 200.0; + brightness_m = 10.0; + } + smooth_brightness = 512; +} + +GLWindow::~GLWindow() { + makeCurrent(); + doneCurrent(); +} + +void GLWindow::initializeGL() { + initializeOpenGLFunctions(); + std::cout << "OpenGL version: " << glGetString(GL_VERSION) << std::endl; + std::cout << "OpenGL vendor: " << glGetString(GL_VENDOR) << std::endl; + std::cout << "OpenGL renderer: " << glGetString(GL_RENDERER) << std::endl; + std::cout << "OpenGL language version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl; + + ui_state = new UIState(); + ui_state->sound = &sound; + ui_state->fb_w = vwp_w; + ui_state->fb_h = vwp_h; + ui_init(ui_state); + + wake(); + + timer->start(0); + backlight_timer->start(BACKLIGHT_DT * 100); +} + +void GLWindow::backlightUpdate() { + // Update brightness + float k = (BACKLIGHT_DT / BACKLIGHT_TS) / (1.0f + BACKLIGHT_DT / BACKLIGHT_TS); + + float clipped_brightness = std::min(1023.0f, (ui_state->light_sensor*brightness_m) + brightness_b); + smooth_brightness = clipped_brightness * k + smooth_brightness * (1.0f - k); + int brightness = smooth_brightness; + + if (!ui_state->awake) { + brightness = 0; + } + + std::thread{set_backlight, brightness}.detach(); +} + +void GLWindow::timerUpdate() { + if (ui_state->started != onroad) { + onroad = ui_state->started; + emit offroadTransition(!onroad); +#ifdef QCOM2 + timer->setInterval(onroad ? 0 : 1000); +#endif + } + + // Fix awake timeout if running 1 Hz when offroad + int dt = timer->interval() == 0 ? 1 : 20; + handle_display_state(ui_state, dt, false); + + ui_update(ui_state); + repaint(); +} + +void GLWindow::resizeGL(int w, int h) { + std::cout << "resize " << w << "x" << h << std::endl; +} + +void GLWindow::paintGL() { + ui_draw(ui_state); +} + +void GLWindow::wake() { + // UI state might not be initialized yet + if (ui_state != nullptr) { + handle_display_state(ui_state, 1, true); + } +} + +GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) { + unsigned int texture; + glGenTextures(1, &texture); + glBindTexture(GL_TEXTURE_2D, texture); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, img->width, img->height, 0, GL_RGB, GL_UNSIGNED_BYTE, *pph); + glGenerateMipmap(GL_TEXTURE_2D); + *pkhr = (EGLImageKHR)1; // not NULL + return texture; +} + +void visionimg_destroy_gl(EGLImageKHR khr, void *ph) { + // empty +} + +FramebufferState* framebuffer_init(const char* name, int32_t layer, int alpha, + int *out_w, int *out_h) { + return (FramebufferState*)1; // not null +} diff --git a/selfdrive/ui/qt/home.hpp b/selfdrive/ui/qt/home.hpp new file mode 100644 index 00000000..5f71e0e4 --- /dev/null +++ b/selfdrive/ui/qt/home.hpp @@ -0,0 +1,99 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "qt_sound.hpp" +#include "widgets/offroad_alerts.hpp" +#include "ui/ui.hpp" + + +// container window for onroad NVG UI +class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions { + Q_OBJECT + +public: + using QOpenGLWidget::QOpenGLWidget; + explicit GLWindow(QWidget *parent = 0); + void wake(); + ~GLWindow(); + + UIState *ui_state = nullptr; + +signals: + void offroadTransition(bool offroad); + +protected: + void initializeGL() override; + void resizeGL(int w, int h) override; + void paintGL() override; + +private: + QTimer *timer; + QTimer *backlight_timer; + + QtSound sound; + + bool onroad = true; + + // TODO: this shouldn't be here + float brightness_b = 0; + float brightness_m = 0; + float smooth_brightness = 0; + +public slots: + void timerUpdate(); + void backlightUpdate(); +}; + +// offroad home screen +class OffroadHome : public QWidget { + Q_OBJECT + +public: + explicit OffroadHome(QWidget *parent = 0); + +private: + QTimer *timer; + + // offroad home screen widgets + QLabel *date; + QStackedLayout *center_layout; + OffroadAlert *alerts_widget; + QPushButton *alert_notification; + +public slots: + void closeAlerts(); + void openAlerts(); + void refresh(); +}; + + +class HomeWindow : public QWidget { + Q_OBJECT + +public: + explicit HomeWindow(QWidget *parent = 0); + GLWindow *glWindow; + +signals: + void openSettings(); + +protected: + void mousePressEvent(QMouseEvent *e) override; + +private: + QGridLayout *layout; + OffroadHome *home; + +private slots: + void setVisibility(bool offroad); +}; + diff --git a/selfdrive/ui/qt/offroad/input_field.cc b/selfdrive/ui/qt/offroad/input_field.cc deleted file mode 100644 index 4696416e..00000000 --- a/selfdrive/ui/qt/offroad/input_field.cc +++ /dev/null @@ -1,56 +0,0 @@ -#include -#include -#include -#include -#include - -#include "input_field.hpp" -#include "keyboard.hpp" - -InputField::InputField(QWidget *parent): QWidget(parent) { - l = new QVBoxLayout(); - QHBoxLayout *r = new QHBoxLayout(); - label = new QLabel(this); - label->setText("password"); - r->addWidget(label); - QPushButton* cancel = new QPushButton("cancel"); - QObject::connect(cancel, SIGNAL(released()), this, SLOT(emitEmpty())); - cancel->setFixedHeight(150); - cancel->setFixedWidth(300); - r->addWidget(cancel); - l->addLayout(r); - l->addSpacing(80); - - line = new QLineEdit(""); - l->addWidget(line); - l->addSpacing(200); - - k = new Keyboard(this); - QObject::connect(k, SIGNAL(emitButton(QString)), this, SLOT(getText(QString))); - l->addWidget(k); - setLayout(l); -} - -void InputField::emitEmpty(){ - emitText(""); - line->setText(""); -} -void InputField::getText(QString s){ - if(!QString::compare(s,"⌫")){ - line->backspace(); - } - - if(!QString::compare(s,"⏎")){ - emitText(line->text()); - line->setText(""); - } - - QVector control_buttons {"⇧", "↑", "ABC", "⏎", "#+=", "⌫", "123"}; - for(QString c :control_buttons){ - if(!QString::compare(s, c)){ - return; - } - } - line->insert(s.left(1)); -} - diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc index 5e97b15e..697dea42 100644 --- a/selfdrive/ui/qt/offroad/onboarding.cc +++ b/selfdrive/ui/qt/offroad/onboarding.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include "onboarding.hpp" #include "common/params.h" @@ -10,7 +11,6 @@ QLabel * title_label(QString text) { QLabel *l = new QLabel(text); l->setStyleSheet(R"(font-size: 100px;)"); - l->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum); return l; } @@ -30,8 +30,7 @@ QWidget * OnboardingWindow::terms_screen() { background-color: #292929; )"); main_layout->addWidget(terms, 1, 0, 1, -1); - - main_layout->addWidget(new QPushButton("Decline"), 2, 0); + main_layout->setRowStretch(1, 1); QPushButton *accept_btn = new QPushButton("Accept"); main_layout->addWidget(accept_btn, 2, 1); @@ -40,6 +39,8 @@ QWidget * OnboardingWindow::terms_screen() { updateActiveScreen(); }); + main_layout->addWidget(new QPushButton("Decline"), 2, 0); + QWidget *widget = new QWidget; widget->setLayout(main_layout); widget->setStyleSheet(R"( @@ -56,14 +57,16 @@ QWidget * OnboardingWindow::terms_screen() { QWidget * OnboardingWindow::training_screen() { - QGridLayout *main_layout = new QGridLayout(); + QVBoxLayout *main_layout = new QVBoxLayout(); main_layout->setMargin(30); main_layout->setSpacing(30); - main_layout->addWidget(title_label("Training Guide"), 0, 0); + main_layout->addWidget(title_label("Training Guide")); + + main_layout->addWidget(new QLabel(), 1); // just a spacer QPushButton *btn = new QPushButton("Continue"); - main_layout->addWidget(btn, 1, 0); + main_layout->addWidget(btn); QObject::connect(btn, &QPushButton::released, [=]() { Params().write_db_value("CompletedTrainingVersion", LATEST_TRAINING_VERSION); updateActiveScreen(); @@ -81,24 +84,18 @@ void OnboardingWindow::updateActiveScreen() { bool training_done = params.get("CompletedTrainingVersion", false).compare(LATEST_TRAINING_VERSION) == 0; if (!accepted_terms) { - swidget->setCurrentIndex(0); + setCurrentIndex(0); } else if (!training_done) { - swidget->setCurrentIndex(1); + setCurrentIndex(1); } else { emit onboardingDone(); } } -OnboardingWindow::OnboardingWindow(QWidget *parent) : QWidget(parent) { - QVBoxLayout * top_layout = new QVBoxLayout; +OnboardingWindow::OnboardingWindow(QWidget *parent) { + addWidget(terms_screen()); + addWidget(training_screen()); - swidget = new QStackedWidget(); - swidget->addWidget(terms_screen()); - swidget->addWidget(training_screen()); - - top_layout->addWidget(swidget); - - setLayout(top_layout); setStyleSheet(R"( * { background-color: black; diff --git a/selfdrive/ui/qt/offroad/onboarding.hpp b/selfdrive/ui/qt/offroad/onboarding.hpp index 137ace82..9e4aabeb 100644 --- a/selfdrive/ui/qt/offroad/onboarding.hpp +++ b/selfdrive/ui/qt/offroad/onboarding.hpp @@ -7,7 +7,7 @@ #define LATEST_TERMS_VERSION "2" #define LATEST_TRAINING_VERSION "0.2.0" -class OnboardingWindow : public QWidget { +class OnboardingWindow : public QStackedWidget { Q_OBJECT public: @@ -16,7 +16,6 @@ public: private: QWidget * terms_screen(); QWidget * training_screen(); - QStackedWidget *swidget; signals: void onboardingDone(); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index eb242408..4e650a0c 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -12,7 +12,8 @@ #include "wifi.hpp" #include "settings.hpp" -#include "input_field.hpp" +#include "widgets/toggle.hpp" +#include "widgets/offroad_alerts.hpp" #include "common/params.h" #include "common/utilpp.h" @@ -20,59 +21,52 @@ ParamsToggle::ParamsToggle(QString param, QString title, QString description, QString icon_path, QWidget *parent): QFrame(parent) , param(param) { QHBoxLayout *hlayout = new QHBoxLayout; - QVBoxLayout *vlayout = new QVBoxLayout; + // Parameter image hlayout->addSpacing(25); - if (icon_path.length()){ + if (icon_path.length()) { QPixmap pix(icon_path); QLabel *icon = new QLabel(); icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); hlayout->addWidget(icon); - } else{ + } else { hlayout->addSpacing(100); } hlayout->addSpacing(25); - checkbox = new QCheckBox(title); - QLabel *label = new QLabel(description); + // Name of the parameter + QLabel *label = new QLabel(title); label->setWordWrap(true); + // toggle switch + Toggle* toggle_switch = new Toggle(this); + toggle_switch->setFixedSize(150, 100); + // TODO: show descriptions on tap - //vlayout->addSpacing(50); - vlayout->addWidget(checkbox); - //vlayout->addWidget(label); - //vlayout->addSpacing(50); - hlayout->addLayout(vlayout); + hlayout->addWidget(label); + hlayout->addSpacing(50); + hlayout->addWidget(toggle_switch); + hlayout->addSpacing(20); setLayout(hlayout); - - checkbox->setChecked(Params().read_db_bool(param.toStdString().c_str())); + if (Params().read_db_bool(param.toStdString().c_str())) { + toggle_switch->togglePosition(); + } setStyleSheet(R"( - QCheckBox { - font-size: 70px; + QLabel { + font-size: 50px; } - QCheckBox::indicator { - width: 100px; - height: 100px; - } - QCheckBox::indicator:unchecked { - image: url(../assets/offroad/circled-checkmark-empty.png); - } - QCheckBox::indicator:checked { - image: url(../assets/offroad/circled-checkmark.png); - } - QLabel { font-size: 40px } * { background-color: #114265; } )"); - QObject::connect(checkbox, SIGNAL(stateChanged(int)), this, SLOT(checkboxClicked(int))); + QObject::connect(toggle_switch, SIGNAL(stateChanged(int)), this, SLOT(checkboxClicked(int))); } -void ParamsToggle::checkboxClicked(int state){ +void ParamsToggle::checkboxClicked(int state) { char value = state ? '1': '0'; Params().write_db_value(param.toStdString().c_str(), &value, 1); } @@ -176,15 +170,18 @@ QWidget * developer_panel() { Params params = Params(); std::string brand = params.read_db_bool("Passive") ? "dashcam" : "openpilot"; - std::string os_version = util::read_file("/VERSION"); std::vector> labels = { {"Version", brand + " v" + params.get("Version", false)}, - {"OS Version", os_version}, {"Git Branch", params.get("GitBranch", false)}, {"Git Commit", params.get("GitCommit", false).substr(0, 10)}, {"Panda Firmware", params.get("PandaFirmwareHex", false)}, }; + std::string os_version = util::read_file("/VERSION"); + if (os_version.size()) { + labels.push_back({"OS Version", "AGNOS " + os_version}); + } + for (auto l : labels) { QString text = QString::fromStdString(l.first + ": " + l.second); main_layout->addWidget(new QLabel(text)); @@ -192,17 +189,19 @@ QWidget * developer_panel() { QWidget *widget = new QWidget; widget->setLayout(main_layout); + widget->setStyleSheet(R"( + QLabel { + font-size: 50px; + } + )"); return widget; } -QWidget * network_panel() { - QVBoxLayout *main_layout = new QVBoxLayout; - - main_layout->addWidget(new WifiUI()); - - QWidget *widget = new QWidget; - widget->setLayout(main_layout); - return widget; +QWidget * network_panel(QWidget * parent) { + WifiUI *w = new WifiUI(); + QObject::connect(w, SIGNAL(openKeyboard()), parent, SLOT(closeSidebar())); + QObject::connect(w, SIGNAL(closeKeyboard()), parent, SLOT(openSidebar())); + return w; } @@ -212,7 +211,6 @@ void SettingsWindow::setActivePanel() { } SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { - // sidebar QVBoxLayout *sidebar_layout = new QVBoxLayout(); panel_layout = new QStackedLayout(); @@ -231,10 +229,10 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { // setup panels panels = { - {"device", device_panel()}, - {"toggles", toggles_panel()}, {"developer", developer_panel()}, - {"network", network_panel()}, + {"device", device_panel()}, + {"network", network_panel(this)}, + {"toggles", toggles_panel()}, }; for (auto &panel : panels) { @@ -258,12 +256,16 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { QHBoxLayout *settings_layout = new QHBoxLayout(); settings_layout->addSpacing(45); - settings_layout->addLayout(sidebar_layout); + + sidebar_widget = new QWidget; + sidebar_widget->setLayout(sidebar_layout); + settings_layout->addWidget(sidebar_widget); + settings_layout->addSpacing(45); settings_layout->addLayout(panel_layout); settings_layout->addSpacing(45); - setLayout(settings_layout); + setLayout(settings_layout); setStyleSheet(R"( * { color: white; @@ -271,3 +273,11 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { } )"); } + +void SettingsWindow::closeSidebar() { + sidebar_widget->setVisible(false); +} + +void SettingsWindow::openSidebar() { + sidebar_widget->setVisible(true); +} diff --git a/selfdrive/ui/qt/offroad/settings.hpp b/selfdrive/ui/qt/offroad/settings.hpp index e4f65a69..b2c0a911 100644 --- a/selfdrive/ui/qt/offroad/settings.hpp +++ b/selfdrive/ui/qt/offroad/settings.hpp @@ -3,24 +3,30 @@ #include #include #include -#include +#include #include +#include "wifi.hpp" + +// *** settings widgets *** class ParamsToggle : public QFrame { Q_OBJECT public: - explicit ParamsToggle(QString param, QString title, QString description, QString icon, QWidget *parent = 0); + explicit ParamsToggle(QString param, QString title, QString description, + QString icon, QWidget *parent = 0); private: - QCheckBox *checkbox; QString param; public slots: void checkboxClicked(int state); }; + +// *** settings window *** + class SettingsWindow : public QWidget { Q_OBJECT @@ -31,9 +37,13 @@ signals: void closeSettings(); private: + QPushButton *sidebar_alert_widget; + QWidget *sidebar_widget; std::map panels; QStackedLayout *panel_layout; -private slots: +public slots: void setActivePanel(); + void closeSidebar(); + void openSidebar(); }; diff --git a/selfdrive/ui/qt/offroad/wifi.cc b/selfdrive/ui/qt/offroad/wifi.cc index 00088d7b..ae641f50 100644 --- a/selfdrive/ui/qt/offroad/wifi.cc +++ b/selfdrive/ui/qt/offroad/wifi.cc @@ -6,11 +6,11 @@ #include #include "wifi.hpp" - +#include "widgets/toggle.hpp" void clearLayout(QLayout* layout) { while (QLayoutItem* item = layout->takeAt(0)) { - if (QWidget* widget = item->widget()){ + if (QWidget* widget = item->widget()) { widget->deleteLater(); } if (QLayout* childLayout = item->layout()) { @@ -20,41 +20,78 @@ void clearLayout(QLayout* layout) { } } -WifiUI::WifiUI(QWidget *parent) : QWidget(parent) { - wifi = new WifiManager; +WifiUI::WifiUI(QWidget *parent, int page_length) : QWidget(parent), networks_per_page(page_length) { + try { + wifi = new WifiManager; + } catch (std::exception &e) { + QLabel* warning = new QLabel("Network manager is inactive!"); + warning->setStyleSheet(R"(font-size: 65px;)"); + + QVBoxLayout* warning_layout = new QVBoxLayout; + warning_layout->addWidget(warning, 0, Qt::AlignCenter); + setLayout(warning_layout); + return; + } + + QObject::connect(wifi, SIGNAL(wrongPassword(QString)), this, SLOT(wrongPassword(QString))); QVBoxLayout * top_layout = new QVBoxLayout; + top_layout->setSpacing(0); swidget = new QStackedWidget; // Networks page wifi_widget = new QWidget; + QVBoxLayout* networkLayout = new QVBoxLayout; + QHBoxLayout *tethering_field = new QHBoxLayout; + tethering_field->addSpacing(50); + + ipv4 = new QLabel(""); + tethering_field->addWidget(ipv4); + tethering_field->addWidget(new QLabel("Enable Tethering")); + + Toggle* toggle_switch = new Toggle(this); + toggle_switch->setFixedSize(150, 100); + tethering_field->addWidget(toggle_switch); + if (wifi->tetheringEnabled()) { + toggle_switch->togglePosition(); + } + QObject::connect(toggle_switch, SIGNAL(stateChanged(int)), this, SLOT(toggleTethering(int))); + + QWidget* tetheringWidget = new QWidget; + tetheringWidget->setLayout(tethering_field); + tetheringWidget->setFixedHeight(150); + networkLayout->addWidget(tetheringWidget); + vlayout = new QVBoxLayout; wifi_widget->setLayout(vlayout); - swidget->addWidget(wifi_widget); + networkLayout->addWidget(wifi_widget); + + QWidget* networkWidget = new QWidget; + networkWidget->setLayout(networkLayout); + swidget->addWidget(networkWidget); // Keyboard page - a = new InputField(); - QObject::connect(a, SIGNAL(emitText(QString)), this, SLOT(receiveText(QString))); - swidget->addWidget(a); + input_field = new InputField(); + QObject::connect(input_field, SIGNAL(emitText(QString)), this, SLOT(receiveText(QString))); + swidget->addWidget(input_field); swidget->setCurrentIndex(0); top_layout->addWidget(swidget); setLayout(top_layout); - a->setStyleSheet(R"( - QLineEdit { - background-color: #114265; - } - )"); - - // TODO: implement (not) connecting with wrong password // Update network list timer = new QTimer(this); QObject::connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); - timer->start(400); + timer->start(2000); // Scan on startup + QLabel *scanning = new QLabel("Scanning for networks"); + scanning->setStyleSheet(R"(font-size: 65px;)"); + vlayout->addWidget(scanning, 0, Qt::AlignCenter); + vlayout->setSpacing(25); + wifi->request_scan(); + refresh(); page = 0; } @@ -65,17 +102,18 @@ void WifiUI::refresh() { wifi->request_scan(); wifi->refreshNetworks(); - + ipv4->setText(wifi->ipv4_address); clearLayout(vlayout); connectButtons = new QButtonGroup(this); QObject::connect(connectButtons, SIGNAL(buttonClicked(QAbstractButton*)), this, SLOT(handleButton(QAbstractButton*))); int i = 0; - for (Network &network : wifi->seen_networks){ + int countWidgets = 0; + int button_height = static_cast(this->height() / (networks_per_page + 1) * 0.6); + for (Network &network : wifi->seen_networks) { QHBoxLayout *hlayout = new QHBoxLayout; - - if(page * networks_per_page <= i && i < (page + 1) * networks_per_page){ + if (page * networks_per_page <= i && i < (page + 1) * networks_per_page) { // SSID hlayout->addSpacing(50); hlayout->addWidget(new QLabel(QString::fromUtf8(network.ssid))); @@ -90,9 +128,10 @@ void WifiUI::refresh() { hlayout->addSpacing(20); // connect button - QPushButton* btn = new QPushButton(network.connected ? "Connected" : "Connect"); + QPushButton* btn = new QPushButton(network.connected == ConnectedType::CONNECTED ? "Connected" : (network.connected == ConnectedType::CONNECTING ? "Connecting" : "Connect")); btn->setFixedWidth(300); - btn->setDisabled(network.connected || network.security_type == SecurityType::UNSUPPORTED); + btn->setFixedHeight(button_height); + btn->setDisabled(network.connected == ConnectedType::CONNECTED || network.connected == ConnectedType::CONNECTING || network.security_type == SecurityType::UNSUPPORTED); hlayout->addWidget(btn); hlayout->addSpacing(20); @@ -103,9 +142,11 @@ void WifiUI::refresh() { vlayout->addWidget(w); w->setStyleSheet(R"( QLabel { - font-size: 40px; + font-size: 50px; } - QPushButton:enabled { + QPushButton { + padding: 0; + font-size: 50px; background-color: #114265; } QPushButton:disabled { @@ -115,31 +156,37 @@ void WifiUI::refresh() { background-color: #114265; } )"); + countWidgets++; } - i+=1; + i++; } + + // Pad vlayout to prevert oversized network widgets in case of low visible network count + for (int i = countWidgets; i < networks_per_page; i++) { + QWidget *w = new QWidget; + vlayout->addWidget(w); + } + QHBoxLayout *prev_next_buttons = new QHBoxLayout; QPushButton* prev = new QPushButton("Previous"); prev->setEnabled(page); - prev->setFixedHeight(100); - + prev->setFixedHeight(button_height); QPushButton* next = new QPushButton("Next"); - next->setFixedHeight(100); - //If there are more visible networks then we can show, enable going to next page - if(wifi->seen_networks.size() > (page + 1) * networks_per_page){ - next->setEnabled(true); - }else{ - next->setDisabled(true); - } + next->setFixedHeight(button_height); + + // If there are more visible networks then we can show, enable going to next page + next->setEnabled(wifi->seen_networks.size() > (page + 1) * networks_per_page); + QObject::connect(prev, SIGNAL(released()), this, SLOT(prevPage())); QObject::connect(next, SIGNAL(released()), this, SLOT(nextPage())); prev_next_buttons->addWidget(prev); prev_next_buttons->addWidget(next); - QWidget * w = new QWidget; + QWidget *w = new QWidget; w->setLayout(prev_next_buttons); w->setStyleSheet(R"( - QPushButton:enabled { + QPushButton { + padding: 0; background-color: #114265; } QPushButton:disabled { @@ -152,28 +199,42 @@ void WifiUI::refresh() { vlayout->addWidget(w); } -void WifiUI::handleButton(QAbstractButton* button) { - QPushButton* btn = static_cast(button); - qDebug() << connectButtons->id(btn); - Network n = wifi->seen_networks[connectButtons->id(btn)]; - a->label->setText("Enter password for \"" + n.ssid + "\""); - if(n.security_type == SecurityType::OPEN){ - wifi->connect(n); - } else if (n.security_type == SecurityType::WPA){ - QString password = getStringFromUser(); - if(password.size()){ - wifi->connect(n, password); - } +void WifiUI::toggleTethering(int enable) { + if (enable) { + wifi->enableTethering(); } else { - qDebug() << "Cannot determine network's security type"; + wifi->disableTethering(); } } -QString WifiUI::getStringFromUser(){ +void WifiUI::handleButton(QAbstractButton* button) { + QPushButton* btn = static_cast(button); + Network n = wifi->seen_networks[connectButtons->id(btn)]; + connectToNetwork(n); +} + +void WifiUI::connectToNetwork(Network n) { + timer->stop(); + if (n.security_type == SecurityType::OPEN) { + wifi->connect(n); + } else if (n.security_type == SecurityType::WPA) { + input_field->setPromptText("Enter password for \"" + n.ssid + "\""); + QString password = getStringFromUser(); + if (password.size()) { + wifi->connect(n, password); + } + } + refresh(); + timer->start(); +} + +QString WifiUI::getStringFromUser() { + emit openKeyboard(); swidget->setCurrentIndex(1); loop.exec(); + emit closeKeyboard(); swidget->setCurrentIndex(0); return text; } @@ -182,6 +243,20 @@ void WifiUI::receiveText(QString t) { loop.quit(); text = t; } + + +void WifiUI::wrongPassword(QString ssid) { + if (loop.isRunning()) { + return; + } + for (Network n : wifi->seen_networks) { + if (n.ssid == ssid) { + input_field->setPromptText("Wrong password for \"" + n.ssid +"\""); + connectToNetwork(n); + } + } +} + void WifiUI::prevPage() { page--; refresh(); diff --git a/selfdrive/ui/qt/offroad/wifi.hpp b/selfdrive/ui/qt/offroad/wifi.hpp index 0343e6be..bad8d935 100644 --- a/selfdrive/ui/qt/offroad/wifi.hpp +++ b/selfdrive/ui/qt/offroad/wifi.hpp @@ -7,36 +7,46 @@ #include #include "wifiManager.hpp" -#include "input_field.hpp" +#include "widgets/input_field.hpp" class WifiUI : public QWidget { Q_OBJECT -private: - WifiManager* wifi; +public: int page; - const int networks_per_page = 10; + explicit WifiUI(QWidget *parent = 0, int page_length = 5); - QStackedWidget* swidget; - QVBoxLayout* vlayout; - QWidget * wifi_widget; +private: + WifiManager *wifi = nullptr; + const int networks_per_page; - InputField *a; + QStackedWidget *swidget; + QVBoxLayout *vlayout; + QWidget *wifi_widget; + + InputField *input_field; QEventLoop loop; - QTimer * timer; + QTimer *timer; QString text; QButtonGroup *connectButtons; + bool tetheringEnabled; + QLabel *ipv4; + void connectToNetwork(Network n); QString getStringFromUser(); -public: - explicit WifiUI(QWidget *parent = 0); - private slots: void handleButton(QAbstractButton* m_button); + void toggleTethering(int enable); void refresh(); void receiveText(QString text); + void wrongPassword(QString ssid); + void prevPage(); void nextPage(); + +signals: + void openKeyboard(); + void closeKeyboard(); }; diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index 987b09c8..ff8977c9 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -1,7 +1,24 @@ #include #include - +#include +#include #include "wifiManager.hpp" +#include "common/params.h" + +/** + * We are using a NetworkManager DBUS API : https://developer.gnome.org/NetworkManager/1.26/spec.html + * */ + +// https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NM80211ApFlags +const int NM_802_11_AP_FLAGS_PRIVACY = 0x00000001; + +// https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NM80211ApSecurityFlags +const int NM_802_11_AP_SEC_PAIR_WEP40 = 0x00000001; +const int NM_802_11_AP_SEC_PAIR_WEP104 = 0x00000002; +const int NM_802_11_AP_SEC_GROUP_WEP40 = 0x00000010; +const int NM_802_11_AP_SEC_GROUP_WEP104 = 0x00000020; +const int NM_802_11_AP_SEC_KEY_MGMT_PSK = 0x00000100; +const int NM_802_11_AP_SEC_KEY_MGMT_802_1X = 0x00000200; QString nm_path = "/org/freedesktop/NetworkManager"; @@ -15,40 +32,65 @@ QString device_iface = "org.freedesktop.NetworkManager.Device"; QString wireless_device_iface = "org.freedesktop.NetworkManager.Device.Wireless"; QString ap_iface = "org.freedesktop.NetworkManager.AccessPoint"; QString connection_iface = "org.freedesktop.NetworkManager.Connection.Active"; +QString ipv4config_iface = "org.freedesktop.NetworkManager.IP4Config"; QString nm_service = "org.freedesktop.NetworkManager"; +const int state_connected = 100; +const int state_need_auth = 60; +const int reason_wrong_password = 8; template -T get_response(QDBusMessage response){ +T get_response(QDBusMessage response) { QVariant first = response.arguments().at(0); QDBusVariant dbvFirst = first.value(); QVariant vFirst = dbvFirst.variant(); return vFirst.value(); } -bool compare_by_strength(const Network &a, const Network &b){ - if (a.connected) return true; - if (b.connected) return false; +bool compare_by_strength(const Network &a, const Network &b) { + if (a.connected == ConnectedType::CONNECTED) return true; + if (b.connected == ConnectedType::CONNECTED) return false; + if (a.connected == ConnectedType::CONNECTING) return true; + if (b.connected == ConnectedType::CONNECTING) return false; return a.strength > b.strength; } -WifiManager::WifiManager(){ - qDBusRegisterMetaType(); +WifiManager::WifiManager() { + qDBusRegisterMetaType(); + qDBusRegisterMetaType(); + connecting_to_network = ""; adapter = get_adapter(); - has_adapter = adapter != ""; + + bool has_adapter = adapter != ""; + if (!has_adapter){ + throw std::runtime_error("Error connecting to panda"); + } + + QDBusInterface nm(nm_service, adapter, device_iface, bus); + bus.connect(nm_service, adapter, device_iface, "StateChanged", this, SLOT(change(unsigned int, unsigned int, unsigned int))); + + QDBusInterface device_props(nm_service, adapter, props_iface, bus); + QDBusMessage response = device_props.call("Get", device_iface, "State"); + raw_adapter_state = get_response(response); + change(raw_adapter_state, 0, 0); + + // Compute tethering ssid as "Weedle" + first 4 characters of a dongle id + tethering_ssid = "weedle"; + std::string bytes = Params().get("DongleId"); + if (bytes.length() >= 4) { + tethering_ssid+="-"+QString::fromStdString(bytes.substr(0,4)); + } } -void WifiManager::refreshNetworks(){ - if (!has_adapter) return; - +void WifiManager::refreshNetworks() { bus = QDBusConnection::systemBus(); seen_networks.clear(); seen_ssids.clear(); - - for (Network &network : get_networks()){ - if(seen_ssids.count(network.ssid)){ + ipv4_address = get_ipv4_address(); + for (Network &network : get_networks()) { + if (seen_ssids.count(network.ssid)) { continue; } seen_ssids.push_back(network.ssid); @@ -56,14 +98,39 @@ void WifiManager::refreshNetworks(){ } } -QList WifiManager::get_networks(){ +QString WifiManager::get_ipv4_address(){ + if (raw_adapter_state != state_connected){ + return ""; + } + QVector conns = get_active_connections(); + for (auto p : conns){ + QString active_connection = p.path(); + QDBusInterface nm(nm_service, active_connection, props_iface, bus); + QDBusObjectPath pth = get_response(nm.call("Get", connection_iface, "Ip4Config")); + QString ip4config = pth.path(); + + QDBusInterface nm2(nm_service, ip4config, props_iface, bus); + const QDBusArgument &arr = get_response(nm2.call("Get", ipv4config_iface, "AddressData")); + QMap pth2; + arr.beginArray(); + while (!arr.atEnd()){ + arr >> pth2; + QString ipv4 = pth2.value("address").value(); + arr.endArray(); + return ipv4; + } + arr.endArray(); + } + return ""; +} + +QList WifiManager::get_networks() { QList r; QDBusInterface nm(nm_service, adapter, wireless_device_iface, bus); QDBusMessage response = nm.call("GetAllAccessPoints"); QVariant first = response.arguments().at(0); QString active_ap = get_active_ap(); - const QDBusArgument &args = first.value(); args.beginArray(); while (!args.atEnd()) { @@ -73,9 +140,19 @@ QList WifiManager::get_networks(){ QByteArray ssid = get_property(path.path(), "Ssid"); unsigned int strength = get_ap_strength(path.path()); SecurityType security = getSecurityType(path.path()); - Network network = {path.path(), ssid, strength, path.path()==active_ap, security}; + ConnectedType ctype; + if (path.path() != active_ap) { + ctype = ConnectedType::DISCONNECTED; + } else { + if (ssid == connecting_to_network) { + ctype = ConnectedType::CONNECTING; + } else { + ctype = ConnectedType::CONNECTED; + } + } + Network network = {path.path(), ssid, strength, ctype, security}; - if (ssid.length()){ + if (ssid.length()) { r.push_back(network); } } @@ -85,54 +162,50 @@ QList WifiManager::get_networks(){ return r; } -SecurityType WifiManager::getSecurityType(QString path){ +SecurityType WifiManager::getSecurityType(QString path) { int sflag = get_property(path, "Flags").toInt(); int wpaflag = get_property(path, "WpaFlags").toInt(); int rsnflag = get_property(path, "RsnFlags").toInt(); int wpa_props = wpaflag | rsnflag; - if(sflag == 0){ + // obtained by looking at flags of networks in the office as reported by an Android phone + const int supports_wpa = NM_802_11_AP_SEC_PAIR_WEP40 | NM_802_11_AP_SEC_PAIR_WEP104 | NM_802_11_AP_SEC_GROUP_WEP40 | NM_802_11_AP_SEC_GROUP_WEP104 | NM_802_11_AP_SEC_KEY_MGMT_PSK; + + if (sflag == 0) { return SecurityType::OPEN; - } else if((sflag & 0x1) && (wpa_props & (0x333) && !(wpa_props & 0x200))) { + } else if ((sflag & NM_802_11_AP_FLAGS_PRIVACY) && (wpa_props & supports_wpa) && !(wpa_props & NM_802_11_AP_SEC_KEY_MGMT_802_1X)) { return SecurityType::WPA; } else { - // qDebug() << "Cannot determine security type for " << get_property(path, "Ssid") << " with flags"; - // qDebug() << "flag " << sflag; - // qDebug() << "WpaFlag " << wpaflag; - // qDebug() << "RsnFlag " << rsnflag; return SecurityType::UNSUPPORTED; } } -void WifiManager::connect(Network n){ +void WifiManager::connect(Network n) { return connect(n, "", ""); } -void WifiManager::connect(Network n, QString password){ +void WifiManager::connect(Network n, QString password) { return connect(n, "", password); } -void WifiManager::connect(Network n, QString username, QString password){ - QString active_ap = get_active_ap(); - if (active_ap!="") { - clear_connections(get_property(active_ap, "Ssid")); - } - clear_connections(n.ssid); - qDebug() << "Connecting to"<< n.ssid << "with username, password =" << username << "," < result = nm_settings.call("AddConnection", QVariant::fromValue(connection)); - if (!result.isValid()) { - qDebug() << result.error().name() << result.error().message(); - } else { - qDebug() << result.value().path(); + nm_settings.call("AddConnection", QVariant::fromValue(connection)); +} + +void WifiManager::deactivate_connections(QString ssid) { + for (QDBusObjectPath active_connection_raw : get_active_connections()) { + QString active_connection = active_connection_raw.path(); + QDBusInterface nm(nm_service, active_connection, props_iface, bus); + QDBusObjectPath pth = get_response(nm.call("Get", connection_iface, "SpecificObject")); + QString Ssid = get_property(pth.path(), "Ssid"); + if (Ssid == ssid) { + QDBusInterface nm2(nm_service, nm_path, nm_iface, bus); + nm2.call("DeactivateConnection", QVariant::fromValue(active_connection_raw)); + } } } -void WifiManager::print_active_connections(){ - //TO-DO clean up, the code is not currently in use. +QVector WifiManager::get_active_connections() { QDBusInterface nm(nm_service, nm_path, props_iface, bus); QDBusMessage response = nm.call("Get", nm_iface, "ActiveConnections"); - QVariant first = response.arguments().at(0); - QDBusVariant dbvFirst = first.value(); - QVariant vFirst = dbvFirst.variant(); - QDBusArgument step4 = vFirst.value(); + const QDBusArgument &arr = get_response(response); + QVector conns; + QDBusObjectPath path; - step4.beginArray(); - while (!step4.atEnd()){ - step4 >> path; - qDebug()<> path; + conns.push_back(path); } - step4.endArray(); + arr.endArray(); + return conns; } -void WifiManager::clear_connections(QString ssid){ +void WifiManager::clear_connections(QString ssid) { QDBusInterface nm(nm_service, nm_settings_path, nm_settings_iface, bus); QDBusMessage response = nm.call("ListConnections"); QVariant first = response.arguments().at(0); @@ -183,13 +263,12 @@ void WifiManager::clear_connections(QString ssid){ QMap > map; dbusArg >> map; - for(QString outer_key : map.keys()) { + for (QString outer_key : map.keys()) { QMap innerMap = map.value(outer_key); - for(QString inner_key : innerMap.keys()) { - if(inner_key == "ssid"){ + for (QString inner_key : innerMap.keys()) { + if (inner_key == "ssid") { QString value = innerMap.value(inner_key).value(); - if(value == ssid){ - // qDebug()<<"Deleting "<(response); return resp; } -QString WifiManager::get_active_ap(){ +QString WifiManager::get_active_ap() { QDBusInterface device_props(nm_service, adapter, props_iface, bus); QDBusMessage response = device_props.call("Get", wireless_device_iface, "ActiveAccessPoint"); QDBusObjectPath r = get_response(response); return r.path(); } -QByteArray WifiManager::get_property(QString network_path ,QString property){ +QByteArray WifiManager::get_property(QString network_path ,QString property) { QDBusInterface device_props(nm_service, network_path, props_iface, bus); QDBusMessage response = device_props.call("Get", ap_iface, property); return get_response(response); } -unsigned int WifiManager::get_ap_strength(QString network_path){ +unsigned int WifiManager::get_ap_strength(QString network_path) { QDBusInterface device_props(nm_service, network_path, props_iface, bus); QDBusMessage response = device_props.call("Get", ap_iface, "Strength"); return get_response(response); } -QString WifiManager::get_adapter(){ +QString WifiManager::get_adapter() { QDBusInterface nm(nm_service, nm_path, nm_iface, bus); QDBusMessage response = nm.call("GetDevices"); @@ -259,3 +336,61 @@ QString WifiManager::get_adapter(){ return adapter_path; } + +void WifiManager::change(unsigned int new_state,unsigned int previous_state,unsigned int change_reason) { + raw_adapter_state = new_state; + if (new_state == state_need_auth && change_reason == reason_wrong_password) { + emit wrongPassword(connecting_to_network); + } else if (new_state == state_connected) { + connecting_to_network = ""; + } +} + +void WifiManager::disconnect() { + QString active_ap = get_active_ap(); + if (active_ap!="" && active_ap!="/") { + deactivate_connections(get_property(active_ap, "Ssid")); + } +} + +//Functions for tethering + +void WifiManager::enableTethering() { + disconnect(); + Connection connection; + connection["connection"]["id"] = "Hotspot"; + connection["connection"]["uuid"] = QUuid::createUuid().toString().remove('{').remove('}'); + connection["connection"]["type"] = "802-11-wireless"; + connection["connection"]["interface-name"] = "wlan0"; + + connection["802-11-wireless"]["band"] = "bg"; + connection["802-11-wireless"]["mode"] = "ap"; + connection["802-11-wireless"]["ssid"] = tethering_ssid.toUtf8(); + + connection["802-11-wireless-security"]["group"] = QStringList("ccmp"); + connection["802-11-wireless-security"]["key-mgmt"] = "wpa-psk"; + connection["802-11-wireless-security"]["pairwise"] = QStringList("ccmp"); + connection["802-11-wireless-security"]["proto"] = QStringList("rsn"); + connection["802-11-wireless-security"]["psk"] = "swagswagcomma"; + + connection["ipv4"]["method"] = "shared"; + QMap address; + address["address"] = "192.168.43.1"; + address["prefix"] = 24u; + connection["ipv4"]["address-data"] = QVariant::fromValue(IpConfig() << address); + connection["ipv4"]["gateway"] = "192.168.43.1"; + connection["ipv6"]["method"] = "ignore"; + + QDBusInterface nm_settings(nm_service, nm_settings_path, nm_settings_iface, bus); + nm_settings.call("AddConnection", QVariant::fromValue(connection)); + +} + +void WifiManager::disableTethering() { + clear_connections(tethering_ssid); +} + +bool WifiManager::tetheringEnabled() { + QString active_ap = get_active_ap(); + return get_property(active_ap, "Ssid") == tethering_ssid; +} diff --git a/selfdrive/ui/qt/offroad/wifiManager.hpp b/selfdrive/ui/qt/offroad/wifiManager.hpp index e107f6ad..13eb90f3 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.hpp +++ b/selfdrive/ui/qt/offroad/wifiManager.hpp @@ -8,43 +8,67 @@ enum class SecurityType { WPA, UNSUPPORTED }; +enum class ConnectedType{ + DISCONNECTED, + CONNECTING, + CONNECTED +}; typedef QMap> Connection; +typedef QVector> IpConfig; struct Network { QString path; QByteArray ssid; unsigned int strength; - bool connected; + ConnectedType connected; SecurityType security_type; }; -class WifiManager{ +class WifiManager : public QWidget { + Q_OBJECT public: explicit WifiManager(); - bool has_adapter; void request_scan(); QVector seen_networks; + QString ipv4_address; void refreshNetworks(); void connect(Network ssid); void connect(Network ssid, QString password); void connect(Network ssid, QString username, QString password); + // Tethering functions + + void enableTethering(); + void disableTethering(); + bool tetheringEnabled(); private: QVector seen_ssids; QString adapter;//Path to network manager wifi-device QDBusConnection bus = QDBusConnection::systemBus(); + unsigned int raw_adapter_state;//Connection status https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NMDeviceState + QString connecting_to_network; + QString tethering_ssid; QString get_adapter(); + QString get_ipv4_address(); QList get_networks(); void connect(QByteArray ssid, QString username, QString password, SecurityType security_type); QString get_active_ap(); + void deactivate_connections(QString ssid); void clear_connections(QString ssid); - void print_active_connections(); + QVector get_active_connections(); uint get_wifi_device_state(); QByteArray get_property(QString network_path, QString property); unsigned int get_ap_strength(QString network_path); SecurityType getSecurityType(QString ssid); + void disconnect(); + +private slots: + void change(unsigned int new_state, unsigned int previous_state, unsigned int change_reason); +signals: + void wrongPassword(QString ssid); + void refresh(); }; diff --git a/selfdrive/ui/qt/qt_window.cc b/selfdrive/ui/qt/qt_window.cc new file mode 100644 index 00000000..7f6b383b --- /dev/null +++ b/selfdrive/ui/qt/qt_window.cc @@ -0,0 +1,26 @@ +#include +#include + +#ifdef QCOM2 +#include +#include +#include +#endif + +#include "qt_window.hpp" + +void setMainWindow(QWidget *w) { + float scale = getenv("SCALE") != NULL ? std::stof(getenv("SCALE")) : 1.0; + w->setFixedSize(vwp_w*scale, vwp_h*scale); + w->show(); + +#ifdef QCOM2 + QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); + wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", w->windowHandle())); + wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); + wl_surface_commit(s); + w->showFullScreen(); +#endif +} + + diff --git a/selfdrive/ui/qt/qt_window.hpp b/selfdrive/ui/qt/qt_window.hpp new file mode 100644 index 00000000..a4485959 --- /dev/null +++ b/selfdrive/ui/qt/qt_window.hpp @@ -0,0 +1,9 @@ +#include + +#ifdef QCOM2 + const int vwp_w = 2160, vwp_h = 1080; +#else + const int vwp_w = 1920, vwp_h = 1080; +#endif + +void setMainWindow(QWidget *w); diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc index 540b58f1..77151f3b 100644 --- a/selfdrive/ui/qt/spinner.cc +++ b/selfdrive/ui/qt/spinner.cc @@ -7,14 +7,8 @@ #include #include -#ifdef QCOM2 -#include -#include -#include -#endif - #include "spinner.hpp" - +#include "qt_window.hpp" Spinner::Spinner(QWidget *parent) { QGridLayout *main_layout = new QGridLayout(); @@ -100,25 +94,7 @@ void Spinner::update(int n) { int main(int argc, char *argv[]) { QApplication a(argc, argv); - - Spinner *spinner = new Spinner(); - - // TODO: get size from QScreen, doesn't work on tici -#ifdef QCOM2 - int w = 2160, h = 1080; -#else - int w = 1920, h = 1080; -#endif - spinner->setFixedSize(w, h); - spinner->show(); - -#ifdef QCOM2 - QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); - wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", spinner->windowHandle())); - wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); - wl_surface_commit(s); - spinner->showFullScreen(); -#endif - + Spinner spinner; + setMainWindow(&spinner); return a.exec(); } diff --git a/selfdrive/ui/qt/text.cc b/selfdrive/ui/qt/text.cc index ab7d9f4c..9af03a91 100644 --- a/selfdrive/ui/qt/text.cc +++ b/selfdrive/ui/qt/text.cc @@ -1,45 +1,21 @@ -#include - -#include #include #include #include #include #include -#include - -#ifdef QCOM2 -#include -#include -#include -#endif +#include "qt_window.hpp" int main(int argc, char *argv[]) { QApplication a(argc, argv); - QWidget *window = new QWidget(); + QWidget window; + setMainWindow(&window); - // TODO: get size from QScreen, doesn't work on tici -#ifdef QCOM2 - int w = 2160, h = 1080; -#else - int w = 1920, h = 1080; -#endif - window->setFixedSize(w, h); + QVBoxLayout *layout = new QVBoxLayout(); + layout->setContentsMargins(125, 125, 125, 125); - QVBoxLayout *main_layout = new QVBoxLayout(); - - QString text = ""; - for (int i = 1; i < argc; i++) { - if (i > 1) { - text.append(" "); - } - text.append(argv[i]); - } - - QLabel *label = new QLabel(text); - label->setAlignment(Qt::AlignTop); - main_layout->addWidget(label); + // TODO: make this scroll + layout->addWidget(new QLabel(argv[1]), 0, Qt::AlignTop); QPushButton *btn = new QPushButton(); #ifdef __aarch64__ @@ -51,40 +27,23 @@ int main(int argc, char *argv[]) { btn->setText("Exit"); QObject::connect(btn, SIGNAL(released()), &a, SLOT(quit())); #endif - main_layout->addWidget(btn); + layout->addWidget(btn, 0, Qt::AlignRight); - window->setLayout(main_layout); - window->setStyleSheet(R"( - QWidget { - margin: 60px; - background-color: black; - } - QLabel { + window.setLayout(layout); + window.setStyleSheet(R"( + * { color: white; + background-color: black; font-size: 60px; } QPushButton { - color: white; - font-size: 50px; - padding: 60px; - margin-left: 1500px; - border-color: white; - border-width: 2px; - border-style: solid; + padding: 50px; + padding-right: 100px; + padding-left: 100px; + border: 2px solid white; border-radius: 20px; } )"); - window->show(); - - -#ifdef QCOM2 - QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); - wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", window->windowHandle())); - wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); - wl_surface_commit(s); - window->showFullScreen(); -#endif - return a.exec(); } diff --git a/selfdrive/ui/qt/ui.cc b/selfdrive/ui/qt/ui.cc index de4f6950..93eb77f9 100644 --- a/selfdrive/ui/qt/ui.cc +++ b/selfdrive/ui/qt/ui.cc @@ -1,12 +1,7 @@ #include -#ifdef QCOM2 -#include -#include -#include -#endif - #include "window.hpp" +#include "qt_window.hpp" int main(int argc, char *argv[]) { QSurfaceFormat fmt; @@ -20,20 +15,8 @@ int main(int argc, char *argv[]) { QSurfaceFormat::setDefaultFormat(fmt); QApplication a(argc, argv); - MainWindow w; - w.setFixedSize(vwp_w, vwp_h); - w.show(); - + setMainWindow(&w); a.installEventFilter(&w); - -#ifdef QCOM2 - QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); - wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", w.windowHandle())); - wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); - wl_surface_commit(s); - w.showFullScreen(); -#endif - return a.exec(); } diff --git a/selfdrive/ui/qt/widgets/input_field.cc b/selfdrive/ui/qt/widgets/input_field.cc new file mode 100644 index 00000000..858c1c75 --- /dev/null +++ b/selfdrive/ui/qt/widgets/input_field.cc @@ -0,0 +1,65 @@ +#include + +#include "input_field.hpp" + +InputField::InputField(QWidget *parent): QWidget(parent) { + layout = new QGridLayout(); + layout->setSpacing(30); + + label = new QLabel(this); + label->setStyleSheet(R"(font-size: 55px;)"); + layout->addWidget(label, 0, 0, Qt::AlignVCenter | Qt::AlignLeft); + layout->setColumnStretch(0, 1); + + QPushButton* cancel = new QPushButton("Cancel"); + cancel->setFixedSize(300, 150); + cancel->setStyleSheet(R"(padding: 0;)"); + layout->addWidget(cancel, 0, 1, Qt::AlignVCenter | Qt::AlignRight); + QObject::connect(cancel, SIGNAL(released()), this, SLOT(emitEmpty())); + + // text box + line = new QLineEdit(); + line->setStyleSheet(R"( + color: black; + background-color: white; + font-size: 45px; + padding: 25px; + )"); + layout->addWidget(line, 1, 0, 1, -1); + + k = new Keyboard(this); + QObject::connect(k, SIGNAL(emitButton(QString)), this, SLOT(getText(QString))); + layout->addWidget(k, 2, 0, 1, -1); + + setLayout(layout); +} + +void InputField::setPromptText(QString text) { + label->setText(text); +} + +void InputField::emitEmpty() { + emitText(""); + line->setText(""); +} + +void InputField::getText(QString s) { + if (!QString::compare(s,"⌫")) { + line->backspace(); + } + + if (!QString::compare(s,"⏎")) { + emitText(line->text()); + line->setText(""); + } + + QVector control_buttons {"⇧", "↑", "ABC", "⏎", "#+=", "⌫", "123"}; + for(QString c : control_buttons) { + if (!QString::compare(s, c)) { + return; + } + } + + line->insert(s.left(1)); +} + diff --git a/selfdrive/ui/qt/offroad/input_field.hpp b/selfdrive/ui/qt/widgets/input_field.hpp similarity index 77% rename from selfdrive/ui/qt/offroad/input_field.hpp rename to selfdrive/ui/qt/widgets/input_field.hpp index a56bac26..8dad0c0d 100644 --- a/selfdrive/ui/qt/offroad/input_field.hpp +++ b/selfdrive/ui/qt/widgets/input_field.hpp @@ -1,10 +1,10 @@ #pragma once +#include +#include #include #include -#include -#include -#include +#include #include "keyboard.hpp" @@ -13,12 +13,13 @@ class InputField : public QWidget { public: explicit InputField(QWidget* parent = 0); - QLabel *label; - + void setPromptText(QString text); + private: QLineEdit *line; Keyboard *k; - QVBoxLayout *l; + QLabel *label; + QGridLayout *layout; public slots: void emitEmpty(); diff --git a/selfdrive/ui/qt/offroad/keyboard.cc b/selfdrive/ui/qt/widgets/keyboard.cc similarity index 71% rename from selfdrive/ui/qt/offroad/keyboard.cc rename to selfdrive/ui/qt/widgets/keyboard.cc index 0cdc38c5..bf192aa5 100644 --- a/selfdrive/ui/qt/offroad/keyboard.cc +++ b/selfdrive/ui/qt/widgets/keyboard.cc @@ -7,35 +7,38 @@ #include "keyboard.hpp" -KeyboardLayout::KeyboardLayout(QWidget* parent, std::vector> layout) : QWidget(parent) { +const int DEFAULT_WIDTH = 1; +const int SPACEBAR_WIDTH = 3; + +KeyboardLayout::KeyboardLayout(QWidget *parent, std::vector> layout) : QWidget(parent) { QVBoxLayout* vlayout = new QVBoxLayout; QButtonGroup* btn_group = new QButtonGroup(this); QObject::connect(btn_group, SIGNAL(buttonClicked(QAbstractButton*)), parent, SLOT(handleButton(QAbstractButton*))); int i = 0; - for(auto s : layout){ + for (auto s : layout) { QHBoxLayout *hlayout = new QHBoxLayout; - if (i == 1){ - hlayout->addSpacing(50); + if (i == 1) { + hlayout->addSpacing(90); } - for(QString p : s){ + for (QString p : s) { QPushButton* btn = new QPushButton(p); - btn->setFixedHeight(100); - - if (p == QString(" ")){ - btn->setFixedWidth(1024); + btn->setFixedHeight(120); + btn_group->addButton(btn); + hlayout->addSpacing(10); + if (p == QString(" ")) { + hlayout->addWidget(btn, SPACEBAR_WIDTH); + } else { + hlayout->addWidget(btn, DEFAULT_WIDTH); } - btn_group->addButton(btn); - hlayout->addSpacing(5); - hlayout->addWidget(btn); } - if (i == 1){ - hlayout->addSpacing(50); + if (i == 1) { + hlayout->addSpacing(90); } vlayout->addLayout(hlayout); @@ -88,7 +91,10 @@ Keyboard::Keyboard(QWidget *parent) : QWidget(parent) { main_layout->setCurrentIndex(0); setStyleSheet(R"( - QPushButton { font-size: 40px } + QPushButton { + padding: 0; + font-size: 50px; + } * { background-color: #99777777; } @@ -96,24 +102,24 @@ Keyboard::Keyboard(QWidget *parent) : QWidget(parent) { } -void Keyboard::handleButton(QAbstractButton* m_button){ +void Keyboard::handleButton(QAbstractButton* m_button) { QString id = m_button->text(); - if(!QString::compare(m_button->text(),"↑")||!QString::compare(m_button->text(),"ABC")){ + if (!QString::compare(m_button->text(), "↑") || !QString::compare(m_button->text(), "ABC")) { main_layout->setCurrentIndex(0); } - if(!QString::compare(m_button->text(),"⇧")){ + if (!QString::compare(m_button->text(), "⇧")) { main_layout->setCurrentIndex(1); } - if(!QString::compare(m_button->text(),"123")){ + if (!QString::compare(m_button->text(), "123")) { main_layout->setCurrentIndex(2); } - if(!QString::compare(m_button->text(),"#+=")){ + if (!QString::compare(m_button->text(), "#+=")) { main_layout->setCurrentIndex(3); } - if(!QString::compare(m_button->text(),"⏎")){ + if (!QString::compare(m_button->text(), "⏎")) { main_layout->setCurrentIndex(0); } - if("A" <= id && id <= "Z"){ + if ("A" <= id && id <= "Z") { main_layout->setCurrentIndex(0); } emit emitButton(m_button->text()); diff --git a/selfdrive/ui/qt/offroad/keyboard.hpp b/selfdrive/ui/qt/widgets/keyboard.hpp similarity index 100% rename from selfdrive/ui/qt/offroad/keyboard.hpp rename to selfdrive/ui/qt/widgets/keyboard.hpp diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc new file mode 100644 index 00000000..08c8bcde --- /dev/null +++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc @@ -0,0 +1,149 @@ +#include +#include +#include +#include +#include +#include + +#include "offroad_alerts.hpp" + +#include "common/params.h" + + +void cleanLayout(QLayout* layout) { + while (QLayoutItem* item = layout->takeAt(0)) { + if (QWidget* widget = item->widget()) { + widget->deleteLater(); + } + if (QLayout* childLayout = item->layout()) { + cleanLayout(childLayout); + } + delete item; + } +} + +QString vectorToQString(std::vector v) { + return QString::fromStdString(std::string(v.begin(), v.end())); +} + +OffroadAlert::OffroadAlert(QWidget* parent) { + vlayout = new QVBoxLayout; + refresh(); + setLayout(vlayout); +} + +void OffroadAlert::refresh() { + cleanLayout(vlayout); + parse_alerts(); + + updateAvailable = false; + std::vector bytes = Params().read_db_bytes("UpdateAvailable"); + if (bytes.size() && bytes[0] == '1') { + updateAvailable = true; + } + + if (updateAvailable) { + // If there is an update available, don't show alerts + alerts.clear(); + + QFrame *f = new QFrame(); + + QVBoxLayout *update_layout = new QVBoxLayout; + update_layout->setMargin(10); + update_layout->setSpacing(20); + + QLabel *title = new QLabel("Update available"); + title->setStyleSheet(R"( + font-size: 55px; + font-weight: bold; + )"); + update_layout->addWidget(title, 0, Qt::AlignTop); + + QString release_notes = QString::fromStdString(Params().get("ReleaseNotes")); + QLabel *notes_label = new QLabel(release_notes); + notes_label->setStyleSheet(R"(font-size: 40px;)"); + notes_label->setWordWrap(true); + update_layout->addWidget(notes_label, 1, Qt::AlignTop); + + QPushButton *update_button = new QPushButton("Reboot and Update"); + update_layout->addWidget(update_button); +#ifdef __aarch64__ + QObject::connect(update_button, &QPushButton::released, [=]() {std::system("sudo reboot");}); +#endif + + f->setLayout(update_layout); + f->setStyleSheet(R"( + .QFrame{ + border-radius: 20px; + border: 2px solid white; + background-color: #114267; + } + QPushButton { + padding: 20px; + font-size: 35px; + color: white; + background-color: blue; + } + )"); + + vlayout->addWidget(f); + vlayout->addSpacing(60); + } else { + vlayout->addSpacing(60); + + for (auto alert : alerts) { + QLabel *l = new QLabel(alert.text); + l->setWordWrap(true); + l->setMargin(60); + + QString style = R"( + font-size: 40px; + font-weight: bold; + border-radius: 30px; + border: 2px solid; + border-color: white; + )"; + style.append("background-color: " + QString(alert.severity ? "#971b1c" : "#114267")); + + l->setStyleSheet(style); + vlayout->addWidget(l); + vlayout->addSpacing(20); + } + } + + QPushButton *hide_btn = new QPushButton(updateAvailable ? "Later" : "Hide alerts"); + hide_btn->setStyleSheet(R"( + padding: 20px; + font-size: 35px; + color: white; + background-color: blue; + )"); + vlayout->addWidget(hide_btn); + QObject::connect(hide_btn, SIGNAL(released()), this, SIGNAL(closeAlerts())); +} + +void OffroadAlert::parse_alerts() { + alerts.clear(); + // We launch in selfdrive/ui + QFile inFile("../controls/lib/alerts_offroad.json"); + inFile.open(QIODevice::ReadOnly | QIODevice::Text); + QByteArray data = inFile.readAll(); + inFile.close(); + + QJsonDocument doc = QJsonDocument::fromJson(data); + if (doc.isNull()) { + qDebug() << "Parse failed"; + } + + QJsonObject json = doc.object(); + for (const QString& key : json.keys()) { + std::vector bytes = Params().read_db_bytes(key.toStdString().c_str()); + + if (bytes.size()) { + QJsonDocument doc_par = QJsonDocument::fromJson(QByteArray(bytes.data(), bytes.size())); + QJsonObject obj = doc_par.object(); + Alert alert = {obj.value("text").toString(), obj.value("severity").toInt()}; + alerts.push_back(alert); + } + } +} diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.hpp b/selfdrive/ui/qt/widgets/offroad_alerts.hpp new file mode 100644 index 00000000..1e425158 --- /dev/null +++ b/selfdrive/ui/qt/widgets/offroad_alerts.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +struct Alert { + QString text; + int severity; +}; + +class OffroadAlert : public QWidget { + Q_OBJECT + +public: + explicit OffroadAlert(QWidget *parent = 0); + QVector alerts; + bool updateAvailable; + +private: + QVBoxLayout *vlayout; + + void parse_alerts(); + +signals: + void closeAlerts(); + +public slots: + void refresh(); +}; diff --git a/selfdrive/ui/qt/widgets/toggle.cc b/selfdrive/ui/qt/widgets/toggle.cc new file mode 100644 index 00000000..bf303091 --- /dev/null +++ b/selfdrive/ui/qt/widgets/toggle.cc @@ -0,0 +1,52 @@ +#include "toggle.hpp" + +Toggle::Toggle(QWidget *parent) : QAbstractButton(parent), +_height(80), +_height_rect(60), +_on(false), +_anim(new QPropertyAnimation(this, "offset_circle", this)) +{ + _radius = _height / 2; + _x_circle = _radius; + _y_circle = _radius; + _y_rect = (_height - _height_rect)/2; +} + +void Toggle::paintEvent(QPaintEvent *e) { + this->setFixedHeight(_height); + QPainter p(this); + p.setPen(Qt::NoPen); + p.setRenderHint(QPainter::Antialiasing, true); + + // Draw toggle background left + p.setBrush(QColor("#33ab4c")); + p.drawRoundedRect(QRect(0, _y_rect, _x_circle + _radius, _height_rect), _height_rect/2, _height_rect/2); + // Draw toggle background right + p.setBrush(QColor("#0a1a26")); + p.drawRoundedRect(QRect(_x_circle - _radius, _y_rect, width() -(_x_circle - _radius), _height_rect), _height_rect/2, _height_rect/2); + + // Draw toggle circle + p.setBrush(QColor("#fafafa")); + p.drawEllipse(QRectF(_x_circle - _radius, _y_circle - _radius, 2 * _radius, 2 * _radius)); +} + +void Toggle::mouseReleaseEvent(QMouseEvent *e) { + if (e->button() & Qt::LeftButton) { + togglePosition(); + emit stateChanged(_on); + } +} + +void Toggle::togglePosition() { + _on = !_on; + const int left = _radius; + const int right = width() - _radius; + _anim->setStartValue(_on ? left : right); + _anim->setEndValue(_on ? right : left); + _anim->setDuration(120); + _anim->start(); +} + +void Toggle::enterEvent(QEvent *e) { + QAbstractButton::enterEvent(e); +} diff --git a/selfdrive/ui/qt/widgets/toggle.hpp b/selfdrive/ui/qt/widgets/toggle.hpp new file mode 100644 index 00000000..98215c25 --- /dev/null +++ b/selfdrive/ui/qt/widgets/toggle.hpp @@ -0,0 +1,35 @@ +#pragma once +#include + +class Toggle : public QAbstractButton { + Q_OBJECT + Q_PROPERTY(int offset_circle READ offset_circle WRITE set_offset_circle) + +public: + Toggle(QWidget* parent = nullptr); + void togglePosition(); + + int offset_circle() const { + return _x_circle; + } + + void set_offset_circle(int o) { + _x_circle = o; + update(); + } + +protected: + void paintEvent(QPaintEvent*) override; + void mouseReleaseEvent(QMouseEvent*) override; + void enterEvent(QEvent*) override; + +private: + bool _on; + int _x_circle, _y_circle; + int _height, _radius; + int _height_rect, _y_rect; + QPropertyAnimation *_anim = nullptr; + +signals: + void stateChanged(int new_state); +}; diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index e5ee59cb..8893ca36 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -1,57 +1,10 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include - #include "window.hpp" -#include "offroad/input_field.hpp" -#include "offroad/settings.hpp" -#include "offroad/onboarding.hpp" - -#include "paint.hpp" -#include "common/util.h" -#include "common/timing.h" - -#define BACKLIGHT_DT 0.25 -#define BACKLIGHT_TS 2.00 - -volatile sig_atomic_t do_exit = 0; - -static void handle_display_state(UIState *s, int dt, bool user_input) { - static int awake_timeout = 0; - awake_timeout = std::max(awake_timeout-dt, 0); - - if (user_input || s->ignition || s->started) { - s->awake = true; - awake_timeout = 30*UI_FREQ; - } else if (awake_timeout == 0){ - s->awake = false; - } -} - -static void set_backlight(int brightness){ - std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness"); - if (brightness_control.is_open()){ - brightness_control << brightness << "\n"; - brightness_control.close(); - } -} MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { main_layout = new QStackedLayout; -#ifdef QCOM2 - set_core_affinity(7); -#endif - - glWindow = new GLWindow(this); - main_layout->addWidget(glWindow); + homeWindow = new HomeWindow(this); + main_layout->addWidget(homeWindow); settingsWindow = new SettingsWindow(this); main_layout->addWidget(settingsWindow); @@ -61,7 +14,7 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { main_layout->setMargin(0); setLayout(main_layout); - QObject::connect(glWindow, SIGNAL(openSettings()), this, SLOT(openSettings())); + QObject::connect(homeWindow, SIGNAL(openSettings()), this, SLOT(openSettings())); QObject::connect(settingsWindow, SIGNAL(closeSettings()), this, SLOT(closeSettings())); // start at onboarding @@ -78,138 +31,16 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { } void MainWindow::openSettings() { - main_layout->setCurrentIndex(1); + main_layout->setCurrentWidget(settingsWindow); } void MainWindow::closeSettings() { - main_layout->setCurrentIndex(0); + main_layout->setCurrentWidget(homeWindow); } - bool MainWindow::eventFilter(QObject *obj, QEvent *event){ if (event->type() == QEvent::MouseButtonPress) { - glWindow->wake(); + homeWindow->glWindow->wake(); } return false; } - - -GLWindow::GLWindow(QWidget *parent) : QOpenGLWidget(parent) { - timer = new QTimer(this); - QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); - - backlight_timer = new QTimer(this); - QObject::connect(backlight_timer, SIGNAL(timeout()), this, SLOT(backlightUpdate())); - - int result = read_param(&brightness_b, "BRIGHTNESS_B", true); - result += read_param(&brightness_m, "BRIGHTNESS_M", true); - if(result != 0) { - brightness_b = 200.0; - brightness_m = 10.0; - } - smooth_brightness = 512; -} - -GLWindow::~GLWindow() { - makeCurrent(); - doneCurrent(); -} - -void GLWindow::initializeGL() { - initializeOpenGLFunctions(); - std::cout << "OpenGL version: " << glGetString(GL_VERSION) << std::endl; - std::cout << "OpenGL vendor: " << glGetString(GL_VENDOR) << std::endl; - std::cout << "OpenGL renderer: " << glGetString(GL_RENDERER) << std::endl; - std::cout << "OpenGL language version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl; - - ui_state = new UIState(); - ui_state->sound = &sound; - ui_state->fb_w = vwp_w; - ui_state->fb_h = vwp_h; - ui_init(ui_state); - - wake(); - - timer->start(0); - backlight_timer->start(BACKLIGHT_DT * 100); -} - -void GLWindow::backlightUpdate(){ - // Update brightness - float k = (BACKLIGHT_DT / BACKLIGHT_TS) / (1.0f + BACKLIGHT_DT / BACKLIGHT_TS); - - float clipped_brightness = std::min(1023.0f, (ui_state->light_sensor*brightness_m) + brightness_b); - smooth_brightness = clipped_brightness * k + smooth_brightness * (1.0f - k); - int brightness = smooth_brightness; - - if (!ui_state->awake){ - brightness = 0; - } - - std::thread{set_backlight, brightness}.detach(); -} - -void GLWindow::timerUpdate(){ -#ifdef QCOM2 - if (ui_state->started != onroad){ - onroad = ui_state->started; - timer->setInterval(onroad ? 0 : 1000); - } -#endif - - // Fix awake timeout if running 1 Hz when offroad - int dt = timer->interval() == 0 ? 1 : 20; - handle_display_state(ui_state, dt, false); - - ui_update(ui_state); - repaint(); -} - -void GLWindow::resizeGL(int w, int h) { - std::cout << "resize " << w << "x" << h << std::endl; -} - -void GLWindow::paintGL() { - ui_draw(ui_state); -} - -void GLWindow::wake(){ - // UI state might not be initialized yet - if (ui_state != nullptr){ - handle_display_state(ui_state, 1, true); - } -} - -void GLWindow::mousePressEvent(QMouseEvent *e) { - wake(); - - // Settings button click - if (!ui_state->scene.uilayout_sidebarcollapsed && settings_btn.ptInRect(e->x(), e->y())) { - emit openSettings(); - } - - // Vision click - if (ui_state->started && (e->x() >= ui_state->scene.viz_rect.x - bdr_s)){ - ui_state->scene.uilayout_sidebarcollapsed = !ui_state->scene.uilayout_sidebarcollapsed; - } -} - - -GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) { - unsigned int texture; - glGenTextures(1, &texture); - glBindTexture(GL_TEXTURE_2D, texture); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, img->width, img->height, 0, GL_RGB, GL_UNSIGNED_BYTE, *pph); - glGenerateMipmap(GL_TEXTURE_2D); - *pkhr = (EGLImageKHR)1; // not NULL - return texture; -} - -void visionimg_destroy_gl(EGLImageKHR khr, void *ph) { - // empty -} - -FramebufferState* framebuffer_init(const char* name, int32_t layer, int alpha, - int *out_w, int *out_h) { - return (FramebufferState*)1; // not null -} diff --git a/selfdrive/ui/qt/window.hpp b/selfdrive/ui/qt/window.hpp index 1f5a4c10..10822f1f 100644 --- a/selfdrive/ui/qt/window.hpp +++ b/selfdrive/ui/qt/window.hpp @@ -1,60 +1,11 @@ #pragma once #include -#include -#include -#include #include -#include "qt/qt_sound.hpp" -#include "ui/ui.hpp" #include "offroad/settings.hpp" #include "offroad/onboarding.hpp" - -#ifdef QCOM2 -const int vwp_w = 2160; -#else -const int vwp_w = 1920; -#endif -const int vwp_h = 1080; - - -class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions { - Q_OBJECT - -public: - using QOpenGLWidget::QOpenGLWidget; - explicit GLWindow(QWidget *parent = 0); - void wake(); - ~GLWindow(); - -protected: - void mousePressEvent(QMouseEvent *e) override; - void initializeGL() override; - void resizeGL(int w, int h) override; - void paintGL() override; - -private: - QTimer * timer; - QTimer * backlight_timer; - - UIState * ui_state = nullptr; - QtSound sound; - - bool onroad = true; - - // TODO: this shouldn't be here - float brightness_b = 0; - float brightness_m = 0; - float smooth_brightness = 0; - -public slots: - void timerUpdate(); - void backlightUpdate(); - -signals: - void openSettings(); -}; +#include "home.hpp" class MainWindow : public QWidget { Q_OBJECT @@ -67,7 +18,7 @@ public: private: QStackedLayout *main_layout; - GLWindow *glWindow; + HomeWindow *homeWindow; SettingsWindow *settingsWindow; OnboardingWindow *onboardingWindow; diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc index 679d347c..65b20bda 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -7,8 +7,7 @@ #include "sidebar.hpp" static void ui_draw_sidebar_background(UIState *s) { - int sbr_x = !s->scene.uilayout_sidebarcollapsed ? 0 : -(sbr_w) + bdr_s * 2; - ui_draw_rect(s->vg, sbr_x, 0, sbr_w, s->fb_h, COLOR_BLACK_ALPHA(85)); + ui_draw_rect(s->vg, 0, 0, sbr_w, s->fb_h, COLOR_BLACK_ALPHA(85)); } static void ui_draw_sidebar_settings_button(UIState *s) { @@ -155,10 +154,10 @@ static void ui_draw_sidebar_connectivity(UIState *s) { } void ui_draw_sidebar(UIState *s) { - ui_draw_sidebar_background(s); if (s->scene.uilayout_sidebarcollapsed) { return; } + ui_draw_sidebar_background(s); ui_draw_sidebar_settings_button(s); ui_draw_sidebar_home_button(s); ui_draw_sidebar_network_strength(s); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index c9f321b0..8240372d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -25,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param } void ui_init(UIState *s) { - s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", + s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "frame", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents"}); s->started = false; @@ -138,7 +138,7 @@ void update_sockets(UIState *s) { s->status = STATUS_WARNING; } else if (alertStatus == cereal::ControlsState::AlertStatus::CRITICAL) { s->status = STATUS_ALERT; - } else{ + } else { s->status = scene.controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } @@ -262,8 +262,8 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; } - // Handle controls timeout - if (s->started && !s->scene.frontview && ((s->sm)->frame - s->started_frame) > 5*UI_FREQ) { + // Handle controls/fcamera timeout + if (s->started && !s->scene.frontview && ((s->sm)->frame - s->started_frame) > 10*UI_FREQ) { if ((s->sm)->rcv_frame("controlsState") < s->started_frame) { // car is started, but controlsState hasn't been seen at all s->scene.alert_text1 = "openpilot Unavailable"; @@ -271,7 +271,8 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::MID; } else if (((s->sm)->frame - (s->sm)->rcv_frame("controlsState")) > 5*UI_FREQ) { // car is started, but controls is lagging or died - if (s->scene.alert_text2 != "Controls Unresponsive") { + if (s->scene.alert_text2 != "Controls Unresponsive" && + s->scene.alert_text1 != "Camera Malfunction") { s->sound->play(AudibleAlert::CHIME_WARNING_REPEAT); LOGE("Controls unresponsive"); } @@ -281,6 +282,18 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::FULL; s->status = STATUS_ALERT; } + + const uint64_t frame_pkt = (s->sm)->rcv_frame("frame"); + const uint64_t frame_delayed = (s->sm)->frame - frame_pkt; + const uint64_t since_started = (s->sm)->frame - s->started_frame; + if ((frame_pkt > s->started_frame || since_started > 15*UI_FREQ) && frame_delayed > 5*UI_FREQ) { + // controls is fine, but rear camera is lagging or died + s->scene.alert_text1 = "Camera Malfunction"; + s->scene.alert_text2 = "Contact Support"; + s->scene.alert_size = cereal::ControlsState::AlertSize::FULL; + s->status = STATUS_DISENGAGED; + s->sound->stop(); + } } // Read params diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 023bc866..b18e51f2 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -7,7 +7,6 @@ #define nvgCreate nvgCreateGL3 #else #include -#include #define NANOVG_GLES3_IMPLEMENTATION #define nvgCreate nvgCreateGLES3 #endif @@ -99,7 +98,6 @@ typedef struct UIScene { bool uilayout_sidebarcollapsed; // responsive layout Rect viz_rect; - int ui_viz_ro; std::string alert_text1; std::string alert_text2; diff --git a/selfdrive/updated.py b/selfdrive/updated.py index bf2427e3..d1eee4d3 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -34,9 +34,9 @@ import threading from pathlib import Path from typing import List, Tuple, Optional -from common.hardware import ANDROID, TICI from common.basedir import BASEDIR from common.params import Params +from selfdrive.hardware import EON, TICI from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.alertmanager import set_offroad_alert @@ -210,11 +210,6 @@ def finalize_update() -> None: shutil.rmtree(FINALIZED) shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True) - # Log git repo corruption - fsck = run(["git", "fsck", "--no-progress"], FINALIZED).rstrip() - if len(fsck): - cloudlog.error(f"found git corruption, git fsck:\n{fsck}") - set_consistent_flag(True) cloudlog.info("done finalizing overlay") @@ -297,7 +292,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool: ] cloudlog.info("git reset success: %s", '\n'.join(r)) - if ANDROID: + if EON: handle_neos_update(wait_helper) # Create the finalized, ready-to-swap update @@ -315,7 +310,11 @@ def main(): if params.get("DisableUpdates") == b"1": raise RuntimeError("updates are disabled by the DisableUpdates param") - if ANDROID and os.geteuid() != 0: + # TODO: remove this after next release + if EON and "letv" not in open("/proc/cmdline").read(): + raise RuntimeError("updates are disabled due to device deprecation") + + if EON and os.geteuid() != 0: raise RuntimeError("updated must be launched as root!") # Set low io priority diff --git a/site_scons/site_tools/cython.py b/site_scons/site_tools/cython.py index ad53f283..45ba797c 100644 --- a/site_scons/site_tools/cython.py +++ b/site_scons/site_tools/cython.py @@ -4,34 +4,35 @@ from SCons.Action import Action cythonAction = Action("$CYTHONCOM") def create_builder(env): - try: - cython = env['BUILDERS']['Cython'] - except KeyError: - cython = SCons.Builder.Builder( - action = cythonAction, - emitter = {}, - suffix = cython_suffix_emitter, - single_source = 1) - env['BUILDERS']['Cython'] = cython - return cython + try: + cython = env['BUILDERS']['Cython'] + except KeyError: + cython = SCons.Builder.Builder( + action = cythonAction, + emitter = {}, + suffix = cython_suffix_emitter, + single_source = 1 + ) + env['BUILDERS']['Cython'] = cython + return cython def cython_suffix_emitter(env, source): - return "$CYTHONCFILESUFFIX" + return "$CYTHONCFILESUFFIX" def generate(env): - env["CYTHON"] = "cythonize" - env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE" - env["CYTHONCFILESUFFIX"] = ".cpp" + env["CYTHON"] = "cythonize" + env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE" + env["CYTHONCFILESUFFIX"] = ".cpp" - c_file, _ = SCons.Tool.createCFileBuilders(env) + c_file, _ = SCons.Tool.createCFileBuilders(env) - c_file.suffix['.pyx'] = cython_suffix_emitter - c_file.add_action('.pyx', cythonAction) + c_file.suffix['.pyx'] = cython_suffix_emitter + c_file.add_action('.pyx', cythonAction) - c_file.suffix['.py'] = cython_suffix_emitter - c_file.add_action('.py', cythonAction) + c_file.suffix['.py'] = cython_suffix_emitter + c_file.add_action('.py', cythonAction) - create_builder(env) + create_builder(env) def exists(env): return True