diff --git a/Jenkinsfile b/Jenkinsfile index df52f59e1..6bd42d13c 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -34,6 +34,9 @@ pipeline { COMMA_JWT = credentials('athena-test-jwt') TEST_DIR = "/data/openpilot" } + options { + timeout(time: 1, unit: 'HOURS') + } stages { @@ -58,7 +61,7 @@ pipeline { when { not { anyOf { - branch 'master-ci'; branch 'devel'; branch 'devel-staging'; branch 'release2'; branch 'release2-staging'; branch 'dashcam'; branch 'dashcam-staging' + branch 'master-ci'; branch 'devel'; branch 'devel-staging'; branch 'release2'; branch 'release2-staging'; branch 'dashcam'; branch 'dashcam-staging'; branch 'testing-closet*' } } } @@ -133,6 +136,7 @@ pipeline { ["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"], + //["test camerad", "CI=1 python selfdrive/camerad/test/test_camerad.py"], // wait for shelf refactor //["test updater", "python installer/updater/test_updater.py"], ]) } diff --git a/README.md b/README.md index 17a095252..772272183 100644 --- a/README.md +++ b/README.md @@ -66,6 +66,7 @@ Supported Cars | ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------| | Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 25mph | | Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 12mph | +| Acura | RDX 2020 | AcuraWatch | 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 | @@ -109,7 +110,7 @@ Supported Cars | Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph | | Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph | | Toyota | Rav4 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | -| Toyota | Rav4 2019-20 | All | openpilot | 0mph | 0mph | +| Toyota | Rav4 2019-21 | All | openpilot | 0mph | 0mph | | Toyota | Rav4 Hybrid 2016-18 | TSS-P | Stock3| 0mph | 0mph | | Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph | | Toyota | Sienna 2018-20 | All | Stock3| 0mph | 0mph | @@ -143,7 +144,7 @@ Community Maintained Cars and Features | 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 | -| Hyundai | Sonata 2019 | All | Stock | 0mph | 0mph | +| Hyundai | Sonata 2019 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Veloster 2019 | SCC + LKAS | Stock | 5mph | 0mph | | Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | | Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | @@ -152,8 +153,8 @@ Community Maintained Cars and Features | Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Sorento 2018 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph | -| Nissan | Leaf 2018-19 | ProPILOT | Stock | 0mph | 0mph | -| Nissan | Rogue 2019 | ProPILOT | Stock | 0mph | 0mph | +| Nissan | Leaf 2018-20 | ProPILOT | Stock | 0mph | 0mph | +| Nissan | Rogue 2018-19 | ProPILOT | Stock | 0mph | 0mph | | Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph | | Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | | Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | @@ -268,7 +269,7 @@ Safety and Testing Testing on PC ------ -For simplified development and experimentation, openpilot runs in the CARLA driving simulator, which allows you to develop openpilot without a car. +For simplified development and experimentation, openpilot can be run in the CARLA driving simulator, which allows you to develop openpilot without a car. The whole setup should only take a few minutes. Steps: 1) Start the CARLA server on first terminal @@ -309,7 +310,7 @@ Directory Structure ├── phonelibs # Libraries used on NEOS devices ├── pyextra # Libraries used on NEOS devices └── selfdrive # Code needed to drive the car - ├── assets # Fonts, images, and sounds for UI + ├── assets # Fonts, images and sounds for UI ├── athena # Allows communication with the app ├── boardd # Daemon to talk to the board ├── camerad # Driver to capture images from the camera sensors diff --git a/RELEASES.md b/RELEASES.md index e8a157567..8149ce4da 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,12 @@ +Version 0.7.10 (2020-10-26) +======================== + * NEOS update: update to Python 3.8.2 and lower CPU frequency + * Improved thermals due to reduced CPU frequency + * Update SNPE to 1.41.0 + * Reduced offroad power consumption + * Various system stability improvements + * Acura RDX 2020 support thanks to csouers! + Version 0.7.9 (2020-10-09) ======================== * Improved car battery power management @@ -29,24 +38,24 @@ Version 0.7.7 (2020-07-20) Version 0.7.6.1 (2020-06-16) ======================== -* Hotfix: update kernel on some comma twos (orders #8570-#8680) + * Hotfix: update kernel on some comma twos (orders #8570-#8680) Version 0.7.6 (2020-06-05) ======================== -* White panda is deprecated, upgrade to comma two or black panda -* 2017 Nissan X-Trail, 2018-19 Leaf and 2019 Rogue support thanks to avolmensky! -* 2017 Mazda CX-5 support in dashcam mode thanks to Jafaral! -* Huge CPU savings in modeld by using thneed! -* Lots of code cleanup and refactors + * White panda is deprecated, upgrade to comma two or black panda + * 2017 Nissan X-Trail, 2018-19 Leaf and 2019 Rogue support thanks to avolmensky! + * 2017 Mazda CX-5 support in dashcam mode thanks to Jafaral! + * Huge CPU savings in modeld by using thneed! + * Lots of code cleanup and refactors Version 0.7.5 (2020-05-13) ======================== -* Right-Hand Drive support for both driving and driver monitoring! -* New driving model: improved at sharp turns and lead speed estimation -* New driver monitoring model: overall improvement on comma two -* Driver camera preview in settings to improve mounting position -* Added support for many Hyundai, Kia, Genesis models thanks to xx979xx! -* Improved lateral tuning for 2020 Toyota Rav 4 (hybrid) + * Right-Hand Drive support for both driving and driver monitoring! + * New driving model: improved at sharp turns and lead speed estimation + * New driver monitoring model: overall improvement on comma two + * Driver camera preview in settings to improve mounting position + * Added support for many Hyundai, Kia, Genesis models thanks to xx979xx! + * Improved lateral tuning for 2020 Toyota Rav 4 (hybrid) Version 0.7.4 (2020-03-20) ======================== @@ -482,96 +491,96 @@ Version 0.3.4 (2017-07-28) Version 0.3.3 (2017-06-28) =========================== - * Improved model trained on more data - * Alpha CR-V support thanks to energee and johnnwvs! - * Using the opendbc project for DBC files - * Minor performance improvements - * UI update thanks to pjlao307 - * Power off button - * 6% more torque on the Civic + * Improved model trained on more data + * Alpha CR-V support thanks to energee and johnnwvs! + * Using the opendbc project for DBC files + * Minor performance improvements + * UI update thanks to pjlao307 + * Power off button + * 6% more torque on the Civic Version 0.3.2 (2017-05-22) =========================== - * Minor stability bugfixes - * Added metrics and rear view mirror disable to settings - * Update model with more crowdsourced data + * Minor stability bugfixes + * Added metrics and rear view mirror disable to settings + * Update model with more crowdsourced data Version 0.3.1 (2017-05-17) =========================== - * visiond stability bugfix - * Add logging for angle and flashing + * visiond stability bugfix + * Add logging for angle and flashing Version 0.3.0 (2017-05-12) =========================== - * Add CarParams struct to improve the abstraction layer - * Refactor visiond IPC to support multiple clients - * Add raw GPS and beginning support for navigation - * Improve model in visiond using crowdsourced data - * Add improved system logging to diagnose instability - * Rewrite baseui in React Native - * Moved calibration to the cloud + * Add CarParams struct to improve the abstraction layer + * Refactor visiond IPC to support multiple clients + * Add raw GPS and beginning support for navigation + * Improve model in visiond using crowdsourced data + * Add improved system logging to diagnose instability + * Rewrite baseui in React Native + * Moved calibration to the cloud Version 0.2.9 (2017-03-01) =========================== - * Retain compatibility with NEOS v1 + * Retain compatibility with NEOS v1 Version 0.2.8 (2017-02-27) =========================== - * Fix bug where frames were being dropped in minute 71 + * Fix bug where frames were being dropped in minute 71 Version 0.2.7 (2017-02-08) =========================== - * Better performance and pictures at night - * Fix ptr alignment issue in boardd - * Fix brake error light, fix crash if too cold + * Better performance and pictures at night + * Fix ptr alignment issue in boardd + * Fix brake error light, fix crash if too cold Version 0.2.6 (2017-01-31) =========================== - * Fix bug in visiond model execution + * Fix bug in visiond model execution Version 0.2.5 (2017-01-30) =========================== - * Fix race condition in manager + * Fix race condition in manager Version 0.2.4 (2017-01-27) =========================== - * OnePlus 3T support - * Enable installation as NEOS app - * Various minor bugfixes + * OnePlus 3T support + * Enable installation as NEOS app + * Various minor bugfixes Version 0.2.3 (2017-01-11) =========================== - * Reduce space usage by 80% - * Add better logging - * Add Travis CI + * Reduce space usage by 80% + * Add better logging + * Add Travis CI Version 0.2.2 (2017-01-10) =========================== - * Board triggers started signal on CAN messages - * Improved autoexposure - * Handle out of space, improve upload status + * Board triggers started signal on CAN messages + * Improved autoexposure + * Handle out of space, improve upload status Version 0.2.1 (2016-12-14) =========================== - * Performance improvements, removal of more numpy - * Fix boardd process priority - * Make counter timer reset on use of steering wheel + * Performance improvements, removal of more numpy + * Fix boardd process priority + * Make counter timer reset on use of steering wheel Version 0.2 (2016-12-12) ========================= - * Car/Radar abstraction layers have shipped, see cereal/car.capnp - * controlsd has been refactored - * Shipped plant model and testing maneuvers - * visiond exits more gracefully now - * Hardware encoder in visiond should always init - * ui now turns off the screen after 30 seconds - * Switch to openpilot release branch for future releases - * Added preliminary Docker container to run tests on PC + * Car/Radar abstraction layers have shipped, see cereal/car.capnp + * controlsd has been refactored + * Shipped plant model and testing maneuvers + * visiond exits more gracefully now + * Hardware encoder in visiond should always init + * ui now turns off the screen after 30 seconds + * Switch to openpilot release branch for future releases + * Added preliminary Docker container to run tests on PC Version 0.1 (2016-11-29) ========================= - * Initial release of openpilot - * Adaptive cruise control is working - * Lane keep assist is working - * Support for Acura ILX 2016 with AcuraWatch Plus - * Support for Honda Civic 2016 Touring Edition + * Initial release of openpilot + * Adaptive cruise control is working + * Lane keep assist is working + * Support for Acura ILX 2016 with AcuraWatch Plus + * Support for Honda Civic 2016 Touring Edition diff --git a/SConstruct b/SConstruct index fc119262b..1b863beba 100644 --- a/SConstruct +++ b/SConstruct @@ -7,6 +7,7 @@ import sys import platform TICI = os.path.isfile('/TICI') +Decider('MD5-timestamp') AddOption('--test', action='store_true', @@ -27,7 +28,7 @@ if platform.system() == "Darwin": if arch == "aarch64" and TICI: arch = "larch64" -webcam = bool(ARGUMENTS.get("use_webcam", 0)) +USE_WEBCAM = os.getenv("USE_WEBCAM") is not None QCOM_REPLAY = arch == "aarch64" and os.getenv("QCOM_REPLAY") is not None if arch == "aarch64" or arch == "larch64": @@ -108,6 +109,7 @@ else: ] rpath = [ + "phonelibs/snpe/x86_64-linux-clang", "external/tensorflow/lib", "cereal", "selfdrive/common" @@ -136,6 +138,7 @@ env = Environment( "-Werror", "-Wno-unknown-warning-option", "-Wno-deprecated-register", + "-Wno-register", "-Wno-inconsistent-missing-override", "-Wno-c99-designator", "-Wno-reorder-init-list", @@ -175,7 +178,7 @@ env = Environment( RPATH=rpath, CFLAGS=["-std=gnu11"] + cflags, - CXXFLAGS=["-std=c++14"] + cxxflags, + CXXFLAGS=["-std=c++1z"] + cxxflags, LIBPATH=libpath + [ "#cereal", "#selfdrive/common", @@ -200,6 +203,7 @@ if arch in ["x86_64", "Darwin", "larch64"]: ] qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"] else: + qt_env['QTDIR'] = "/usr" qt_dirs = [ f"/usr/include/{real_arch}-linux-gnu/qt5", f"/usr/include/{real_arch}-linux-gnu/qt5/QtWidgets", @@ -259,7 +263,7 @@ def abspath(x): # still needed for apks zmq = 'zmq' -Export('env', 'qt_env', 'arch', 'zmq', 'SHARED', 'webcam', 'QCOM_REPLAY') +Export('env', 'qt_env', 'arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY') # cereal and messaging are shared with the system SConscript(['cereal/SConscript']) diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 237b0b367..f6d37b3e4 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/car.capnp b/cereal/car.capnp index 17bc461b3..558107a44 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -25,7 +25,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { canError @0; steerUnavailable @1; brakeUnavailable @2; - gasUnavailable @3; wrongGear @4; doorOpen @5; seatbeltNotLatched @6; @@ -98,12 +97,12 @@ struct CarEvent @0x9b1657f34caf3ad3 { whitePandaUnsupported @81; belowEngageSpeed @84; noGps @85; - focusRecoverActive @86; wrongCruiseMode @87; - neosUpdateRequired @88; modeldLagging @89; deviceFalling @90; + fanMalfunction @91; + gasUnavailableDEPRECATED @3; dataNeededDEPRECATED @16; modelCommIssueDEPRECATED @27; ipasOverrideDEPRECATED @33; @@ -114,6 +113,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { invalidGiraffeHondaDEPRECATED @49; canErrorPersistentDEPRECATED @83; startupWhitePandaDEPRECATED @82; + focusRecoverActiveDEPRECATED @86; + neosUpdateRequiredDEPRECATED @88; } } diff --git a/cereal/log.capnp b/cereal/log.capnp index c8419500a..ac86bb025 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -571,7 +571,7 @@ struct ControlsState @0x97ff69c53601abf1 { enum AlertStatus { normal @0; # low priority alert for user's convenience - userPrompt @1; # mid piority alert that might require user intervention + userPrompt @1; # mid priority alert that might require user intervention critical @2; # high priority alert that needs immediate user intervention } @@ -1983,6 +1983,7 @@ struct Boot { wallTimeNanos @0 :UInt64; lastKmsg @1 :Data; lastPmsg @2 :Data; + launchLog @3 :Text; } struct LiveParametersData { diff --git a/cereal/messaging/messaging_pyx.pyx b/cereal/messaging/messaging_pyx.pyx index 1bb407315..8b3e84743 100644 --- a/cereal/messaging/messaging_pyx.pyx +++ b/cereal/messaging/messaging_pyx.pyx @@ -140,9 +140,9 @@ cdef class PubSocket: else: raise MessagingError - def send(self, string data): + def send(self, bytes data): length = len(data) - r = self.socket.send(data.c_str(), length) + r = self.socket.send(data, length) if r != length: if errno.errno == errno.EADDRINUSE: diff --git a/cereal/messaging/messaging_pyx_setup.py b/cereal/messaging/messaging_pyx_setup.py index 8e67a104c..43dc9c987 100644 --- a/cereal/messaging/messaging_pyx_setup.py +++ b/cereal/messaging/messaging_pyx_setup.py @@ -31,7 +31,7 @@ class BuildExtWithoutPlatformSuffix(build_ext): sourcefiles = ['messaging_pyx.pyx'] -extra_compile_args = ["-std=c++14", "-Wno-nullability-completeness"] +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 diff --git a/cereal/messaging/msgq.cc b/cereal/messaging/msgq.cc index 53bdb9af0..30b0225cd 100644 --- a/cereal/messaging/msgq.cc +++ b/cereal/messaging/msgq.cc @@ -114,6 +114,7 @@ int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){ int rc = ftruncate(fd, size + sizeof(msgq_header_t)); if (rc < 0){ + close(fd); return -1; } char * mem = (char*)mmap(NULL, size + sizeof(msgq_header_t), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); diff --git a/common/SConscript b/common/SConscript index 35abeb9ee..d17b35fed 100644 --- a/common/SConscript +++ b/common/SConscript @@ -4,3 +4,11 @@ Import('env', 'cython_dependencies') env.Command(['common_pyx.so', 'clock.cpp'], cython_dependencies + ['common_pyx_setup.py', 'clock.pyx'], "cd common && python3 common_pyx_setup.py build_ext --inplace") + +# Build cython params module +env.Command(['params_pyx.so', 'params_pyx.cpp'], + cython_dependencies + [ + 'params_pyx_setup.py', 'params_pyx.pyx', 'params_pxd.pxd', + '#selfdrive/common/params.cc', '#selfdrive/common/params.h', + '#selfdrive/common/util.c', '#selfdrive/common/util.h'], + "cd common && python3 params_pyx_setup.py build_ext --inplace") diff --git a/common/apk.py b/common/apk.py index c04c19aa5..275cdefed 100644 --- a/common/apk.py +++ b/common/apk.py @@ -31,9 +31,17 @@ def start_offroad(): system("am start -n ai.comma.plus.offroad/.MainActivity") def set_package_permissions(): - pm_grant("ai.comma.plus.offroad", "android.permission.ACCESS_FINE_LOCATION") - pm_grant("ai.comma.plus.offroad", "android.permission.READ_PHONE_STATE") - pm_grant("ai.comma.plus.offroad", "android.permission.READ_EXTERNAL_STORAGE") + try: + output = subprocess.check_output(['dumpsys', 'package', 'ai.comma.plus.offroad'], encoding="utf-8") + given_permissions = output.split("runtime permissions")[1] + except Exception: + given_permissions = "" + + wanted_permissions = ["ACCESS_FINE_LOCATION", "READ_PHONE_STATE", "READ_EXTERNAL_STORAGE"] + for permission in wanted_permissions: + if permission not in given_permissions: + pm_grant("ai.comma.plus.offroad", "android.permission."+permission) + appops_set("ai.comma.plus.offroad", "SU", "allow") appops_set("ai.comma.plus.offroad", "WIFI_SCAN", "allow") diff --git a/common/basedir.py b/common/basedir.py index d98509ed6..a131f0d1a 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -4,7 +4,5 @@ BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__ from common.hardware import PC if PC: PERSIST = os.path.join(BASEDIR, "persist") - PARAMS = os.path.join(BASEDIR, "persist", "params") else: PERSIST = "/persist" - PARAMS = "/data/params" diff --git a/common/clock.pyx b/common/clock.pyx index 654d53f8f..9702e488c 100644 --- a/common/clock.pyx +++ b/common/clock.pyx @@ -1,3 +1,4 @@ +# cython: language_level = 3 from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t IF UNAME_SYSNAME == "Darwin": diff --git a/common/common_pyx_setup.py b/common/common_pyx_setup.py index 9d0f0fec8..d8f653e67 100644 --- a/common/common_pyx_setup.py +++ b/common/common_pyx_setup.py @@ -4,9 +4,9 @@ from Cython.Build import cythonize from common.cython_hacks import BuildExtWithoutPlatformSuffix sourcefiles = ['clock.pyx'] -extra_compile_args = ["-std=c++11"] +extra_compile_args = ["-std=c++1z"] -setup(name='Common', +setup(name='common', cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, ext_modules=cythonize( Extension( @@ -14,7 +14,7 @@ setup(name='Common', language="c++", sources=sourcefiles, extra_compile_args=extra_compile_args, - ) + ), + nthreads=4, ), - nthreads=4, ) diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py index 0fbe16296..e6ee2c23f 100644 --- a/common/ffi_wrapper.py +++ b/common/ffi_wrapper.py @@ -44,7 +44,7 @@ def compile_code(name, c_code, c_header, directory, cflags="", libraries=None): ffibuilder = FFI() ffibuilder.set_source(name, c_code, source_extension='.cpp', libraries=libraries) ffibuilder.cdef(c_header) - os.environ['OPT'] = "-fwrapv -O2 -DNDEBUG -std=c++11" + os.environ['OPT'] = "-fwrapv -O2 -DNDEBUG -std=c++1z" os.environ['CFLAGS'] = cflags ffibuilder.compile(verbose=True, debug=False, tmpdir=directory) diff --git a/common/hardware_tici.py b/common/hardware_tici.py index 223bfc1cc..2212edd48 100644 --- a/common/hardware_tici.py +++ b/common/hardware_tici.py @@ -2,6 +2,7 @@ import serial from common.hardware_base import HardwareBase from cereal import log +import subprocess NetworkType = log.ThermalData.NetworkType @@ -40,7 +41,7 @@ class Tici(HardwareBase): return "" def reboot(self, reason=None): - print("REBOOT!") + subprocess.check_output(["sudo", "reboot"]) def get_network_type(self): return NetworkType.wifi diff --git a/common/kalman/simple_kalman_impl.pxd b/common/kalman/simple_kalman_impl.pxd index c81f95927..cb39a45bc 100644 --- a/common/kalman/simple_kalman_impl.pxd +++ b/common/kalman/simple_kalman_impl.pxd @@ -1,3 +1,5 @@ +# cython: language_level = 3 + cdef class KF1D: cdef public: double x0_0 @@ -13,4 +15,4 @@ cdef class KF1D: double A_K_0 double A_K_1 double A_K_2 - double A_K_3 \ No newline at end of file + double A_K_3 diff --git a/common/kalman/simple_kalman_setup.py b/common/kalman/simple_kalman_setup.py index fb4886a0f..d0781c9ff 100644 --- a/common/kalman/simple_kalman_setup.py +++ b/common/kalman/simple_kalman_setup.py @@ -6,4 +6,5 @@ from common.cython_hacks import BuildExtWithoutPlatformSuffix setup(name='Simple Kalman Implementation', cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize(Extension("simple_kalman_impl", ["simple_kalman_impl.pyx"]))) + ext_modules=cythonize(Extension("simple_kalman_impl", + ["simple_kalman_impl.pyx"]))) diff --git a/common/params.py b/common/params.py old mode 100755 new mode 100644 index d30f2d0df..5a7a0d410 --- a/common/params.py +++ b/common/params.py @@ -1,409 +1,4 @@ -#!/usr/bin/env python3 -"""ROS has a parameter server, we have files. - -The parameter store is a persistent key value store, implemented as a directory with a writer lock. -On Android, we store params under params_dir = /data/params. The writer lock is a file -"/.lock" taken using flock(), and data is stored in a directory symlinked to by -"/d". - -Each key, value pair is stored as a file with named with contents , located in - /d/ - -Readers of a single key can just open("/d/") and read the file contents. -Readers who want a consistent snapshot of multiple keys should take the lock. - -Writers should take the lock before modifying anything. Writers should also leave the DB in a -consistent state after a crash. The implementation below does this by copying all params to a temp -directory /, then atomically symlinking / to / -before deleting the old / directory. - -Writers that only modify a single key can simply take the lock, then swap the corresponding value -file in place without messing with /d. -""" -import time -import os -import errno -import shutil -import fcntl -import tempfile -import threading -from enum import Enum -from common.basedir import PARAMS - - -def mkdirs_exists_ok(path): - try: - os.makedirs(path) - except OSError: - if not os.path.isdir(path): - raise - - -class TxType(Enum): - PERSISTENT = 1 - CLEAR_ON_MANAGER_START = 2 - CLEAR_ON_PANDA_DISCONNECT = 3 - - -class UnknownKeyName(Exception): - pass - - -keys = { - "AccessToken": [TxType.CLEAR_ON_MANAGER_START], - "AthenadPid": [TxType.PERSISTENT], - "CalibrationParams": [TxType.PERSISTENT], - "CarBatteryCapacity": [TxType.PERSISTENT], - "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "CommunityFeaturesToggle": [TxType.PERSISTENT], - "CompletedTrainingVersion": [TxType.PERSISTENT], - "DisablePowerDown": [TxType.PERSISTENT], - "DisableUpdates": [TxType.PERSISTENT], - "DoUninstall": [TxType.CLEAR_ON_MANAGER_START], - "DongleId": [TxType.PERSISTENT], - "GitBranch": [TxType.PERSISTENT], - "GitCommit": [TxType.PERSISTENT], - "GitRemote": [TxType.PERSISTENT], - "GithubSshKeys": [TxType.PERSISTENT], - "HasAcceptedTerms": [TxType.PERSISTENT], - "HasCompletedSetup": [TxType.PERSISTENT], - "IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START], - "IsLdwEnabled": [TxType.PERSISTENT], - "IsMetric": [TxType.PERSISTENT], - "IsOffroad": [TxType.CLEAR_ON_MANAGER_START], - "IsRHD": [TxType.PERSISTENT], - "IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], - "IsUpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], - "IsUploadRawEnabled": [TxType.PERSISTENT], - "LastAthenaPingTime": [TxType.PERSISTENT], - "LastUpdateTime": [TxType.PERSISTENT], - "LastUpdateException": [TxType.PERSISTENT], - "LiveParameters": [TxType.PERSISTENT], - "OpenpilotEnabledToggle": [TxType.PERSISTENT], - "LaneChangeEnabled": [TxType.PERSISTENT], - "PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "Passive": [TxType.PERSISTENT], - "RecordFront": [TxType.PERSISTENT], - "ReleaseNotes": [TxType.PERSISTENT], - "ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], - "SubscriberInfo": [TxType.PERSISTENT], - "TermsVersion": [TxType.PERSISTENT], - "TrainingVersion": [TxType.PERSISTENT], - "UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], - "UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START], - "Version": [TxType.PERSISTENT], - "Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START], - "Offroad_ConnectivityNeededPrompt": [TxType.CLEAR_ON_MANAGER_START], - "Offroad_TemperatureTooHigh": [TxType.CLEAR_ON_MANAGER_START], - "Offroad_PandaFirmwareMismatch": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], - "Offroad_InvalidTime": [TxType.CLEAR_ON_MANAGER_START], - "Offroad_IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], - "Offroad_NeosUpdate": [TxType.CLEAR_ON_MANAGER_START], - "Offroad_UpdateFailed": [TxType.CLEAR_ON_MANAGER_START], -} - - -def fsync_dir(path): - fd = os.open(path, os.O_RDONLY) - try: - os.fsync(fd) - finally: - os.close(fd) - - -class FileLock(): - def __init__(self, path, create, lock_ex): - self._path = path - self._create = create - self._fd = None - self._lock_ex = lock_ex - - def acquire(self): - self._fd = os.open(self._path, os.O_CREAT if self._create else 0) - fcntl.flock(self._fd, fcntl.LOCK_EX if self._lock_ex else fcntl.LOCK_SH) - - def release(self): - if self._fd is not None: - os.close(self._fd) - self._fd = None - - -class DBAccessor(): - def __init__(self, path): - self._path = path - self._vals = None - - def keys(self): - self._check_entered() - return self._vals.keys() - - def get(self, key): - self._check_entered() - - if self._vals is None: - return None - - try: - return self._vals[key] - except KeyError: - return None - - def _get_lock(self, create, lock_ex): - lock = FileLock(os.path.join(self._path, ".lock"), create, lock_ex) - lock.acquire() - return lock - - def _read_values_locked(self): - """Callers should hold a lock while calling this method.""" - vals = {} - try: - data_path = self._data_path() - keys = os.listdir(data_path) - for key in keys: - with open(os.path.join(data_path, key), "rb") as f: - vals[key] = f.read() - except (OSError, IOError) as e: - # Either the DB hasn't been created yet, or somebody wrote a bug and left the DB in an - # inconsistent state. Either way, return empty. - if e.errno == errno.ENOENT: - return {} - - return vals - - def _data_path(self): - return os.path.join(self._path, "d") - - def _check_entered(self): - if self._vals is None: - raise Exception("Must call __enter__ before using DB") - - -class DBReader(DBAccessor): - def __enter__(self): - try: - lock = self._get_lock(False, False) - except OSError as e: - # Do not create lock if it does not exist. - if e.errno == errno.ENOENT: - self._vals = {} - return self - - try: - # Read everything. - self._vals = self._read_values_locked() - return self - finally: - lock.release() - - def __exit__(self, exc_type, exc_value, traceback): - pass - - -class DBWriter(DBAccessor): - def __init__(self, path): - super(DBWriter, self).__init__(path) - self._lock = None - self._prev_umask = None - - def put(self, key, value): - self._vals[key] = value - - def delete(self, key): - self._vals.pop(key, None) - - def __enter__(self): - mkdirs_exists_ok(self._path) - - # Make sure we can write and that permissions are correct. - self._prev_umask = os.umask(0) - - try: - os.chmod(self._path, 0o777) - self._lock = self._get_lock(True, True) - self._vals = self._read_values_locked() - except Exception: - os.umask(self._prev_umask) - self._prev_umask = None - raise - - return self - - def __exit__(self, exc_type, exc_value, traceback): - self._check_entered() - - try: - # data_path refers to the externally used path to the params. It is a symlink. - # old_data_path is the path currently pointed to by data_path. - # tempdir_path is a path where the new params will go, which the new data path will point to. - # new_data_path is a temporary symlink that will atomically overwrite data_path. - # - # The current situation is: - # data_path -> old_data_path - # We're going to write params data to tempdir_path - # tempdir_path -> params data - # Then point new_data_path to tempdir_path - # new_data_path -> tempdir_path - # Then atomically overwrite data_path with new_data_path - # data_path -> tempdir_path - old_data_path = None - new_data_path = None - tempdir_path = tempfile.mkdtemp(prefix=".tmp", dir=self._path) - - try: - # Write back all keys. - os.chmod(tempdir_path, 0o777) - for k, v in self._vals.items(): - with open(os.path.join(tempdir_path, k), "wb") as f: - f.write(v) - f.flush() - os.fsync(f.fileno()) - fsync_dir(tempdir_path) - - data_path = self._data_path() - try: - old_data_path = os.path.join(self._path, os.readlink(data_path)) - except (OSError, IOError): - # NOTE(mgraczyk): If other DB implementations have bugs, this could cause - # copies to be left behind, but we still want to overwrite. - pass - - new_data_path = "{}.link".format(tempdir_path) - os.symlink(os.path.basename(tempdir_path), new_data_path) - os.rename(new_data_path, data_path) - fsync_dir(self._path) - finally: - # If the rename worked, we can delete the old data. Otherwise delete the new one. - success = new_data_path is not None and os.path.exists(data_path) and ( - os.readlink(data_path) == os.path.basename(tempdir_path)) - - if success: - if old_data_path is not None: - shutil.rmtree(old_data_path) - else: - shutil.rmtree(tempdir_path) - - # Regardless of what happened above, there should be no link at new_data_path. - if new_data_path is not None and os.path.islink(new_data_path): - os.remove(new_data_path) - finally: - os.umask(self._prev_umask) - self._prev_umask = None - - # Always release the lock. - self._lock.release() - self._lock = None - - -def read_db(params_path, key): - path = "%s/d/%s" % (params_path, key) - try: - with open(path, "rb") as f: - return f.read() - except IOError: - return None - - -def write_db(params_path, key, value): - if isinstance(value, str): - value = value.encode('utf8') - - prev_umask = os.umask(0) - lock = FileLock(params_path + "/.lock", True, True) - lock.acquire() - - try: - tmp_path = tempfile.NamedTemporaryFile(mode="wb", prefix=".tmp", dir=params_path, delete=False) - with tmp_path as f: - f.write(value) - f.flush() - os.fsync(f.fileno()) - os.chmod(tmp_path.name, 0o666) - - path = "%s/d/%s" % (params_path, key) - os.rename(tmp_path.name, path) - fsync_dir(os.path.dirname(path)) - finally: - os.umask(prev_umask) - lock.release() - - -class Params(): - def __init__(self, db=PARAMS): - self.db = db - - # create the database if it doesn't exist... - if not os.path.exists(self.db + "/d"): - with self.transaction(write=True): - pass - - def clear_all(self): - shutil.rmtree(self.db, ignore_errors=True) - with self.transaction(write=True): - pass - - def transaction(self, write=False): - if write: - return DBWriter(self.db) - else: - return DBReader(self.db) - - def _clear_keys_with_type(self, tx_type): - with self.transaction(write=True) as txn: - for key in keys: - if tx_type in keys[key]: - txn.delete(key) - - def manager_start(self): - self._clear_keys_with_type(TxType.CLEAR_ON_MANAGER_START) - - def panda_disconnect(self): - self._clear_keys_with_type(TxType.CLEAR_ON_PANDA_DISCONNECT) - - def delete(self, key): - with self.transaction(write=True) as txn: - txn.delete(key) - - def get(self, key, block=False, encoding=None): - if key not in keys: - raise UnknownKeyName(key) - - while 1: - ret = read_db(self.db, key) - if not block or ret is not None: - break - # is polling really the best we can do? - time.sleep(0.05) - - if ret is not None and encoding is not None: - ret = ret.decode(encoding) - - return ret - - def put(self, key, dat): - """ - Warning: This function blocks until the param is written to disk! - In very rare cases this can take over a second, and your code will hang. - - Use the put_nonblocking helper function in time sensitive code, but - in general try to avoid writing params as much as possible. - """ - - if key not in keys: - raise UnknownKeyName(key) - - write_db(self.db, key, dat) - - -def put_nonblocking(key, val): - def f(key, val): - params = Params() - params.put(key, val) - - t = threading.Thread(target=f, args=(key, val)) - t.start() - return t +from common.params_pyx import Params, UnknownKeyName, put_nonblocking # pylint: disable=no-name-in-module, import-error +assert Params +assert UnknownKeyName +assert put_nonblocking diff --git a/common/params_pxd.pxd b/common/params_pxd.pxd new file mode 100644 index 000000000..e437a09b0 --- /dev/null +++ b/common/params_pxd.pxd @@ -0,0 +1,16 @@ +from libcpp.string cimport string +from libcpp cimport bool + +cdef extern from "selfdrive/common/params.cc": + pass + +cdef extern from "selfdrive/common/util.c": + pass + +cdef extern from "selfdrive/common/params.h": + cdef cppclass Params: + Params(bool) + Params(string) + string get(string, bool) nogil + int delete_db_value(string) + int write_db_value(string, string) diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx new file mode 100755 index 000000000..75ebacb3f --- /dev/null +++ b/common/params_pyx.pyx @@ -0,0 +1,160 @@ +# distutils: language = c++ +# cython: language_level = 3 +from libcpp cimport bool +from libcpp.string cimport string +from params_pxd cimport Params as c_Params + +import os +import threading +from common.basedir import BASEDIR + +cdef enum TxType: + PERSISTENT = 1 + CLEAR_ON_MANAGER_START = 2 + CLEAR_ON_PANDA_DISCONNECT = 3 + +keys = { + b"AccessToken": [TxType.CLEAR_ON_MANAGER_START], + b"AthenadPid": [TxType.PERSISTENT], + b"CalibrationParams": [TxType.PERSISTENT], + b"CarBatteryCapacity": [TxType.PERSISTENT], + b"CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"CommunityFeaturesToggle": [TxType.PERSISTENT], + b"CompletedTrainingVersion": [TxType.PERSISTENT], + b"DisablePowerDown": [TxType.PERSISTENT], + b"DisableUpdates": [TxType.PERSISTENT], + b"DoUninstall": [TxType.CLEAR_ON_MANAGER_START], + b"DongleId": [TxType.PERSISTENT], + b"GitBranch": [TxType.PERSISTENT], + b"GitCommit": [TxType.PERSISTENT], + b"GitRemote": [TxType.PERSISTENT], + b"GithubSshKeys": [TxType.PERSISTENT], + b"HasAcceptedTerms": [TxType.PERSISTENT], + b"HasCompletedSetup": [TxType.PERSISTENT], + b"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START], + b"IsLdwEnabled": [TxType.PERSISTENT], + b"IsMetric": [TxType.PERSISTENT], + b"IsOffroad": [TxType.CLEAR_ON_MANAGER_START], + b"IsRHD": [TxType.PERSISTENT], + b"IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], + b"IsUpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], + b"IsUploadRawEnabled": [TxType.PERSISTENT], + b"LastAthenaPingTime": [TxType.PERSISTENT], + b"LastUpdateTime": [TxType.PERSISTENT], + b"LastUpdateException": [TxType.PERSISTENT], + b"LiveParameters": [TxType.PERSISTENT], + b"OpenpilotEnabledToggle": [TxType.PERSISTENT], + b"LaneChangeEnabled": [TxType.PERSISTENT], + b"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"Passive": [TxType.PERSISTENT], + b"RecordFront": [TxType.PERSISTENT], + b"ReleaseNotes": [TxType.PERSISTENT], + b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], + b"SubscriberInfo": [TxType.PERSISTENT], + b"TermsVersion": [TxType.PERSISTENT], + b"TrainingVersion": [TxType.PERSISTENT], + b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], + b"UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START], + b"Version": [TxType.PERSISTENT], + b"Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START], + b"Offroad_ConnectivityNeededPrompt": [TxType.CLEAR_ON_MANAGER_START], + b"Offroad_TemperatureTooHigh": [TxType.CLEAR_ON_MANAGER_START], + b"Offroad_PandaFirmwareMismatch": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + b"Offroad_InvalidTime": [TxType.CLEAR_ON_MANAGER_START], + b"Offroad_IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], + b"Offroad_NeosUpdate": [TxType.CLEAR_ON_MANAGER_START], + b"Offroad_UpdateFailed": [TxType.CLEAR_ON_MANAGER_START], +} + +def ensure_bytes(v): + if isinstance(v, str): + return v.encode() + else: + return v + + +class UnknownKeyName(Exception): + pass + +cdef class Params: + cdef c_Params* p + + def __cinit__(self, d=None, bool persistent_params=False): + if d is None: + self.p = new c_Params(persistent_params) + else: + self.p = new c_Params(d.encode()) + + def __dealloc__(self): + del self.p + + def clear_all(self, tx_type=None): + for key in keys: + if tx_type is None or tx_type in keys[key]: + self.delete(key) + + def manager_start(self): + self.clear_all(TxType.CLEAR_ON_MANAGER_START) + + def panda_disconnect(self): + self.clear_all(TxType.CLEAR_ON_PANDA_DISCONNECT) + + def get(self, key, block=False, encoding=None): + key = ensure_bytes(key) + + if key not in keys: + raise UnknownKeyName(key) + + cdef string k = key + cdef bool b = block + + cdef string val + with nogil: + val = self.p.get(k, b) + + if val == b"": + if block: + # If we got no value while running in blocked mode + # it means we got an interrupt while waiting + raise KeyboardInterrupt + else: + return None + + if encoding is not None: + return val.decode(encoding) + else: + return val + + def put(self, key, dat): + """ + Warning: This function blocks until the param is written to disk! + In very rare cases this can take over a second, and your code will hang. + Use the put_nonblocking helper function in time sensitive code, but + in general try to avoid writing params as much as possible. + """ + key = ensure_bytes(key) + dat = ensure_bytes(dat) + + if key not in keys: + raise UnknownKeyName(key) + + self.p.write_db_value(key, dat) + + def delete(self, key): + key = ensure_bytes(key) + self.p.delete_db_value(key) + + +def put_nonblocking(key, val, d=None): + def f(key, val): + params = Params(d) + params.put(key, val) + + t = threading.Thread(target=f, args=(key, val)) + t.start() + return t diff --git a/common/params_pyx_setup.py b/common/params_pyx_setup.py new file mode 100644 index 000000000..28dd9ee50 --- /dev/null +++ b/common/params_pyx_setup.py @@ -0,0 +1,33 @@ +import os +import subprocess +from distutils.core import Extension, setup +from Cython.Build import cythonize + +from common.cython_hacks import BuildExtWithoutPlatformSuffix +from common.basedir import BASEDIR +from common.hardware import TICI + +ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg + +sourcefiles = ['params_pyx.pyx'] +extra_compile_args = ["-std=c++11"] + +if ARCH == "aarch64": + if TICI: + extra_compile_args += ["-DQCOM2"] + else: + extra_compile_args += ["-DQCOM"] + + +setup(name='common', + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, + ext_modules=cythonize( + Extension( + "params_pyx", + language="c++", + sources=sourcefiles, + include_dirs=[BASEDIR, os.path.join(BASEDIR, 'selfdrive')], + extra_compile_args=extra_compile_args + ) + ) +) diff --git a/common/profiler.py b/common/profiler.py index ac28bdac4..e4d208aca 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -36,10 +36,10 @@ class Profiler(): if not self.enabled: return self.iter += 1 - print("******* Profiling *******") + print("******* Profiling %d *******" % self.iter) for n, ms in sorted(self.cp.items(), key=lambda x: -x[1]): if n in self.cp_ignored: - print("%30s: %9.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms/self.tot*100)) + print("%30s: %9.2f avg: %7.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100)) else: - print("%30s: %9.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100)) + print("%30s: %9.2f avg: %7.2f percent: %3.0f" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100)) print("Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot)) diff --git a/common/realtime.py b/common/realtime.py index 7f4ad5c0d..c883ec053 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -31,7 +31,7 @@ def set_core_affinity(core): os.sched_setaffinity(0, [core,]) -def config_rt_process(core, priority): +def config_realtime_process(core, priority): gc.disable() set_realtime_priority(priority) set_core_affinity(core) diff --git a/common/transformations/setup.py b/common/transformations/setup.py index c239a9745..32546192d 100644 --- a/common/transformations/setup.py +++ b/common/transformations/setup.py @@ -1,42 +1,20 @@ -import os import numpy -import sysconfig from Cython.Build import cythonize -from Cython.Distutils import build_ext from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module - -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) - +from common.cython_hacks import BuildExtWithoutPlatformSuffix setup( name='Cython transformations wrapper', cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, ext_modules=cythonize( - Extension( - "transformations", - sources=["transformations.pyx"], - language="c++", - extra_compile_args=["-std=c++14"], - include_dirs=[numpy.get_include()], + Extension( + "transformations", + sources=["transformations.pyx"], + language="c++", + extra_compile_args=["-std=c++1z", "-Wno-cpp"], + include_dirs=[numpy.get_include()], + ), + nthreads=4, ) -)) +) diff --git a/common/transformations/transformations.pxd b/common/transformations/transformations.pxd index cb3ee53b1..7af009870 100644 --- a/common/transformations/transformations.pxd +++ b/common/transformations/transformations.pxd @@ -1,3 +1,4 @@ +#cython: language_level=3 from libcpp cimport bool cdef extern from "orientation.cc": diff --git a/common/transformations/transformations.pyx b/common/transformations/transformations.pyx index 194257e03..12d6e2ab5 100644 --- a/common/transformations/transformations.pyx +++ b/common/transformations/transformations.pyx @@ -26,7 +26,7 @@ cdef np.ndarray[double, ndim=2] matrix2numpy(Matrix3 m): [m(2, 0), m(2, 1), m(2, 2)], ]) -cdef Matrix3 numpy2matrix (np.ndarray[double, ndim=2, mode="fortran"] m): +cdef Matrix3 numpy2matrix(np.ndarray[double, ndim=2, mode="fortran"] m): assert m.shape[0] == 3 assert m.shape[1] == 3 return Matrix3(m.data) diff --git a/installer/updater/Makefile b/installer/updater/Makefile index d252fc2aa..54bd7c9df 100644 --- a/installer/updater/Makefile +++ b/installer/updater/Makefile @@ -10,7 +10,7 @@ WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=format-extra-args CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++1z -g -fPIC -O2 $(WARN_FLAGS) CURL_FLAGS = -I$(PHONELIBS)/curl/include CURL_LIBS = $(PHONELIBS)/curl/lib/libcurl.a \ @@ -34,6 +34,7 @@ all: updater OBJS = opensans_regular.ttf.o \ opensans_semibold.ttf.o \ opensans_bold.ttf.o \ + ../../selfdrive/common/util.o \ ../../selfdrive/common/touch.o \ ../../selfdrive/common/framebuffer.o \ $(PHONELIBS)/json11/json11.o \ diff --git a/installer/updater/update.json b/installer/updater/update.json index 6e2692bfa..046793a87 100644 --- a/installer/updater/update.json +++ b/installer/updater/update.json @@ -1,7 +1,7 @@ { - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a.zip", - "ota_hash": "efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e.img", - "recovery_len": 15861036, - "recovery_hash": "97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e" + "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-ba3ecb158edc760beda0d32e0eea4311031e460afa97fc180dc83f76cf512694.zip", + "ota_hash": "ba3ecb158edc760beda0d32e0eea4311031e460afa97fc180dc83f76cf512694", + "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-e35dc1939dab4c6c2cbae3f225b07515f1a5c02afb232dc22e93f17c9840499f.img", + "recovery_len": 15926572, + "recovery_hash": "e35dc1939dab4c6c2cbae3f225b07515f1a5c02afb232dc22e93f17c9840499f" } diff --git a/installer/updater/update_kernel.json b/installer/updater/update_kernel.json deleted file mode 100644 index 41dc595b0..000000000 --- a/installer/updater/update_kernel.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-3bd2b3bdd6a501569e00b8f12786d65e0fd2788c0dd238f8c986e3e2e504683a-kernel.zip", - "ota_hash": "3bd2b3bdd6a501569e00b8f12786d65e0fd2788c0dd238f8c986e3e2e504683a", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e.img", - "recovery_len": 15861036, - "recovery_hash": "97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e" -} diff --git a/installer/updater/updater b/installer/updater/updater index 66047420c..729de8434 100755 Binary files a/installer/updater/updater and b/installer/updater/updater differ diff --git a/installer/updater/updater.cc b/installer/updater/updater.cc index ca0b9270b..d737ea248 100644 --- a/installer/updater/updater.cc +++ b/installer/updater/updater.cc @@ -231,6 +231,8 @@ struct Updater { &fb_w, &fb_h); assert(fb); + framebuffer_set_power(fb, HWC_POWER_MODE_NORMAL); + vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); assert(vg); diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 7124ef3c4..9686c436e 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -12,8 +12,8 @@ function two_init { # Restrict Android and other system processes to the first two cores echo 0-1 > /dev/cpuset/background/cpus echo 0-1 > /dev/cpuset/system-background/cpus - echo 0-1 > /dev/cpuset/foreground/boost/cpus echo 0-1 > /dev/cpuset/foreground/cpus + echo 0-1 > /dev/cpuset/foreground/boost/cpus echo 0-1 > /dev/cpuset/android/cpus # openpilot gets all the cores @@ -27,6 +27,8 @@ 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 + # restrict unbound kworkers to first two cores + #find /sys/devices/virtual/workqueue -name cpumask -exec sh -c 'echo 3 > {}' ';' # Check for NEOS update if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then @@ -43,10 +45,6 @@ function two_init { fi "$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json" - else - if [[ $(uname -v) == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020" ]]; then - "$DIR/installer/updater/updater" "file://$DIR/installer/updater/update_kernel.json" - fi fi # One-time fix for a subset of OP3T with gyro orientation offsets. @@ -117,6 +115,9 @@ function launch { ln -sfn $(pwd) /data/pythonpath export PYTHONPATH="$PWD" + # write tmux scrollback to a file + tmux capture-pane -pq -S-1000 > /tmp/launch_log + # start manager cd selfdrive ./manager.py diff --git a/launch_env.sh b/launch_env.sh index 9a86d315c..ac578facf 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$REQUIRED_NEOS_VERSION" ]; then - export REQUIRED_NEOS_VERSION="14" + export REQUIRED_NEOS_VERSION="15" fi if [ -z "$PASSIVE" ]; then diff --git a/opendbc/acura_rdx_2020_can_generated.dbc b/opendbc/acura_rdx_2020_can_generated.dbc new file mode 100644 index 000000000..b4adc6625 --- /dev/null +++ b/opendbc/acura_rdx_2020_can_generated.dbc @@ -0,0 +1,433 @@ +CM_ "AUTOGENERATED FILE, DO NOT EDIT" + + +CM_ "Imported file _bosch_2020.dbc starts here" +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BU_: EBCM EON CAM RADAR PCM EPS VSA SCM BDY XXX EPB + +BO_ 148 KINEMATICS: 8 XXX + SG_ LAT_ACCEL : 7|10@0+ (-0.035,17.92) [-20|20] "m/s2" EON + SG_ LONG_ACCEL : 25|10@0+ (-0.035,17.92) [-20|20] "m/s2" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + +BO_ 228 STEERING_CONTROL: 5 EON + SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS + SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS + SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS + SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS + SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS + +BO_ 229 BOSCH_SUPPLEMENTAL_1: 8 XXX + SG_ SET_ME_X04 : 0|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00 : 8|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_X80 : 16|8@1+ (1,0) [0|255] "" XXX + SG_ SET_ME_X10 : 24|8@1+ (1,0) [0|255] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 232 BRAKE_HOLD: 7 XXX + SG_ XMISSION_SPEED : 7|14@0- (1,0) [1|0] "" XXX + SG_ COMPUTER_BRAKE : 39|16@0+ (1,0) [0|0] "" XXX + SG_ COMPUTER_BRAKE_REQUEST : 29|1@0+ (1,0) [0|0] "" XXX + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" XXX + +BO_ 342 STEERING_SENSORS: 6 EPS + SG_ STEER_ANGLE : 7|16@0- (-0.1,0) [-500|500] "deg" EON + SG_ STEER_ANGLE_RATE : 23|16@0- (1,0) [-3000|3000] "deg/s" EON + SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" EON + +BO_ 344 ENGINE_DATA: 8 PCM + SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON + SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON + SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON + SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 380 POWERTRAIN_DATA: 8 PCM + SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON + SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON + SG_ GAS_PRESSED : 39|1@0+ (1,0) [0|1] "" EON + SG_ ACC_STATUS : 38|1@0+ (1,0) [0|1] "" EON + SG_ BOH_17C : 37|5@0+ (1,0) [0|1] "" EON + SG_ BRAKE_SWITCH : 32|1@0+ (1,0) [0|1] "" EON + SG_ BOH2_17C : 47|10@0+ (1,0) [0|1] "" EON + SG_ BRAKE_PRESSED : 53|1@0+ (1,0) [0|1] "" EON + SG_ BOH3_17C : 52|5@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 399 STEER_STATUS: 7 EPS + SG_ STEER_TORQUE_SENSOR : 7|16@0- (-1,0) [-31000|31000] "tbd" EON + SG_ STEER_ANGLE_RATE : 23|16@0- (-0.1,0) [-31000|31000] "deg/s" EON + SG_ STEER_STATUS : 39|4@0+ (1,0) [0|15] "" EON + SG_ STEER_CONTROL_ACTIVE : 35|1@0+ (1,0) [0|1] "" EON + SG_ STEER_CONFIG_INDEX : 43|4@0+ (1,0) [0|15] "" EON + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON + +BO_ 420 VSA_STATUS: 8 VSA + SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON + SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 427 STEER_MOTOR_TORQUE: 3 EPS + SG_ CONFIG_VALID : 7|1@0+ (1,0) [0|1] "" EON + SG_ MOTOR_TORQUE : 1|10@0+ (1,0) [0|256] "" EON + SG_ OUTPUT_DISABLED : 22|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 21|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 19|4@0+ (1,0) [0|15] "" EON + +BO_ 450 EPB_STATUS: 8 EPB + SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" EON + SG_ EPB_STATE : 29|2@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 464 WHEEL_SPEEDS: 8 VSA + SG_ WHEEL_SPEED_FL : 7|15@0+ (0.01,0) [0|250] "kph" EON + SG_ WHEEL_SPEED_FR : 8|15@0+ (0.01,0) [0|250] "kph" EON + SG_ WHEEL_SPEED_RL : 25|15@0+ (0.01,0) [0|250] "kph" EON + SG_ WHEEL_SPEED_RR : 42|15@0+ (0.01,0) [0|250] "kph" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + +BO_ 479 ACC_CONTROL: 8 EON + SG_ SET_TO_0 : 20|5@0+ (1,0) [0|1] "" XXX + SG_ CONTROL_ON : 23|3@0+ (1,0) [0|5] "" XXX + SG_ GAS_COMMAND : 7|16@0- (1,0) [0|0] "" XXX + SG_ ACCEL_COMMAND : 31|11@0- (0.01,0) [0|0] "m/s2" XXX + SG_ BRAKE_LIGHTS : 62|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_REQUEST : 34|1@0+ (1,0) [0|1] "" XXX + SG_ STANDSTILL : 35|1@0+ (1,0) [0|1] "" XXX + SG_ STANDSTILL_RELEASE : 36|1@0+ (1,0) [0|1] "" XXX + SG_ AEB_STATUS : 33|1@0+ (1,0) [0|1] "" XXX + SG_ AEB_BRAKING : 47|1@0+ (1,0) [0|1] "" XXX + SG_ AEB_PREPARE : 43|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 490 VEHICLE_DYNAMICS: 8 VSA + SG_ LAT_ACCEL : 7|16@0- (0.0015,0) [-20|20] "m/s2" EON + SG_ LONG_ACCEL : 23|16@0- (0.0015,0) [-20|20] "m/s2" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + +BO_ 495 ACC_CONTROL_ON: 8 XXX + SG_ SET_TO_75 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_30 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ ZEROS_BOH : 23|8@0+ (1,0) [0|255] "" XXX + SG_ ZEROS_BOH2 : 47|16@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_FF : 15|8@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_3 : 6|7@0+ (1,0) [0|4095] "" XXX + SG_ CONTROL_ON : 7|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + +BO_ 545 XXX_16: 6 SCM + SG_ ECON_ON : 23|1@0+ (1,0) [0|1] "" XXX + SG_ DRIVE_MODE : 37|2@0+ (1,0) [0|3] "" XXX + SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY + SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY + +BO_ 576 LEFT_LANE_LINE_1: 8 CAM + SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX + SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX + SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX + SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX + SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 577 LEFT_LANE_LINE_2: 8 CAM + SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX + SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX + SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX + SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 579 RIGHT_LANE_LINE_1: 8 CAM + SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX + SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX + SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX + SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX + SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 580 RIGHT_LANE_LINE_2: 8 CAM + SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX + SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX + SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX + SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 582 ADJACENT_LEFT_LANE_LINE_1: 8 CAM + SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX + SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX + SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX + SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX + SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 583 ADJACENT_LEFT_LANE_LINE_2: 8 CAM + SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX + SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX + SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX + SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 585 ADJACENT_RIGHT_LANE_LINE_1: 8 CAM + SG_ LINE_DISTANCE_VISIBLE : 39|9@0+ (1,0) [0|1] "" XXX + SG_ LINE_PROBABILITY : 46|6@0+ (0.015625,0) [0|1] "" XXX + SG_ LINE_OFFSET : 23|12@0+ (0.004,-8.192) [0|1] "Meters" XXX + SG_ LINE_ANGLE : 7|12@0+ (0.0005,-1.024) [0|1] "" XXX + SG_ FRAME_INDEX : 8|4@1+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM + SG_ LINE_FAR_EDGE_POSITION : 55|8@0+ (1,-128) [0|1] "" XXX + SG_ LINE_SOLID : 13|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_DASHED : 14|1@0+ (1,0) [0|1] "" XXX + SG_ LINE_CURVATURE : 23|12@0+ (0.00001,-0.02048) [0|1] "" XXX + SG_ LINE_PARAMETER : 39|12@0+ (1,0) [0|1] "" XXX + SG_ FRAME_INDEX : 7|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|1] "" XXX + +BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA + SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON + SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON + SG_ WHEEL_SPEED_RL : 23|8@0+ (1,0) [0|255] "mph" EON + SG_ WHEEL_SPEED_RR : 31|8@0+ (1,0) [0|255] "mph" EON + SG_ SET_TO_X55 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_X55_2 : 47|8@0+ (1,0) [0|255] "" EON + SG_ LONG_COUNTER : 55|8@0+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + + BO_ 662 SCM_BUTTONS: 4 SCM + SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON + SG_ CRUISE_SETTING : 3|2@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 29|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 27|4@0+ (1,0) [0|15] "" EON + +BO_ 773 SEATBELT_STATUS: 7 BDY + SG_ SEATBELT_DRIVER_LAMP : 7|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_PASS_UNLATCHED : 10|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_PASS_LATCHED : 11|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_DRIVER_UNLATCHED : 12|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_DRIVER_LATCHED : 13|1@0+ (1,0) [0|1] "" EON + SG_ PASS_AIRBAG_OFF : 14|1@0+ (1,0) [0|1] "" EON + SG_ PASS_AIRBAG_ON : 15|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 51|4@0+ (1,0) [0|3] "" EON + +BO_ 777 CAR_SPEED: 8 PCM + SG_ ROUGH_CAR_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX + SG_ CAR_SPEED : 7|16@0+ (0.01,0) [0|65535] "kph" XXX + SG_ ROUGH_CAR_SPEED_3 : 39|16@0+ (0.01,0) [0|65535] "kph" XXX + SG_ ROUGH_CAR_SPEED_2 : 31|8@0+ (1,0) [0|255] "mph" XXX + SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 780 ACC_HUD: 8 ADAS + SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY + SG_ DTC_MODE : 39|1@0+ (1,0) [0|1] "" BDY + SG_ BOH : 38|1@0+ (1,0) [0|1] "" BDY + SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY + SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY + SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY + SG_ BOH_3 : 43|1@0+ (1,0) [0|3] "" BDY + SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY + SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY + SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY + SG_ ZEROS_BOH : 7|24@0+ (0.002759506,0) [0|100] "m/s" BDY + SG_ FCM_OFF : 35|1@0+ (1,0) [0|1] "" BDY + SG_ SET_TO_1 : 36|1@0+ (1,0) [0|1] "" XXX + SG_ HUD_DISTANCE : 47|2@0+ (1,0) [0|3] "" BDY + SG_ HUD_LEAD : 45|2@0+ (1,0) [0|3] "" BDY + SG_ ACC_PROBLEM : 37|1@0+ (1,0) [0|1] "" BDY + SG_ ACC_ON : 52|1@0+ (1,0) [0|1] "" XXX + SG_ BOH_6 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ SET_TO_X1 : 55|1@0+ (1,0) [0|1] "" XXX + SG_ IMPERIAL_UNIT : 54|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 804 CRUISE: 8 PCM + SG_ TRIP_FUEL_CONSUMED : 23|16@0+ (1,0) [0|255] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 806 SCM_FEEDBACK: 8 SCM + SG_ DRIVERS_DOOR_OPEN : 17|1@0+ (1,0) [0|1] "" XXX + SG_ MAIN_ON : 28|1@0+ (1,0) [0|1] "" EON + SG_ RIGHT_BLINKER : 27|1@0+ (1,0) [0|1] "" EON + SG_ LEFT_BLINKER : 26|1@0+ (1,0) [0|1] "" EON + SG_ CMBS_STATES : 22|2@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 829 LKAS_HUD: 5 ADAS + SG_ CAM_TEMP_HIGH : 7|1@0+ (1,0) [0|255] "" BDY + SG_ SET_ME_X41 : 6|7@0+ (1,0) [0|127] "" BDY + SG_ BOH : 6|7@0+ (1,0) [0|127] "" BDY + SG_ DASHED_LANES : 14|1@0+ (1,0) [0|1] "" BDY + SG_ DTC : 13|1@0+ (1,0) [0|1] "" BDY + SG_ LKAS_PROBLEM : 12|1@0+ (1,0) [0|1] "" BDY + SG_ LKAS_OFF : 11|1@0+ (1,0) [0|1] "" BDY + SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY + SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY + SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY + SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY + SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY + SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY + SG_ LDW_OFF : 27|1@0+ (1,0) [0|1] "" BDY + SG_ CLEAN_WINDSHIELD : 26|1@0+ (1,0) [0|1] "" BDY + SG_ SET_ME_X48 : 31|8@0+ (1,0) [0|255] "" BDY + SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" BDY + SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" BDY + +BO_ 862 CAMERA_MESSAGES: 8 CAM + SG_ ZEROS_BOH : 7|50@0+ (1,0) [0|127] "" BDY + SG_ AUTO_HIGHBEAMS_ACTIVE : 53|1@0+ (1,0) [0|1] "" XXX + SG_ HIGHBEAMS_ON : 52|1@0+ (1,0) [0|1] "" XXX + SG_ ZEROS_BOH_2 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 884 STALK_STATUS: 8 XXX + SG_ AUTO_HEADLIGHTS : 46|1@0+ (1,0) [0|1] "" EON + SG_ HIGH_BEAM_HOLD : 47|1@0+ (1,0) [0|1] "" EON + SG_ HIGH_BEAM_FLASH : 45|1@0+ (1,0) [0|1] "" EON + SG_ HEADLIGHTS_ON : 54|1@0+ (1,0) [0|1] "" EON + SG_ WIPER_SWITCH : 53|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + +BO_ 891 STALK_STATUS_2: 8 XXX + SG_ WIPERS : 17|2@0+ (1,0) [0|3] "" EON + SG_ LOW_BEAMS : 35|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAMS : 34|1@0+ (1,0) [0|1] "" XXX + SG_ PARK_LIGHTS : 36|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + +CM_ SG_ 479 AEB_STATUS "set for the duration of AEB event"; +CM_ SG_ 479 AEB_BRAKING "set when braking is commanded during AEB event"; +CM_ SG_ 479 AEB_PREPARE "set 1s before AEB"; +CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded"; +CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded"; +CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded"; +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" + + +BO_ 304 GAS_PEDAL_2: 8 PCM + SG_ ENGINE_TORQUE_ESTIMATE : 7|16@0- (1,0) [-1000|1000] "Nm" EON + SG_ ENGINE_TORQUE_REQUEST : 23|16@0- (1,0) [-1000|1000] "Nm" EON + SG_ CAR_GAS : 39|8@0+ (1,0) [0|255] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 419 GEARBOX: 8 PCM + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + SG_ GEAR_SHIFTER : 29|6@0+ (1,0) [0|63] "" EON + SG_ GEAR : 7|8@0+ (1,0) [0|255] "" EON + +BO_ 432 STANDSTILL: 7 VSA + SG_ BRAKE_ERROR_1 : 11|1@0+ (1,0) [0|1] "" EON + SG_ BRAKE_ERROR_2 : 9|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON + +BO_ 446 BRAKE_MODULE: 3 VSA + SG_ BRAKE_PRESSED : 4|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 21|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 19|4@0+ (1,0) [0|15] "" XXX + +BO_ 927 RADAR_HUD: 8 RADAR + SG_ ZEROS_BOH : 7|10@0+ (1,0) [0|127] "" BDY + SG_ CMBS_OFF : 12|1@0+ (1,0) [0|1] "" BDY + SG_ RESUME_INSTRUCTION : 21|1@0+ (1,0) [0|1] "" XXX + SG_ SET_TO_1 : 13|1@0+ (1,0) [0|1] "" BDY + SG_ ZEROS_BOH2 : 11|4@0+ (1,0) [0|1] "" XXX + SG_ APPLY_BRAKES_FOR_CANC : 23|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_ALERTS : 20|5@0+ (1,0) [0|1] "" BDY + SG_ SET_TO_0 : 22|1@0+ (1,0) [0|1] "" XXX + SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|255] "" XXX + SG_ BOH : 40|1@0+ (1,0) [0|1] "" XXX + SG_ BOH_2 : 30|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 1302 ODOMETER: 8 XXX + SG_ ODOMETER : 7|24@0+ (1,0) [0|16777215] "km" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + +VAL_ 419 GEAR_SHIFTER 32 "D" 8 "R" 4 "P" ; +VAL_ 545 ECON_ON_2 0 "off" 3 "on" ; +VAL_ 662 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ; +VAL_ 662 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ; +VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ; +VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ; +VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; + +CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0"; diff --git a/opendbc/can/common_pyx_setup.py b/opendbc/can/common_pyx_setup.py index 04f95359a..a72998425 100644 --- a/opendbc/can/common_pyx_setup.py +++ b/opendbc/can/common_pyx_setup.py @@ -34,7 +34,7 @@ class BuildExtWithoutPlatformSuffix(build_ext): return get_ext_filename_without_platform_suffix(filename) -extra_compile_args = ["-std=c++14", "-Wno-nullability-completeness"] +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"] diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc index dcdaec6c8..d9c69c6b8 100644 --- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc +++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc @@ -388,7 +388,7 @@ BO_ 304 GAS_PEDAL_2: 8 PCM SG_ CAR_GAS : 39|8@0+ (1,0) [0|255] "" EON SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON - + BO_ 401 GEARBOX: 8 PCM SG_ GEAR_SHIFTER : 5|6@0+ (1,0) [0|63] "" EON SG_ BOH : 45|6@0+ (1,0) [0|63] "" XXX @@ -405,6 +405,11 @@ BO_ 432 STANDSTILL: 7 VSA SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON +BO_ 506 LEGACY_BRAKE_COMMAND: 8 ADAS + SG_ CHIME : 40|8@1+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + BO_ 892 CRUISE_PARAMS: 8 PCM SG_ CRUISE_SPEED_OFFSET : 31|8@0- (0.1,0) [-128|127] "kph" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON 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 a96ad676c..885de7131 100644 --- a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc +++ b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc @@ -400,6 +400,11 @@ BO_ 432 STANDSTILL: 7 VSA SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON +BO_ 506 LEGACY_BRAKE_COMMAND: 8 ADAS + SG_ CHIME : 40|8@1+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + BO_ 892 CRUISE_PARAMS: 8 PCM SG_ CRUISE_SPEED_OFFSET : 31|8@0- (0.1,0) [-128|127] "kph" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index ce38bf50a..e09310e4b 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -424,6 +424,14 @@ BO_ 956 GEAR_PACKET: 8 XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX +BO_ 1653 Date_Time: 8 XXX + SG_ Year : 23|8@0+ (1,0) [0|255] "" XXX + SG_ Month : 31|8@0+ (1,0) [0|255] "" XXX + SG_ Day : 39|8@0+ (1,0) [0|255] "" XXX + SG_ Hour : 47|8@0+ (1,0) [0|255] "" XXX + SG_ Minute : 55|8@0+ (1,0) [0|255] "" XXX + SG_ Second : 63|8@0+ (1,0) [0|255] "" XXX + CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libPlatformValidatorShared.so b/phonelibs/snpe/aarch64-android-clang6.0/libPlatformValidatorShared.so new file mode 100644 index 000000000..5df5b07b1 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libPlatformValidatorShared.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so b/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so index f8338a6e3..6ef777e15 100644 Binary files a/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so and b/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libSNPE_G.so b/phonelibs/snpe/aarch64-android-clang6.0/libSNPE_G.so new file mode 100644 index 000000000..565c80bb6 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libSNPE_G.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libcalculator.so b/phonelibs/snpe/aarch64-android-clang6.0/libcalculator.so new file mode 100644 index 000000000..3aa1ec3ac Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libcalculator.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libcalculator_domains.so b/phonelibs/snpe/aarch64-android-clang6.0/libcalculator_domains.so new file mode 100644 index 000000000..bef3fc201 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libcalculator_domains.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libhta.so b/phonelibs/snpe/aarch64-android-clang6.0/libhta.so new file mode 100644 index 000000000..bfd3554cf Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libhta.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so index 98b8c2d8a..8a6b4e3fc 100644 Binary files a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so and b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains.so new file mode 100644 index 000000000..0663d25e3 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_system.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_system.so new file mode 100644 index 000000000..33d354ff7 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_system.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2.so new file mode 100644 index 000000000..5a6334459 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2_system.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2_system.so new file mode 100644 index 000000000..2d74ae0f0 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2_system.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_skel.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_skel.so deleted file mode 100644 index eecf45749..000000000 Binary files a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_skel.so and /dev/null differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsymphony-cpu.so b/phonelibs/snpe/aarch64-android-clang6.0/libsymphony-cpu.so old mode 100755 new mode 100644 diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libPlatformValidatorShared.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libPlatformValidatorShared.so new file mode 100644 index 000000000..32f8e2f60 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libPlatformValidatorShared.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so index 45cf75d44..6f497cfa0 100644 Binary files a/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so and b/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so index 9452b0e4a..dd3bfe5af 100644 Binary files a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so and b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so index eb3d336ea..05cf724b7 100644 Binary files a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so and b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so index 61a910ff5..11db2cdf6 100644 Binary files a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so index c12328c7d..cc8a8dc2a 100644 Binary files a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so old mode 100755 new mode 100644 diff --git a/phonelibs/snpe/dsp/libcalculator_domains_skel.so b/phonelibs/snpe/dsp/libcalculator_domains_skel.so new file mode 100644 index 000000000..cb8897f23 Binary files /dev/null and b/phonelibs/snpe/dsp/libcalculator_domains_skel.so differ diff --git a/phonelibs/snpe/dsp/libcalculator_skel.so b/phonelibs/snpe/dsp/libcalculator_skel.so new file mode 100644 index 000000000..45515ea4f Binary files /dev/null and b/phonelibs/snpe/dsp/libcalculator_skel.so differ diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_domains_skel.so b/phonelibs/snpe/dsp/libsnpe_dsp_domains_skel.so new file mode 100644 index 000000000..d4e341262 Binary files /dev/null and b/phonelibs/snpe/dsp/libsnpe_dsp_domains_skel.so differ diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_skel.so b/phonelibs/snpe/dsp/libsnpe_dsp_skel.so new file mode 100644 index 000000000..cb37356a9 Binary files /dev/null and b/phonelibs/snpe/dsp/libsnpe_dsp_skel.so differ diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_v65_domains_v2_skel.so b/phonelibs/snpe/dsp/libsnpe_dsp_v65_domains_v2_skel.so new file mode 100644 index 000000000..97a1499a0 Binary files /dev/null and b/phonelibs/snpe/dsp/libsnpe_dsp_v65_domains_v2_skel.so differ diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_v66_domains_v2_skel.so b/phonelibs/snpe/dsp/libsnpe_dsp_v66_domains_v2_skel.so new file mode 100644 index 000000000..008e9e978 Binary files /dev/null and b/phonelibs/snpe/dsp/libsnpe_dsp_v66_domains_v2_skel.so differ diff --git a/phonelibs/snpe/include/DiagLog/IDiagLog.hpp b/phonelibs/snpe/include/DiagLog/IDiagLog.hpp index af1ef8917..018b56725 100644 --- a/phonelibs/snpe/include/DiagLog/IDiagLog.hpp +++ b/phonelibs/snpe/include/DiagLog/IDiagLog.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2015 Qualcomm Technologies, Inc. +// Copyright (c) 2015, 2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -8,14 +8,11 @@ #ifndef __IDIAGLOG_HPP_ #define __IDIAGLOG_HPP_ -#ifndef ZDL_LOGGING_EXPORT -#define ZDL_LOGGING_EXPORT __attribute__((visibility("default"))) -#endif - #include #include "DiagLog/Options.hpp" #include "DlSystem/String.hpp" +#include "DlSystem/ZdlExportDefine.hpp" namespace zdl { @@ -26,10 +23,10 @@ namespace DiagLog @{ */ /// @brief . -/// +/// /// Interface for controlling logging for zdl components. -class ZDL_LOGGING_EXPORT IDiagLog +class ZDL_EXPORT IDiagLog { public: @@ -48,7 +45,7 @@ public: /// /// @return Diag log options object. virtual Options getOptions() = 0; - + /// @brief . /// /// Allows for setting the log mask once diag logging has started diff --git a/phonelibs/snpe/include/DiagLog/Options.hpp b/phonelibs/snpe/include/DiagLog/Options.hpp index ea247e953..798fa3f12 100644 --- a/phonelibs/snpe/include/DiagLog/Options.hpp +++ b/phonelibs/snpe/include/DiagLog/Options.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2015 Qualcomm Technologies, Inc. +// Copyright (c) 2015, 2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -8,12 +8,8 @@ #ifndef __DIAGLOG_OPTIONS_HPP_ #define __DIAGLOG_OPTIONS_HPP_ -#ifndef ZDL_LOGGING_EXPORT -#define ZDL_LOGGING_EXPORT __attribute__((visibility("default"))) -#endif - #include -#include +#include "DlSystem/ZdlExportDefine.hpp" namespace zdl { @@ -25,7 +21,7 @@ namespace DiagLog /// @brief . /// /// Options for setting up diagnostic logging for zdl components. -class ZDL_LOGGING_EXPORT Options +class ZDL_EXPORT Options { public: Options() : @@ -40,12 +36,12 @@ public: } /// @brief . - /// + /// /// Enables diag logging only on the specified area mask (DNN_RUNTIME=ON | OFF) std::string DiagLogMask; /// @brief . - /// + /// /// The path to the directory where log files will be written. /// The path may be relative or absolute. Relative paths are interpreted /// from the current working directory. @@ -53,15 +49,15 @@ public: std::string LogFileDirectory; /// @brief . - /// + /// //// The name used for log files. If this value is empty then BaseName will be /// used as the default file name. /// Default value is "DiagLog" std::string LogFileName; /// @brief . - /// - /// The maximum number of log files to create. If set to 0 no log rotation + /// + /// The maximum number of log files to create. If set to 0 no log rotation /// will be used and the log file name specified will be used each time, overwriting /// any existing log file that may exist. /// Default value is 20 diff --git a/phonelibs/snpe/include/DlContainer/IDlContainer.hpp b/phonelibs/snpe/include/DlContainer/IDlContainer.hpp index f2022ca9f..4e29b39bb 100644 --- a/phonelibs/snpe/include/DlContainer/IDlContainer.hpp +++ b/phonelibs/snpe/include/DlContainer/IDlContainer.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2015,2019 Qualcomm Technologies, Inc. +// Copyright (c) 2015-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -14,7 +14,6 @@ #include #include #include -#include #include "DlSystem/ZdlExportDefine.hpp" #include "DlSystem/String.hpp" @@ -76,7 +75,7 @@ public: * * @return A pointer to the initialized container */ - ZDL_EXPORT static std::unique_ptr + static std::unique_ptr open(const std::string &filename) noexcept; /** @@ -86,7 +85,7 @@ public: * * @return A pointer to the initialized container */ - ZDL_EXPORT static std::unique_ptr + static std::unique_ptr open(const zdl::DlSystem::String &filename) noexcept; /** @@ -97,7 +96,7 @@ public: * * @return A pointer to the initialized container */ - ZDL_EXPORT static std::unique_ptr + static std::unique_ptr open(const std::vector &buffer) noexcept; /** @@ -110,7 +109,7 @@ public: * * @return A pointer to the initialized container */ - ZDL_EXPORT static std::unique_ptr + static std::unique_ptr open(const uint8_t* buffer, const size_t size) noexcept; diff --git a/phonelibs/snpe/include/DlSystem/DlEnums.hpp b/phonelibs/snpe/include/DlSystem/DlEnums.hpp index 5b8b6b35f..144706355 100644 --- a/phonelibs/snpe/include/DlSystem/DlEnums.hpp +++ b/phonelibs/snpe/include/DlSystem/DlEnums.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2014-2019 Qualcomm Technologies, Inc. +// Copyright (c) 2014-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -128,9 +128,13 @@ enum class ProfilingLevel_t BASIC = 1, /// Detailed profiling - /// Collects more runtime stats in the DiagLog + /// Collects more runtime stats in the DiagLog, including per-layer statistics /// Performance may be impacted - DETAILED = 2 + DETAILED = 2, + + /// Moderate profiling + /// Collects more runtime stats in the DiagLog, no per-layer statistics + MODERATE = 3 }; /** diff --git a/phonelibs/snpe/include/DlSystem/DlError.hpp b/phonelibs/snpe/include/DlSystem/DlError.hpp index a8b0b4ea7..f0a66e8e4 100644 --- a/phonelibs/snpe/include/DlSystem/DlError.hpp +++ b/phonelibs/snpe/include/DlSystem/DlError.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2016-2019 Qualcomm Technologies, Inc. +// Copyright (c) 2016-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -79,6 +79,7 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_DLCONTAINER_BAD_DNN_FORMAT_VERSION = 312, SNPE_DLCONTAINER_UNKNOWN_AXIS_ANNOTATION = 313, SNPE_DLCONTAINER_UNKNOWN_SHUFFLE_TYPE = 314, + SNPE_DLCONTAINER_TEMP_FILE_FAILURE = 315, // Network errors SNPE_NETWORK_EMPTY_NETWORK = 400, @@ -191,7 +192,11 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { // DlCaching errors SNPE_DLCACHING_INVALID_METADATA = 1500, - SNPE_DLCACHING_INVALID_INITBLOB = 1501 + SNPE_DLCACHING_INVALID_INITBLOB = 1501, + + // Infrastructure Errors + SNPE_INFRA_CLUSTERMGR_INSTANCE_INVALID = 1600, + SNPE_INFRA_CLUSTERMGR_EXECUTE_SYNC_FAILED = 1601 }; diff --git a/phonelibs/snpe/include/DlSystem/DlOptional.hpp b/phonelibs/snpe/include/DlSystem/DlOptional.hpp index 35416b426..4f83e6b4e 100644 --- a/phonelibs/snpe/include/DlSystem/DlOptional.hpp +++ b/phonelibs/snpe/include/DlSystem/DlOptional.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2016 Qualcomm Technologies, Inc. +// Copyright (c) 2016, 2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -11,6 +11,7 @@ #include #include +#include #include "DlSystem/ZdlExportDefine.hpp" @@ -173,25 +174,25 @@ private: template typename std::enable_if::value, const Q&>::type GetReference() const noexcept { - if (!isReference()) throw std::bad_exception(); + if (!isReference()) std::terminate(); return *static_cast(m_StoragePtr); } template typename std::enable_if::value, const Q&>::type GetReference() const noexcept { - if (!isPointer()) throw std::bad_exception(); + if (!isPointer()) std::terminate(); return static_cast(m_StoragePtr); } template typename std::enable_if::value, Q&>::type GetReference() noexcept { - if (!isReference()) throw std::bad_exception(); + if (!isReference()) std::terminate(); return *m_StoragePtr; } template typename std::enable_if::value, Q&>::type GetReference() noexcept { - if (!isPointer()) throw std::bad_exception(); + if (!isPointer()) std::terminate(); return m_StoragePtr; } diff --git a/phonelibs/snpe/include/DlSystem/ITensor.hpp b/phonelibs/snpe/include/DlSystem/ITensor.hpp index ed6d5f107..2f006c857 100644 --- a/phonelibs/snpe/include/DlSystem/ITensor.hpp +++ b/phonelibs/snpe/include/DlSystem/ITensor.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2015-2018 Qualcomm Technologies, Inc. +// Copyright (c) 2015-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -14,7 +14,6 @@ #include "TensorShape.hpp" #include "ZdlExportDefine.hpp" #include -#include #include #include diff --git a/phonelibs/snpe/include/DlSystem/ITensorItrImpl.hpp b/phonelibs/snpe/include/DlSystem/ITensorItrImpl.hpp index 7923c160b..069e7a1d6 100644 --- a/phonelibs/snpe/include/DlSystem/ITensorItrImpl.hpp +++ b/phonelibs/snpe/include/DlSystem/ITensorItrImpl.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2015 Qualcomm Technologies, Inc. +// Copyright (c) 2015-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -13,7 +13,6 @@ #include #include -#include namespace DlSystem { diff --git a/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp b/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp index aefa0eda9..5e5731dd8 100644 --- a/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp +++ b/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2017-2019 Qualcomm Technologies, Inc. +// Copyright (c) 2017-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -160,6 +160,14 @@ public: m_StepExactly0(stepFor0), m_QuantizedStepSize(stepSize) {}; + UserBufferEncodingTf8(const zdl::DlSystem::UserBufferEncoding &ubEncoding) : UserBufferEncodingUnsigned8Bit(ubEncoding.getElementType()){ + const zdl::DlSystem::UserBufferEncodingTf8* ubEncodingTf8 + = dynamic_cast (&ubEncoding); + if (ubEncodingTf8) { + m_StepExactly0 = ubEncodingTf8->getStepExactly0(); + m_QuantizedStepSize = ubEncodingTf8->getQuantizedStepSize(); + } + } /** * @brief Sets the step value that represents 0 @@ -246,6 +254,17 @@ public: bitWidth(bWidth), m_StepExactly0(stepFor0), m_QuantizedStepSize(stepSize){}; + + UserBufferEncodingTfN(const zdl::DlSystem::UserBufferEncoding &ubEncoding) : UserBufferEncoding(ubEncoding.getElementType()){ + const zdl::DlSystem::UserBufferEncodingTfN* ubEncodingTfN + = dynamic_cast (&ubEncoding); + if (ubEncodingTfN) { + m_StepExactly0 = ubEncodingTfN->getStepExactly0(); + m_QuantizedStepSize = ubEncodingTfN->getQuantizedStepSize(); + bitWidth = ubEncodingTfN->bitWidth; + } + } + size_t getElementSize() const noexcept override; /** * @brief Sets the step value that represents 0 @@ -283,7 +302,7 @@ public: * @return Minimum representable floating point value */ float getMin() const { - return m_QuantizedStepSize * (0 - (double)m_StepExactly0); + return static_cast(m_QuantizedStepSize * (0 - (double)m_StepExactly0)); } /** @@ -293,7 +312,7 @@ public: * @return Maximum representable floating point value */ float getMax() const{ - return m_QuantizedStepSize * (pow(2,bitWidth)-1 - (double)m_StepExactly0); + return static_cast(m_QuantizedStepSize * (pow(2,bitWidth)-1 - (double)m_StepExactly0)); }; /** diff --git a/phonelibs/snpe/include/DlSystem/String.hpp b/phonelibs/snpe/include/DlSystem/String.hpp index 97b07726c..c1eba3bad 100644 --- a/phonelibs/snpe/include/DlSystem/String.hpp +++ b/phonelibs/snpe/include/DlSystem/String.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2017 Qualcomm Technologies, Inc. +// Copyright (c) 2017, 2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -12,10 +12,7 @@ #include #include #include - -#ifndef ZDL_EXPORT -#define ZDL_EXPORT __attribute__((visibility("default"))) -#endif +#include "DlSystem/ZdlExportDefine.hpp" namespace zdl { @@ -105,4 +102,3 @@ ZDL_EXPORT std::ostream& operator<<(std::ostream& os, const String& str) noexcep /** @} */ /* end_addtogroup c_plus_plus_apis C++ */ #endif // PLATFORM_STANDARD_STRING_HPP - diff --git a/phonelibs/snpe/include/DlSystem/TensorShapeMap.hpp b/phonelibs/snpe/include/DlSystem/TensorShapeMap.hpp index 00dc41a11..cef946e2e 100644 --- a/phonelibs/snpe/include/DlSystem/TensorShapeMap.hpp +++ b/phonelibs/snpe/include/DlSystem/TensorShapeMap.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2017 Qualcomm Technologies, Inc. +// Copyright (c) 2017-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -8,7 +8,6 @@ #include #include #include -#include #include "ZdlExportDefine.hpp" #include "DlSystem/TensorShape.hpp" #include "DlSystem/StringList.hpp" diff --git a/phonelibs/snpe/include/DlSystem/ZdlExportDefine.hpp b/phonelibs/snpe/include/DlSystem/ZdlExportDefine.hpp index dd0704dd8..92eb786d3 100644 --- a/phonelibs/snpe/include/DlSystem/ZdlExportDefine.hpp +++ b/phonelibs/snpe/include/DlSystem/ZdlExportDefine.hpp @@ -1,16 +1,13 @@ //============================================================================= // -// Copyright (c) 2015 Qualcomm Technologies, Inc. +// Copyright (c) 2015, 2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // //============================================================================= -#ifndef _ZDL_EXPORT_DEFINE_HPP_ -#define _ZDL_EXPORT_DEFINE_HPP_ +#pragma once #ifndef ZDL_EXPORT -#define ZDL_EXPORT __attribute__((visibility("default"))) -#endif - +#define ZDL_EXPORT #endif diff --git a/phonelibs/snpe/include/PlatformValidator/PlatformValidator.hpp b/phonelibs/snpe/include/PlatformValidator/PlatformValidator.hpp new file mode 100644 index 000000000..e74278ead --- /dev/null +++ b/phonelibs/snpe/include/PlatformValidator/PlatformValidator.hpp @@ -0,0 +1,93 @@ +// ============================================================================= +// +// Copyright (c) 2018-2019 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +// ============================================================================= + +#ifndef SNPE_PLATFORMVALIDATOR_HPP +#define SNPE_PLATFORMVALIDATOR_HPP + +#include "DlSystem/DlEnums.hpp" +#include "DlSystem/DlMacros.hpp" +SNPE_DISABLE_WARNINGS("-Wdelete-non-virtual-dtor","-Wdelete-non-virtual-dtor") +#include +#include +SNPE_ENABLE_WARNINGS + +namespace zdl +{ + namespace SNPE + { + class PlatformValidator; + + class IPlatformValidatorRuntime; + } +} + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** +* The class for checking SNPE compatibility/capability of a device. +* +*/ + +class zdl::SNPE::PlatformValidator +{ +public: + /** + * @brief Default Constructor of the PlatformValidator Class + * + * @return A new instance of a PlatformValidator object + * that can be used to check the SNPE compatibility + * of a device + */ + PlatformValidator(); + + ~PlatformValidator(); + + /** + * @brief Sets the runtime processor for compatibility check + * + * @return Void + */ + void setRuntime(zdl::DlSystem::Runtime_t runtime); + + /** + * @brief Checks if the Runtime prerequisites for SNPE are available. + * + * @return True if the Runtime prerequisites are available, else false. + */ + bool isRuntimeAvailable(); + + /** + * @brief Returns the core version for the Runtime selected. + * + * @return String which contains the actual core version value + */ + std::string getCoreVersion(); + + /** + * @brief Returns the library version for the Runtime selected. + * + * @return String which contains the actual lib version value + */ + std::string getLibVersion(); + + /** + * @brief Runs a small program on the runtime and Checks if SNPE is supported for Runtime. + * + * @return If True, the device is ready for SNPE execution, else not. + */ + + bool runtimeCheck(); + +private: + zdl::DlSystem::Runtime_t m_runtimeType; + std::unique_ptr m_platformValidatorRuntime; +}; +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#endif //SNPE_PLATFORMVALIDATOR_HPP diff --git a/phonelibs/snpe/include/SNPE/PSNPE.hpp b/phonelibs/snpe/include/SNPE/PSNPE.hpp index 732b7aa4d..c0a6fb55d 100644 --- a/phonelibs/snpe/include/SNPE/PSNPE.hpp +++ b/phonelibs/snpe/include/SNPE/PSNPE.hpp @@ -1,6 +1,6 @@ // ============================================================================= // -// Copyright (c) 2019 Qualcomm Technologies, Inc. +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -10,7 +10,6 @@ #define PSNPE_HPP #include -#include #include #include "SNPE/SNPE.hpp" #include "DlSystem/UserBufferMap.hpp" @@ -73,22 +72,40 @@ struct ZDL_EXPORT InputOutputAsyncCallbackParam }; }; +/** + * @brief This callback is called when the output data is ready, only use for Output Async mode + */ using OutputAsyncCallbackFunc = std::function; +/** + * @brief This callback is called when the output data is ready, only use for Output-Input Async mode + */ using InputOutputAsyncCallbackFunc = std::function; +/** + * @brief This callback is called when the input data is ready,only use for Output-Input Async mode + */ +using InputOutputAsyncInputCallback = std::function(const std::vector &, + const zdl::DlSystem::StringList &)>; /** * @brief . * - * A structure BulkSNPE configuration + * A structure PSNPE configuration + * */ struct ZDL_EXPORT BuildConfig final { - BuildMode buildMode = BuildMode::SERIAL; - zdl::DlContainer::IDlContainer* container; - zdl::DlSystem::StringList outputBufferNames; - RuntimeConfigList runtimeConfigList; - OutputAsyncCallbackFunc outputCallback; - InputOutputAsyncCallbackFunc inputOutputCallback; - InputOutputTransmissionMode inputOutputTransmissionMode = InputOutputTransmissionMode::sync; + BuildMode buildMode = BuildMode::SERIAL; ///< Specify build in serial mode or parallel mode + zdl::DlContainer::IDlContainer* container;///< The opened container ptr + zdl::DlSystem::StringList outputBufferNames;///< Specify the output layer name + RuntimeConfigList runtimeConfigList;///< The runtime config list for PSNPE, @see RuntimeConfig + size_t inputThreadNumbers = 1;///< Specify the number of threads used in the execution phase to process input data, only used in inputOutputAsync mode + size_t outputThreadNumbers = 1;///< Specify the number of threads used in the execution phase to process output data, only used in inputOutputAsync and outputAsync mode + OutputAsyncCallbackFunc outputCallback;///< The callback to deal with output data ,only used in outputAsync mode + InputOutputAsyncCallbackFunc inputOutputCallback;///< The callback to deal with output data ,only used in inputOutputAsync mode + InputOutputAsyncInputCallback inputOutputInputCallback;///< The callback to deal with input data ,only used in inputOutputAsync mode + InputOutputTransmissionMode inputOutputTransmissionMode = InputOutputTransmissionMode::sync;///< Specify execution mode + zdl::DlSystem::ProfilingLevel_t profilingLevel = zdl::DlSystem::ProfilingLevel_t::OFF;///< Specify profiling level for Diaglog + uint64_t encode[2] = {0, 0}; + bool enableInitCache = false; }; /** * @brief . @@ -130,7 +147,8 @@ class ZDL_EXPORT PSNPE final * * @return True if executed successfully; flase, otherwise. */ - bool executeInputOutputAsync(const ApplicationBufferMap& inputMap, size_t dataIndex, bool isTF8buff) noexcept; + bool executeInputOutputAsync(const std::vector& inputMap, size_t dataIndex, bool isTF8buff) noexcept; + bool executeInputOutputAsync(const std::vector& inputMap, size_t dataIndex, bool isTF8buff,bool isTF8Outputbuff) noexcept; /** * @brief Returns the input layer names of the network. * @@ -161,6 +179,8 @@ class ZDL_EXPORT PSNPE final */ const zdl::DlSystem::TensorShape getBufferAttributesDims(const char *name) const noexcept; + zdl::DlSystem::Optional getInputOutputBufferAttributes(const char *name) const noexcept; + private: PSNPE(const PSNPE&) = delete; PSNPE& operator=(const PSNPE&) = delete; diff --git a/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp b/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp index ecd438b91..837dba092 100644 --- a/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp +++ b/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2019 Qualcomm Technologies, Inc. +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -9,80 +9,77 @@ #define PSNPE_RUNTIMECONFIGLIST_HPP #include -#include "DlSystem/DlEnums.hpp" #include "DlContainer/IDlContainer.hpp" -#include "DlSystem/ZdlExportDefine.hpp" +#include "DlSystem/DlEnums.hpp" #include "DlSystem/RuntimeList.hpp" - +#include "DlSystem/TensorShapeMap.hpp" +#include "DlSystem/ZdlExportDefine.hpp" namespace zdl { -namespace PSNPE -{ +namespace PSNPE { /** @addtogroup c_plus_plus_apis C++ @{ */ /** - * @brief . - * - * The structure for configuring a BulkSNPE runtime - * - */ + * @brief . + * + * The structure for configuring a BulkSNPE runtime + * + */ struct ZDL_EXPORT RuntimeConfig final { - zdl::DlSystem::Runtime_t runtime; - zdl::DlSystem::RuntimeList runtimeList; - zdl::DlSystem::PerformanceProfile_t perfProfile; - bool enableCPUFallback; - RuntimeConfig(): runtime{zdl::DlSystem::Runtime_t::CPU_FLOAT32}, - perfProfile{zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE}, - enableCPUFallback{false} - {} - RuntimeConfig(const RuntimeConfig& other) - { - runtime = other.runtime; - runtimeList = other.runtimeList; - perfProfile = other.perfProfile; - enableCPUFallback = other.enableCPUFallback; - } + zdl::DlSystem::Runtime_t runtime; + zdl::DlSystem::RuntimeList runtimeList; + zdl::DlSystem::PerformanceProfile_t perfProfile; + zdl::DlSystem::TensorShapeMap inputDimensionsMap; + bool enableCPUFallback; + RuntimeConfig() + : runtime{zdl::DlSystem::Runtime_t::CPU_FLOAT32}, + perfProfile{zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE}, + enableCPUFallback{false} {} + RuntimeConfig(const RuntimeConfig& other) { + runtime = other.runtime; + runtimeList = other.runtimeList; + perfProfile = other.perfProfile; + enableCPUFallback = other.enableCPUFallback; + inputDimensionsMap = other.inputDimensionsMap; + } - RuntimeConfig& operator=(const RuntimeConfig &other) - { - this->runtimeList = other.runtimeList; - this->runtime = other.runtime; - this->perfProfile = other.perfProfile; - this->enableCPUFallback = other.enableCPUFallback; - return *this; - } - - ~RuntimeConfig() {} + RuntimeConfig& operator=(const RuntimeConfig& other) { + this->runtimeList = other.runtimeList; + this->runtime = other.runtime; + this->perfProfile = other.perfProfile; + this->enableCPUFallback = other.enableCPUFallback; + this->inputDimensionsMap = other.inputDimensionsMap; + return *this; + } + ~RuntimeConfig() {} }; /** -* @brief . -* -* The class for creating a RuntimeConfig container. -* -*/ -class ZDL_EXPORT RuntimeConfigList final -{ -public: - RuntimeConfigList(); - RuntimeConfigList(const size_t size); - void push_back(const RuntimeConfig &runtimeConfig); - RuntimeConfig& operator[](const size_t index); - RuntimeConfigList& operator =(const RuntimeConfigList &other); - size_t size() const noexcept; - size_t capacity() const noexcept; - void clear() noexcept; - ~RuntimeConfigList() = default; - -private: - void swap(const RuntimeConfigList &other); - std::vector m_runtimeConfigs; + * @brief . + * + * The class for creating a RuntimeConfig container. + * + */ +class ZDL_EXPORT RuntimeConfigList final { + public: + RuntimeConfigList(); + RuntimeConfigList(const size_t size); + void push_back(const RuntimeConfig& runtimeConfig); + RuntimeConfig& operator[](const size_t index); + RuntimeConfigList& operator=(const RuntimeConfigList& other); + size_t size() const noexcept; + size_t capacity() const noexcept; + void clear() noexcept; + ~RuntimeConfigList() = default; + private: + void swap(const RuntimeConfigList& other); + std::vector m_runtimeConfigs; }; /** @} */ /* end_addtogroup c_plus_plus_apis C++ */ -} // namespace PSNPE -} // namespace zdl -#endif //PSNPE_RUNTIMECONFIGLIST_HPP +} // namespace PSNPE +} // namespace zdl +#endif // PSNPE_RUNTIMECONFIGLIST_HPP diff --git a/phonelibs/snpe/include/SNPE/SNPE.hpp b/phonelibs/snpe/include/SNPE/SNPE.hpp index e0821b90b..239690ab2 100644 --- a/phonelibs/snpe/include/SNPE/SNPE.hpp +++ b/phonelibs/snpe/include/SNPE/SNPE.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2015-2017 Qualcomm Technologies, Inc. +// Copyright (c) 2015-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -9,9 +9,6 @@ #ifndef _SNPE_SNPE_HPP_ #define _SNPE_SNPE_HPP_ -#include -#include - #include "DlSystem/DlOptional.hpp" #include "DlSystem/DlVersion.hpp" #include "DlSystem/IBufferAttributes.hpp" diff --git a/phonelibs/snpe/include/SnpeUdo/UdoBase.h b/phonelibs/snpe/include/SnpeUdo/UdoBase.h new file mode 100644 index 000000000..be370e1ac --- /dev/null +++ b/phonelibs/snpe/include/SnpeUdo/UdoBase.h @@ -0,0 +1,483 @@ +//============================================================================== +// +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +#ifndef SNPE_UDO_BASE_H +#define SNPE_UDO_BASE_H + +#include + +// Provide values to use for API version. +#define API_VERSION_MAJOR 1 +#define API_VERSION_MINOR 5 +#define API_VERSION_TEENY 0 + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +// Defines a bitmask of enum values. +typedef uint32_t SnpeUdo_Bitmask_t; + +// A string of characters, rather than an array of bytes. +// Assumed to be UTF-8. +typedef char* SnpeUdo_String_t; + +// The maximum allowable length of a SnpeUdo_String_t in bytes, +// including null terminator. SNPE will truncate strings longer +// than this. +#define SNPE_UDO_MAX_STRING_SIZE 1024 + +/** + * An enum which holds the various error types. + * The error types are divided to classes : + * 0 - 99 : generic errors + * 100 - 200 : errors related to configuration + * + */ +typedef enum +{ + /// No Error + SNPE_UDO_NO_ERROR = 0, + /// Unsupported value for core type + SNPE_UDO_WRONG_CORE = 1, + /// Invalid attribute/argument passed into UDO API + SNPE_UDO_INVALID_ARGUMENT = 2, + /// Unsupported feature error + SNPE_UDO_UNSUPPORTED_FEATURE = 3, + /// Error relating to memory allocation + SNPE_UDO_MEM_ALLOC_ERROR = 4, + /* Configuration Specific errors */ + /// No op with given attributes available in library + SNPE_UDO_WRONG_OPERATION = 100, + /// Unsupported value for core type in UDO configuration + SNPE_UDO_WRONG_CORE_TYPE = 101, + /// Wrong number of params in UDO definition + SNPE_UDO_WRONG_NUM_OF_PARAMS = 102, + /// Wrong number of dimensions for tensor(s) in UDO definition + SNPE_UDO_WRONG_NUM_OF_DIMENSIONS = 103, + /// Wrong number of input tensors in UDO definition + SNPE_UDO_WRONG_NUM_OF_INPUTS = 104, + /// Wrong number of output tensors in UDO definition + SNPE_UDO_WRONG_NUM_OF_OUTPUTS = 105, + SNPE_UDO_PROGRAM_CACHE_NOT_FOUND = 106, + SNPE_UDO_UNKNOWN_ERROR = 0xFFFFFFFF +} SnpeUdo_ErrorType_t; + +/** + * An enum which holds the various data types. + * Designed to be used as single values or combined into a bitfield parameter + * (0x1, 0x2, 0x4, etc) + * \n FIXED_XX types are targeted for data in tensors. + * \n UINT / INT types are targeted for scalar params + */ +typedef enum +{ + /// data type: 16-bit floating point + SNPE_UDO_DATATYPE_FLOAT_16 = 0x01, + /// data type: 32-bit floating point + SNPE_UDO_DATATYPE_FLOAT_32 = 0x02, + /// data type: 4-bit fixed point + SNPE_UDO_DATATYPE_FIXED_4 = 0x04, + /// data type: 8-bit fixed point + SNPE_UDO_DATATYPE_FIXED_8 = 0x08, + /// data type: 16-bit fixed point + SNPE_UDO_DATATYPE_FIXED_16 = 0x10, + /// data type: 32-bit fixed point + SNPE_UDO_DATATYPE_FIXED_32 = 0x20, + /// data type: 8-bit unsigned integer + SNPE_UDO_DATATYPE_UINT_8 = 0x100, + /// data type: 16-bit unsigned integer + SNPE_UDO_DATATYPE_UINT_16 = 0x200, + /// data type: 32-bit unsigned integer + SNPE_UDO_DATATYPE_UINT_32 = 0x400, + /// data type: 8-bit signed integer + SNPE_UDO_DATATYPE_INT_8 = 0x1000, + /// data type: 16-bit signed integer + SNPE_UDO_DATATYPE_INT_16 = 0x2000, + /// data type: 32-bit signed integer + SNPE_UDO_DATATYPE_INT_32 = 0x4000, + SNPE_UDO_DATATYPE_LAST = 0xFFFFFFFF +} SnpeUdo_DataType_t; + +/** + * An enum which holds the various layouts. + * Designed to be used as single values or combined into a bitfield parameter + * (0x1, 0x2, 0x4, etc) + */ +typedef enum +{ + /// data layout (4D): NHWC (batch-height-width-channel) + SNPE_UDO_LAYOUT_NHWC = 0x01, + /// data layout (4D): NCHW (batch-channel-height-width) + SNPE_UDO_LAYOUT_NCHW = 0x02, + /// data layout (5D): NDHWC (batch-dimension-height-width-channel) + SNPE_UDO_LAYOUT_NDHWC = 0x04, + SNPE_UDO_LAYOUT_GPU_OPTIMAL1 = 0x08, + SNPE_UDO_LAYOUT_GPU_OPTIMAL2 = 0x10, + SNPE_UDO_LAYOUT_DSP_OPTIMAL1 = 0x11, + SNPE_UDO_LAYOUT_DSP_OPTIMAL2 = 0x12, + // Indicates no data will be allocated for this tensor. + // Used to specify optional inputs/outputs positionally. + SNPE_UDO_LAYOUT_NULL = 0x13, + SNPE_UDO_LAYOUT_LAST = 0xFFFFFFFF +} SnpeUdo_TensorLayout_t; + +/** + * An enum which holds the UDO library Core type . + * Designed to be used as single values or combined into a bitfield parameter + * (0x1, 0x2, 0x4, etc) + */ +typedef enum +{ + /// Library target IP Core is undefined + SNPE_UDO_CORETYPE_UNDEFINED = 0x00, + /// Library target IP Core is CPU + SNPE_UDO_CORETYPE_CPU = 0x01, + /// Library target IP Core is GPU + SNPE_UDO_CORETYPE_GPU = 0x02, + /// Library target IP Core is DSP + SNPE_UDO_CORETYPE_DSP = 0x04, + SNPE_UDO_CORETYPE_LAST = 0xFFFFFFFF +} SnpeUdo_CoreType_t; + +/** + * An enum to specify the parameter type : Scalar or Tensor + */ +typedef enum +{ + /// UDO static param type: scalar + SNPE_UDO_PARAMTYPE_SCALAR, + /// UDO static param type: string + SNPE_UDO_PARAMTYPE_STRING, + /// UDO static param type: tensor + SNPE_UDO_PARAMTYPE_TENSOR, + SNPE_UDO_PARAMTYPE_LAST = 0xFFFFFFFF +} SnpeUdo_ParamType_t; + +/** + * An enum to specify quantization type + */ +typedef enum +{ + /// Tensor Quantization type: NONE. Signifies unquantized tensor data + SNPE_UDO_QUANTIZATION_NONE, + /// Tensor Quantization type: Tensorflow-style + SNPE_UDO_QUANTIZATION_TF, + SNPE_UDO_QUANTIZATION_QMN, + SNPE_UDO_QUANTIZATION_LAST = 0xFFFFFFFF +} SnpeUdo_QuantizationType_t; + +/** + * @brief A struct which is used to provide a version number using 3 values : major, minor, teeny + * + */ +typedef struct +{ + /// version field: major - for backward-incompatible changes + uint32_t major; + /// version field: minor - for backward-compatible feature updates + uint32_t minor; + /// version field: teeny - for minor bug-fixes and clean-up + uint32_t teeny; +} SnpeUdo_Version_t; + +/** + * @brief A struct returned from version query, contains the Library version and API version + * + */ +typedef struct +{ + /// Version of UDO library. Controlled by users + SnpeUdo_Version_t libVersion; + /// Version of SNPE UDO API used in compiling library. Determined by SNPE + SnpeUdo_Version_t apiVersion; +} SnpeUdo_LibVersion_t; + +/** + * @brief A union to hold the value of a generic type. Allows defining a parameter struct + * in a generic way, with a "value" location that holds the data regardless of the type. + * + */ +typedef union +{ + /// value type: float + float floatValue; + /// value type: unsigned 32-bit integer + uint32_t uint32Value; + /// value type: signed 32-bit integer + int32_t int32Value; + /// value type: unsigned 16-bit integer + uint16_t uint16Value; + /// value type: signed 16-bit integer + int16_t int16Value; + /// value type: unsigned 8-bit integer + uint8_t uint8Value; + /// value type: signed 8-bit integer + int8_t int8Value; +} SnpeUdo_Value_t; + +/** + * @brief A struct which defines a scalar parameter : name, data type, and union of values + * + */ +typedef struct +{ + /// The parameter data type : float, int, etc. + SnpeUdo_DataType_t dataType; + /// a union of specified type which holds the data + SnpeUdo_Value_t dataValue; +} SnpeUdo_ScalarParam_t; + +/** + * @brief A struct which defines the quantization parameters in case of Tensorflow style quantization + * + */ +typedef struct +{ + /// minimum value of the quantization range of data + float minValue; + /// maximum value of the quantization range of data + float maxValue; +} SnpeUdo_TFQuantize_t; + +/** + * @brief A struct which defines the quantization type, and union of supported quantization structs + * + */ +typedef struct +{ + /// quantization type (only TF-style currently supported) + SnpeUdo_QuantizationType_t quantizeType; + union + { + /// TF-style min-max quantization ranges + SnpeUdo_TFQuantize_t TFParams; + }; +} SnpeUdo_QuantizeParams_t; + +/** + * @brief A struct which defines the datatype associated with a specified core-type + * This should be used to denote the datatypes for a single tensor info, depending + * on the intended execution core. + * + */ +typedef struct +{ + /// The IP Core + SnpeUdo_CoreType_t coreType; + /// The associated datatype for this coreType + SnpeUdo_DataType_t dataType; +} SnpeUdo_PerCoreDatatype_t; + +/** + * @brief A struct which defines a tensor parameter : name, data type, layout, quantization, more. + * Also holds a pointer to the tensor data. + * + */ +typedef struct +{ + /// The maximum allowable dimensions of the tensor. The memory held in + /// _tensorData_ is guaranteed to be large enough for this. + uint32_t* maxDimensions; + /// The current dimensions of the tensor. An operation may modify the current + /// dimensions of its output, to indicate cases where the output has been + /// "resized". + /// Note that for static parameters, the current and max dimensions must + /// match. + uint32_t* currDimensions; + /// Quantization params applicable to the tensor. Currently only supports + /// Tensorflow quantization style. + SnpeUdo_QuantizeParams_t quantizeParams; + /// Number of dimensions to the tensor: 3D, 4D, etc. + uint32_t tensorRank; + /// The parameter data type: float, int, etc. + SnpeUdo_DataType_t dataType; + /// The tensor layout type: NCHW, NHWC, etc. + SnpeUdo_TensorLayout_t layout; + /// Opaque pointer to tensor data. User may be required to re-interpret the pointer + /// based on core-specific definitions. + void* tensorData; +} SnpeUdo_TensorParam_t; + +/** + * @brief A struct which defines tensor information for activation tensors only + * + * It describes an activation tensor object using its name, the intended layout and the datatype + * it will take depending on the intended runtime core. The repeated field indicates that + * that the tensor info describes several input/output activation tensors, which all share the + * aforementioned properties. + */ +typedef struct +{ + /// The tensor name + SnpeUdo_String_t tensorName; + /// The tensor layout type: NCHW, NHWC, etc. + SnpeUdo_TensorLayout_t layout; + /// The per core datatype: {SNPE_UDO_DATATYPE, SNPE_UDO_CORE_TYPE} + SnpeUdo_PerCoreDatatype_t* perCoreDatatype; + /// A boolean field indicating that this tensorinfo will be repeated e.x for ops such as Concat or Split + bool repeated; + +} SnpeUdo_TensorInfo_t; + + +/** + * @brief struct which defines a UDO parameter - a union of scalar, tensor and string parameters + * + */ +typedef struct +{ + /// Type is scalar or tensor + SnpeUdo_ParamType_t paramType; + /// The param name, for example : "offset", "activation_type" + SnpeUdo_String_t paramName; + union + { + /// scalar param value + SnpeUdo_ScalarParam_t scalarParam; + /// tensor param value + SnpeUdo_TensorParam_t tensorParam; + /// string param value + SnpeUdo_String_t stringParam; + }; +} SnpeUdo_Param_t; + +/** + * @brief A struct which defines Operation information which is specific for IP core (CPU, GPU, DSP ...) + * + */ +typedef struct +{ + /// The IP Core + SnpeUdo_CoreType_t udoCoreType; + /// Bitmask, defines supported internal calculation types (like FLOAT_32, etc) + /// Based on SnpeUdo_DataType + SnpeUdo_Bitmask_t operationCalculationTypes; +} SnpeUdo_OpCoreInfo_t; + +/** + * @brief A struct which defines the common and core-specific Operation information + * + */ +typedef struct +{ + /// Operation type + SnpeUdo_String_t operationType; + /// A bitmask describing which IP Cores (CPU, GPU, DSP ...) support this operation + /// Translated based on SnpeUdo_CoreType + SnpeUdo_Bitmask_t supportedByCores; + /// Number of static parameters defined by the op + uint32_t numOfStaticParams; + /// Array of static parameters. Can be scalar or tensor params + SnpeUdo_Param_t* staticParams; + /// Number of input tensors this op receives + uint32_t numOfInputs; + /// Array of input tensor names to this operation + SnpeUdo_String_t* inputNames; + /// Number of output tensors this op receives + uint32_t numOfOutputs; + /// Array of output tensor names to this operation + SnpeUdo_String_t* outputNames; + /// Number of cores that the op can execute on + uint32_t numOfCoreInfo; + /// Array of per-core information entries + SnpeUdo_OpCoreInfo_t* opPerCoreInfo; + /// Array of input tensor infos for this operation + SnpeUdo_TensorInfo_t* inputInfos; + /// Array of output tensor infos for this operation + SnpeUdo_TensorInfo_t* outputInfos; +} SnpeUdo_OperationInfo_t; + +/** + * @brief A struct which provides the implementation library info : type, name + * + */ +typedef struct +{ + /// Defines the IP Core that this implementation library is targeting + SnpeUdo_CoreType_t udoCoreType; + /// library name. will be looked at in the standard library path + SnpeUdo_String_t libraryName; +} SnpeUdo_LibraryInfo_t; + +/** + * @brief A struct returned by the registration library and contains information on the UDO package : + * name, operations, libraries, etc. + * + */ +typedef struct +{ + /// A string containing the package name + SnpeUdo_String_t packageName; + /// A bitmask describing supported IP cores (CPU, GPU, DSP ...) + /// Translated based on SnpeUdo_CoreType + SnpeUdo_Bitmask_t supportedCoreTypes; + /// The number of implementation libraries in the package + uint32_t numOfImplementationLib; + /// Array of implementation libraries names/types + SnpeUdo_LibraryInfo_t* implementationLib; + /// A string containing all operation types separated by space + SnpeUdo_String_t operationsString; + /// Number of supported operations + uint32_t numOfOperations; + /// Array of Operation info structs. Each entry describes one + /// Operation (name, params, inputs, outputs) + SnpeUdo_OperationInfo_t* operationsInfo; +} SnpeUdo_RegInfo_t; + +/** +* @brief A struct returned by the implementation library and contains information on the +* specific library: name, IP Core, operations, etc. +* +*/ +typedef struct +{ + /// Defines the IP Core that this implementation library is targeting + SnpeUdo_CoreType_t udoCoreType; + /// A string containing the package name + SnpeUdo_String_t packageName; + /// A string containing all operation types separated by space + SnpeUdo_String_t operationsString; + /// Number of supported operations + uint32_t numOfOperations; +} SnpeUdo_ImpInfo_t; + +/** + * @brief This struct defines an operation. It is used for validation + * or creation of an operation. + * In case of using it for creation, the static params which are tensors + * contain pointers to the real data (weights, for example), and input/output + * tensors also include pointers to the buffers used. + */ +typedef struct +{ + /// The IP Core that the operation is defined for - CPU, GPU, DSP... + SnpeUdo_CoreType_t udoCoreType; + /// Operation type + SnpeUdo_String_t operationType; + /// The number of static parameters provided in the staticParams array. + /// this number has to match the number provided by the UDO Registration library information + uint32_t numOfStaticParams; + /// Array of static parameters + SnpeUdo_Param_t* staticParams; + /// The number of input parameters provided in inputs array. + /// this number has to match the number provided by the UDO Registration library information + uint32_t numOfInputs; + /// Array of input tensors, providing layout, data type, sizes, etc + /// When used to create an operation, also contains the initial location of the data + SnpeUdo_TensorParam_t* inputs; + /// The number of output parameters provided in inputs array. + /// this number has to match the number provided by the UDO Registration library information + uint32_t numOfOutputs; + /// Array of output tensors, providing layout, data type, sizes, etc + /// When used to create an operation, also contains the initial location of the data + SnpeUdo_TensorParam_t* outputs; +} SnpeUdo_OpDefinition_t; + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#endif //SNPE_UDO_BASE_H diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImpl.h b/phonelibs/snpe/include/SnpeUdo/UdoImpl.h new file mode 100644 index 000000000..bd29bc251 --- /dev/null +++ b/phonelibs/snpe/include/SnpeUdo/UdoImpl.h @@ -0,0 +1,323 @@ +//============================================================================== +// +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +#ifndef SNPE_UDO_IMPL_H +#define SNPE_UDO_IMPL_H + +#include + +#include "SnpeUdo/UdoShared.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +typedef struct _SnpeUdo_OpFactory_t* SnpeUdo_OpFactory_t; +typedef struct _SnpeUdo_Operation_t* SnpeUdo_Operation_t; + +/** + * @brief Initialize the shared library's data structures. Calling any other + * library function before this one will result in error. + * + * @param[in] globalInfrastructure Global core-specific infrastructure to be + * used by operations created in this library. The definition and + * semantics of this object will be defined in the corresponding + * implementation header for the core type. + * @return Error code + */ +SnpeUdo_ErrorType_t +SnpeUdo_initImplLibrary(void* globalInfrastructure); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_InitImplLibraryFunction_t)(void*); + +/** + * @brief A function to query the API version of the UDO implementation library. + * The function populates a SnpeUdo_LibVersion_t struct, which contains a SnpeUdo_Version_t + * struct for API version and library version. + * + * @param[in, out] version A pointer to struct which contains major, minor, teeny information for + * library and api versions. + * + * @return Error code + */ +SnpeUdo_ErrorType_t +SnpeUdo_getImplVersion(SnpeUdo_LibVersion_t** version); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_getImplVersion_t)(SnpeUdo_LibVersion_t** version); + +/** + * @brief Release the shared library's data structures, and invalidate any + * handles returned by the library. The behavior of any outstanding + * asynchronous calls made to this library when this function is called + * are undefined. All library functions (except SnpeUdo_initImplLibrary) will + * return an error after this function has been successfully called. + * + * It should be possible to call SnpeUdo_initImplLibrary after calling this + * function, and re-initialize the library. + * + * @return Error code + */ +SnpeUdo_ErrorType_t +SnpeUdo_terminateImplLibrary(void); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_TerminateImplLibraryFunction_t)(void); + + +/** + * @brief A function to query info on the UDO implementation library. + * The function populates a structure which contains information about + * operations that are part of this library + * + * @param[in, out] implementationInfo A pointer to struct which contains information + * on the operations + * + * @return error code + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_getImpInfo(SnpeUdo_ImpInfo_t** implementationInfo); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_GetImpInfoFunction_t)(SnpeUdo_ImpInfo_t** implementationInfo); + +/** + * @brief A function to create an operation factory. + * The function receives the operation type, and an array of static parameters, + * and returns operation factory handler + * + * @param[in] udoCoreType The Core type to create the operation on. An error will + * be returned if this does not match the core type of the library. + * + * @param[in] perFactoryInfrastructure CreateOpFactory infrastructure appropriate to this + * core type. The definition and semantics of this object will be defined + * in the corresponding implementation header for the core type. + * + * @param[in] operationType A string containing Operation type. for example "MY_CONV" + * + * @param[in] numOfStaticParams The number of static parameters. + * + * @param[in] staticParams Array of static parameters + * + * @param[in,out] opFactory Handler to Operation Factory, to be used when creating operations + * + * @return Error Code + */ +SnpeUdo_ErrorType_t +SnpeUdo_createOpFactory(SnpeUdo_CoreType_t udoCoreType, + void* perFactoryInfrastructure, + SnpeUdo_String_t operationType, + uint32_t numOfStaticParams, + SnpeUdo_Param_t* staticParams, + SnpeUdo_OpFactory_t* opFactory); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_CreateOpFactoryFunction_t)(SnpeUdo_CoreType_t, + void*, + SnpeUdo_String_t, + uint32_t, + SnpeUdo_Param_t*, + SnpeUdo_OpFactory_t*); + + +/** + * @brief A function to release the resources allocated for an operation factory + * created by this library. + * + * @param[in] factory The operation factory to release. Upon success this handle will be invalidated. + * + * @return Error Code + */ +SnpeUdo_ErrorType_t +SnpeUdo_releaseOpFactory(SnpeUdo_OpFactory_t opFactory); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_ReleaseOpFactoryFunction_t)(SnpeUdo_OpFactory_t); + +/** + * @brief A function to create an operation from the factory. + * The function receives array of inputs and array of outputs, and creates an operation + * instance, returning the operation instance handler. + * + * @param[in] opFactory OpFactory instance containing the parameters for this operation. + * + * @param[in] perOpInfrastructure Per-Op infrastructure for this operation. The definition + * and semantics of this object will be defined in the implementation header + * appropriate to this core type. + * + * @param[in] numOfInputs The number of input tensors this operation will receive. + * + * @param[in] inputs Array of input tensors, providing both the sizes and initial + * location of the data. + * + * @param[in] numOfOutputs Number of output tensors this operation will produce. + * + * @param[in] outputs Array of output tensors, providing both the sizes and + * initial location of the data. + * + * @param[in,out] operation Handle for newly created operation instance. + * + * @return Error Code + */ +SnpeUdo_ErrorType_t +SnpeUdo_createOperation(SnpeUdo_OpFactory_t opFactory, + void* perOpInfrastructure, + uint32_t numOfInputs, + SnpeUdo_TensorParam_t* inputs, + uint32_t numOfOutputs, + SnpeUdo_TensorParam_t* outputs, + SnpeUdo_Operation_t* operation); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_CreateOperationFunction_t)(SnpeUdo_OpFactory_t, + void*, + uint32_t, + SnpeUdo_TensorParam_t*, + uint32_t, + SnpeUdo_TensorParam_t*, + SnpeUdo_Operation_t*); + +/** + * @brief A pointer to notification function. + * + * The notification function supports the non-blocking (e.g. asynchronous) execution use-case. + * In case an "executeUdoOp" function is called with "blocking" set to zero, and a + * notify function, this function will be called by the implementation library at the + * end of execution. The implementation library will pass the notify function the ID + * that was provided to it when "executeUdoOp" was called. + * + * @param[in] ID 32-bit value, that was provided to executeUdoOp by the calling entity. + * Can be used to track the notifications, in case of multiple execute calls issued. + * + * @return Error code + * + */ +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_ExternalNotify_t)(const uint32_t ID); + +/** + * @brief Operation execution function. + * + * Calling this function will run the operation on set of inputs, generating a set of outputs. + * The call can be blocking (synchronous) or non-blocking (asynchronous). To support the + * non-blocking mode, the calling entity can pass an ID and a notification function. + * At the end of the execution this notification function would be called, passing it the ID. + * NOTE: Asynchronous execution mode not supported in this release. + * + * @param[in] operation handle to the operation on which execute is invoked + * @param[in] blocking flag to indicate execution mode. + * If set, execution is blocking, + * e.g SnpeUdo_executeOp call does not return until execution is done. + * If not set, SnpeUdo_executeOp returns immediately, and the + * library will call the notification function (if set) when execution is done. + * + * @param[in] ID 32-bit number that can be used by the calling entity to track execution + * in case of non-blocking execution. + * For example, it can be a sequence number, increased by one on each call. + * + * @param[in] notifyFunc Pointer to notification function. if the pointer is set, and execution is + * non-blocking, the library will call this function at end of execution, + * passing the number provided as ID + * + * @return Error code + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_executeOp(SnpeUdo_Operation_t operation, + bool blocking, + const uint32_t ID, + SnpeUdo_ExternalNotify_t notifyFunc); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_ExecuteOpFunction_t)(SnpeUdo_Operation_t, + bool, + const uint32_t, + SnpeUdo_ExternalNotify_t); + +/** + * @brief A function to setting the inputs & outputs. part of SnpeUdo_Operation struct, + * returned from creation of a new operation instance. + * Not supported in this release. + * + * This function allows the calling entity to change some of the inputs and outputs + * between calls to execute. + * Note that the change is limited to changing the pointer to the tensor data only. + * Any other change may be rejected by the implementation library, causing + * immediate invalidation of the operation instance + * + * @param[in] operation Operation on which IO tensors are set + * + * @param[in] inputs array of tensor parameters. The calling entity may provide a subset of the + * operation inputs, providing only those that it wants to change. + * + * @param[in] outputs array of tensor parameters. The calling entity may provide a subset of the + * operation outputs, providing only those that it wants to change. + * + * @return Error code + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_setOpIO(SnpeUdo_Operation_t operation, + SnpeUdo_TensorParam_t* inputs, + SnpeUdo_TensorParam_t* outputs); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_SetOpIOFunction_t)(SnpeUdo_Operation_t, + SnpeUdo_TensorParam_t*, + SnpeUdo_TensorParam_t*); + +/** + * @brief A function to return execution times. + * + * This function can be called to query the operation execution times on the IP core + * on which the operation is run. The time is provided in micro-seconds + * + * @param[in] operation Handle to operation whose execution time is being profiled + * + * @param[in,out] executionTime pointer to a uint32 value.This function writes the operation + * execution time in usec into this value. + * + * @return Error code + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_profileOp(SnpeUdo_Operation_t operation, uint32_t *executionTime); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_ProfileOpFunction_t)(SnpeUdo_Operation_t, uint32_t*); + +/** + * @brief A function to release the operation instance + * \n When it is called, the implementation library needs to release all resources + * allocated for this operation instance. + * \n Note that all function pointers which are part of SnpeUdo_Operation become + * invalid once releaseUdoOp call returns. + * + * @param[in] operation Handle to operation to be released + * @return Error code + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_releaseOp(SnpeUdo_Operation_t operation); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_ReleaseOpFunction_t)(SnpeUdo_Operation_t); + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif //SNPE_UDO_IMPL_H diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImplCpu.h b/phonelibs/snpe/include/SnpeUdo/UdoImplCpu.h new file mode 100644 index 000000000..3bbe0638e --- /dev/null +++ b/phonelibs/snpe/include/SnpeUdo/UdoImplCpu.h @@ -0,0 +1,44 @@ +//============================================================================== +// +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +// Header to be used by a CPU UDO Implementation library + +#ifndef SNPE_UDO_IMPL_CPU_H +#define SNPE_UDO_IMPL_CPU_H + +#include + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * @brief This struct provides the infrastructure needed by a developer of + * CPU UDO Implementation library. + * + * The framework/runtime which loads the CPU UDO implementation library provides + * this infrastructure data to the loaded library at the time of op factory creation. + * as an opaque pointer. It contains hooks for the UDO library to invoke supported + * functionality at the time of execution + * + * @param getData function pointer to retrieve raw tensor data from opaque pointer + * passed into the UDO when creating an instance. + * @param getDataSize function pointer to retrieve tensor data size from opaque pointer + */ + +typedef struct +{ + /// function pointer to retrieve raw tensor data from opaque pointer + /// passed into the UDO when creating an instance. + float* (*getData)(void*); + /// function pointer to retrieve tensor data size from opaque pointer + size_t (*getDataSize) (void*); +} SnpeUdo_CpuInfrastructure_t; + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#endif // SNPE_UDO_IMPL_CPU_H \ No newline at end of file diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImplDsp.h b/phonelibs/snpe/include/SnpeUdo/UdoImplDsp.h new file mode 100644 index 000000000..7e85bf15d --- /dev/null +++ b/phonelibs/snpe/include/SnpeUdo/UdoImplDsp.h @@ -0,0 +1,187 @@ +//============================================================================== +// +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +//============================================================================== +/* + * THIS HEADER FILE IS COPIED FROM HEXAGON-NN PROJECT + * + */ +//============================================================================== + + +// Header to be used by a DSP Hexnn UDO Implementation library + +#ifndef SNPE_UDO_IMPL_DSP_H +#define SNPE_UDO_IMPL_DSP_H +#include +#include "SnpeUdo/UdoImpl.h" + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * @brief A function to validate that a set of params is supported by an operation + * This function is HexNN specific, use case is when registration library is not in use. + * Optional function. + * + * @param[in] operationType Operation type + * @param[in] numOfStaticParams Number of static params defined by the op + * @param[in] staticParams Array of static params to the op + * @return Error code, indicating if the operation can be created on this set of configuration or not. + * + */ + +SnpeUdo_ErrorType_t +SnpeUdo_validateOperation (SnpeUdo_String_t operationType, + uint32_t numOfStaticParams, + const SnpeUdo_Param_t* staticParams); + +typedef SnpeUdo_ErrorType_t (*SnpeUdo_ValidateOperationFunction_t) (SnpeUdo_String_t, + uint32_t, + const SnpeUdo_Param_t*); + + +// enum used for indicating input/outout tensor data layouts on DSP, plain vs d32 +typedef enum { + SNPE_UDO_DSP_TENSOR_LAYOUT_PLAIN, + SNPE_UDO_DSP_TENSOR_LAYOUT_D32 +} SnpeUdo_HexNNTensorLayout_t; + +/** + * @brief A function to query numbers of inputs and outputs, + * quantization type of each input and each output as arrays, + * and data layout (plain vs d32) of each input and each output as arrays + * of an operation. + * inputsQuantTypes and inputsLayouts should point to arrays of size numOfInputs + * outputsQuantTypes and outputsLayouts should point to arrays of size numOfOutputs + * + * Note: inputsLayouts and inputsLayouts can point to NULL, in this case, it is + * assumed all inputs and/or outputs have plain data layouts, i.e. no D32 + * + * @param[in] operationType Operation type + * @param[in] numOfStaticParams Number of static params defined by the op + * @param[in] staticParams Array of static params to the op + * @param[in,out] numOfInputs Number of input tensors to the op + * @param[in,out] inputsQuantTypes Array of Quantization info for each input tensor + * @param[in,out] inputsLayouts Array of layout type for each input tensor + * @param[in,out] numOfOutputs Number of output tensors to the op + * @param[in,out] outputsQuantTypes Array of Quantization info for each output tensor + * @param[in,out] outputsLayouts Array of layout type for each output tensor + * @return error code, indicating status of query + */ + +SnpeUdo_ErrorType_t +SnpeUdo_queryOperation (SnpeUdo_String_t operationType, + uint32_t numOfStaticParams, + const SnpeUdo_Param_t* staticParams, + uint32_t* numOfInputs, + SnpeUdo_QuantizationType_t** inputsQuantTypes, + SnpeUdo_HexNNTensorLayout_t** inputsLayouts, + uint32_t* numOfOutputs, + SnpeUdo_QuantizationType_t** outputsQuantTypes, + SnpeUdo_HexNNTensorLayout_t** outputsLayouts); + +typedef SnpeUdo_ErrorType_t (*SnpeUdo_QueryOperationFunction_t) (SnpeUdo_String_t, + uint32_t, + const SnpeUdo_Param_t*, + uint32_t*, + SnpeUdo_QuantizationType_t**, + SnpeUdo_HexNNTensorLayout_t**, + uint32_t*, + SnpeUdo_QuantizationType_t**, + SnpeUdo_HexNNTensorLayout_t**); + + + +// Global infrastructure functions supported by Hexagon-NN v2 +typedef void (*workerThread_t) (void* perOpInfrastructure, void* userData); +typedef int (*udoSetOutputTensorSize_t) (void* perOpInfrastructure, uint32_t outIdx, uint32_t size); +typedef int (*udoGetInputD32Paddings_t) (void* perOpInfrastructure, uint32_t inIdx, + uint32_t* heightPadBefore, uint32_t* heightPadAfter, + uint32_t* widthPadBefore, uint32_t* widthPadAfter, + uint32_t* depthPadBefore, uint32_t* depthPadAfter); +typedef int (*udoSetOutputD32ShapeSizePaddings_t) (void* perOpInfrastructure, uint32_t outIdx, + uint32_t batch, + uint32_t height, uint32_t heightPadBefore, uint32_t heightPadAfter, + uint32_t width, uint32_t widthPadBefore, uint32_t widthPadAfter, + uint32_t depth, uint32_t depthPadBefore, uint32_t depthPadAfter, + SnpeUdo_DataType_t dataType); +typedef void* (*udoMemalign_t) (size_t n, size_t size); +typedef void* (*udoMalloc_t) (size_t size); +typedef void* (*udoCalloc_t) (size_t n, size_t size); +typedef void (*udoFree_t) (void* ptr); +typedef uint32_t (*udoGetVtcmSize_t) (void* perOpInfrastructure); +typedef void* (*udoGetVtcmPtr_t) (void* perOpInfrastructure); +typedef uint32_t (*udoVtcmIsReal_t) (void* perOpInfrastructure); +typedef void (*udoRunWorkerThreads_t) (void* perOpInfrastructure, uint32_t nThreads, workerThread_t w, void* userData); + +typedef struct hexNNv2GlobalInfra { + udoSetOutputTensorSize_t udoSetOutputTensorSize; + udoGetInputD32Paddings_t udoGetInputD32Paddings; + udoSetOutputD32ShapeSizePaddings_t udoSetOutputD32ShapeSizePaddings; + udoMemalign_t udoMemalign; + udoMalloc_t udoMalloc; + udoCalloc_t udoCalloc; + udoFree_t udoFree; + udoGetVtcmSize_t udoGetVtcmSize; + udoGetVtcmPtr_t udoGetVtcmPtr; + udoVtcmIsReal_t udoVtcmIsReal; + udoRunWorkerThreads_t udoRunWorkerThreads; +} SnpeUdo_HexNNv2GlobalInfra_t; + +// hexnn types +typedef enum hexnnInfraType { + UDO_INFRA_HEXNN_V2, + UDO_INFRA_HEXNN_V3 // reserved, do not use +} SnpeUdo_HexNNInfraType_t; + + +/** + * @brief Infrastructures needed by a developer of DSP Hexnn UDO Implementation library. + * + * The framework/runtime which loads the Hexnn UDO implementation library provides + * this infrastructure to the loaded library by calling "SnpeUdo_initImplLibrary" + * function, and passing it (cast to void*). The Hexnn UDO library is expected + * to cast it back to this structure. + * + */ +typedef struct dspGlobalInfrastructure { + SnpeUdo_Version_t dspInfraVersion; // api version + SnpeUdo_HexNNInfraType_t infraType; + SnpeUdo_HexNNv2GlobalInfra_t hexNNv2Infra; +} SnpeUdo_DspGlobalInfrastructure_t; + + +/** + * hexnn v2 per op factory infrastructure + * + * The framework/runtime passes per op factory infrastructure as a void pointer + * to HexNN UDO implementation library by calling function "SnpeUdo_createOpFactory". + * UDO implementation library is expected to cast it back to this following struct. + * + */ +typedef struct hexnnv2OpFactoryInfra { + unsigned long graphId; +} SnpeUdo_HexNNv2OpFactoryInfra_t; + + +/** + * hexnn v2 per operation infrastructure + * + * The framework/runtime passes per operation infrastructure as a void pointer + * to HexNN UDO implementation library by calling function "SnpeUdo_createOperation". + * UDO implementation library is expected to cast it to the following type and save it. + * + * This is needed to be passed back into some functions from global infrastructure. + * + */ +typedef void* SnpeUdo_HexNNv2OpInfra_t; + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#endif // SNPE_UDO_IMPL_DSP_H diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImplGpu.h b/phonelibs/snpe/include/SnpeUdo/UdoImplGpu.h new file mode 100644 index 000000000..1af654d11 --- /dev/null +++ b/phonelibs/snpe/include/SnpeUdo/UdoImplGpu.h @@ -0,0 +1,112 @@ +//============================================================================== +// +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +// Header to be used by a GPU UDO Implementation library + +#ifndef SNPE_UDO_IMPL_GPU_H +#define SNPE_UDO_IMPL_GPU_H + +#include "CL/cl.h" +#include "SnpeUdo/UdoBase.h" + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * This header defines version 0.0.0 of the GPU UDO Infrastructure. + * It defines the interpretation of the global and per-OpFactory infrastructure pointers + * as well as the interpretation of tensorData pointers. + * + * The per-Operation infrastructure pointer is defined to be null, and should not be used. + * + * The SnpeUdoTensorParam_t struct below provides the interpretation for + * the tensorData opaque pointer for SnpeUdoTensorParams representing inputs or outputs. + * + * The tensorData opaque pointer populated in SnpeUdoScalarParam_t structs should be interpreted + * as a host-readable data pointer. + * + */ + +/** + * @brief Function to retrieve opencl program from Program Cache repository. + * @param programCache is opaque pointer to Program Cache repository provided by + * SNPE GPU UDO runtime. + * @param programName is name associated with opencl program for UDO. + * @param program is pointer to opencl program which will be populated with + * valid opencl program if found in Program Cache repository. + * @return SnpeUdo_ErrorType_t is error type returned. SNPE_UDO_NO_ERROR is returned + * on success. + */ +typedef SnpeUdo_ErrorType_t (*SnpeUdo_getProgram_t) + (void* programCache, const char* programName, cl_program* program); + +/** + * @brief Function to store valid opencl program in Program Cache repository. + * @param programCache is opaque pointer to Program Cache repository provided by + * SNPE GPU UDO runtime. + * @param programName is name associated with opencl program for UDO. + * @param program is valid opencl program after program is built. + * @return SnpeUdo_ErrorType_t is error type returned. SNPE_UDO_NO_ERROR is returned + * on success. + * */ +typedef SnpeUdo_ErrorType_t (*SnpeUdo_storeProgram_t) + (void* programCache, const char * programName, cl_program program); + +/** + * @brief Global Infrastructure Definition for GPU UDO Implementations. + */ +typedef struct { + // Infrastructure definition version. This header is 0.0.0 + SnpeUdo_Version_t gpuInfraVersion; + SnpeUdo_getProgram_t SnpeUdo_getProgram; + SnpeUdo_storeProgram_t SnpeUdo_storeProgram; +} SnpeUdo_GpuInfrastructure_t; + +/** + * @brief Per OpFactory Infrastructure Definition for GPU UDO Implementations. + * @note This version of the infrastructure definition guarantees that the same + * Per OpFactory infrastructure pointer will be provided to all OpFactories + * in the same network. + */ +typedef struct +{ + cl_context context; + cl_command_queue commandQueue; + void* programCache; +} SnpeUdo_GpuOpFactoryInfrastructure_t; + +/** + * @brief Opaque tensorData definition for operation inputs and outputs. + * + * The following is a list of all SnpeUdoTensorLayout_t values supported by the + * GPU UDO implementation, and how the parameters of the struct should be + * interpreted in each case: + * + * SNPE_UDO_LAYOUT_NHWC: + * mem shall be single-element array, pointing to a cl buffer memory object. + * the dimensions of this object match the dimensions specified in the encompassing + * SnpeUdoTensorParam_t's currDimensions. + * + * memCount shall be 1. + * + * paddedRank and paddedDimensions are undefined and shall be ignored by the UDO + * implementation. + * + */ +typedef struct +{ + cl_mem* mem; + uint32_t memCount; + uint32_t paddedRank; + uint32_t* paddedDimensions; + +} SnpeUdo_GpuTensorData_t; + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#endif // SNPE_UDO_IMPL_GPU_H diff --git a/phonelibs/snpe/include/SnpeUdo/UdoReg.h b/phonelibs/snpe/include/SnpeUdo/UdoReg.h new file mode 100644 index 000000000..a5d239883 --- /dev/null +++ b/phonelibs/snpe/include/SnpeUdo/UdoReg.h @@ -0,0 +1,108 @@ +//============================================================================== +// +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +#ifndef SNPE_UDO_REG_H +#define SNPE_UDO_REG_H + +#include "SnpeUdo/UdoShared.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * @brief Initialize the shared library's data structures. Calling any other + * library function before this one will result in an error being returned. + * + * @return Error code + */ +SnpeUdo_ErrorType_t +SnpeUdo_initRegLibrary(void); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_InitRegLibraryFunction_t)(void); + +/** + * @brief A function to query the API version of the UDO registration library. + * The function populates a SnpeUdo_LibVersion_t struct, which contains a SnpeUdo_Version_t + * struct for API version and library version. + * + * @param[in, out] version A pointer to struct which contains major, minor, teeny information for + * library and api versions. + * + * @return Error code + */ +SnpeUdo_ErrorType_t +SnpeUdo_getRegLibraryVersion(SnpeUdo_LibVersion_t** version); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_getRegLibraryVersion_t)(SnpeUdo_LibVersion_t** version); + +/** + * @brief Release the shared library's data structures, and invalidate any + * handles returned by the library. The behavior of any outstanding + * asynchronous calls made to this library when this function is called + * are undefined. All library functions (except SnpeUdo_InitRegLibrary) will + * return an error after this function has been successfully called. + * + * It should be possible to call SnpeUdo_InitRegLibrary after calling this + * function, and re-initialize the library. + * + * @return Error code + */ +SnpeUdo_ErrorType_t +SnpeUdo_terminateRegLibrary(void); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_TerminateRegLibraryFunction_t)(void); + + +/** + * @brief A function to query the info on the UDO set. + * The function populates a structure which contains information about + * the package and operations contained in it. + * + * @param[in, out] registrationInfo A struct which contains information on the set of UDOs + * + * @return Error code + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_getRegInfo(SnpeUdo_RegInfo_t** registrationInfo); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_GetRegInfoFunction_t)(SnpeUdo_RegInfo_t** registrationInfo); + +/** + * @brief A function to validate that a set of params is supported by an operation + * The function receives an operation definition struct, and returns if this configuration is + * supported (e.g. if an operation can be created using this configuration) + * + * @param[in] opDefinition A struct of SnpeUdo_OpDefinition type, containing the information needed to + * validate that an operation can be created with this configuration. + * + * @return Error code, indicating is the operation can be created on this set or not. + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_validateOperation(SnpeUdo_OpDefinition_t* opDefinition); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_ValidateOperationFunction_t)(SnpeUdo_OpDefinition_t* opDefinition); + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif //SNPE_UDO_REG_H diff --git a/phonelibs/snpe/include/SnpeUdo/UdoShared.h b/phonelibs/snpe/include/SnpeUdo/UdoShared.h new file mode 100644 index 000000000..6fd50c318 --- /dev/null +++ b/phonelibs/snpe/include/SnpeUdo/UdoShared.h @@ -0,0 +1,46 @@ +//============================================================================== +// +// Copyright (c) 2019-2020 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +#ifndef SNPE_UDO_SHARED_H +#define SNPE_UDO_SHARED_H + +#include "SnpeUdo/UdoBase.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * @brief A function to return the various versions as they relate to the UDO + * The function returns a struct containing the the following: + * libVersion: the version of the implementation library compiled for the UDO. Set by user + * apiVersion: the version of the UDO API used in compiling the implementation library. + * Set by SNPE + * + * @param[in, out] version A pointer to Version struct of type SnpeUdo_LibVersion_t + * + * @return Error code + * + */ +SnpeUdo_ErrorType_t +SnpeUdo_getVersion (SnpeUdo_LibVersion_t** version); + +typedef SnpeUdo_ErrorType_t +(*SnpeUdo_GetVersionFunction_t) (SnpeUdo_LibVersion_t** version); + +#ifdef __cplusplus +} // extern "C" +#endif + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#endif // SNPE_UDO_SHARED_H diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 9823ad985..7fa6d7ae3 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -43,6 +43,7 @@ volatile sig_atomic_t do_exit = 0; bool spoofing_started = false; bool fake_send = false; bool connected_once = false; +bool ignition = false; struct tm get_time(){ time_t rawtime; @@ -70,7 +71,7 @@ void safety_setter_thread() { return; }; - std::vector value_vin = read_db_bytes("CarVin"); + std::vector value_vin = Params().read_db_bytes("CarVin"); if (value_vin.size() > 0) { // sanity check VIN format assert(value_vin.size() == 17); @@ -92,7 +93,7 @@ void safety_setter_thread() { return; }; - params = read_db_bytes("CarParams"); + params = Params().read_db_bytes("CarParams"); if (params.size() > 0) break; usleep(100*1000); } @@ -123,13 +124,15 @@ bool usb_connect() { return false; } + Params params = Params(); + if (getenv("BOARDD_LOOPBACK")) { panda->set_loopback(true); } const char *fw_sig_buf = panda->get_firmware_version(); if (fw_sig_buf){ - write_db_value("PandaFirmware", fw_sig_buf, 128); + params.write_db_value("PandaFirmware", fw_sig_buf, 128); // Convert to hex for offroad char fw_sig_hex_buf[16] = {0}; @@ -138,7 +141,7 @@ bool usb_connect() { fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF); } - write_db_value("PandaFirmwareHex", fw_sig_hex_buf, 16); + params.write_db_value("PandaFirmwareHex", fw_sig_hex_buf, 16); LOGW("fw signature: %.*s", 16, fw_sig_hex_buf); delete[] fw_sig_buf; @@ -149,7 +152,7 @@ bool usb_connect() { if (serial_buf) { size_t serial_sz = strnlen(serial_buf, 16); - write_db_value("PandaDongleId", serial_buf, serial_sz); + params.write_db_value("PandaDongleId", serial_buf, serial_sz); LOGW("panda serial: %.*s", serial_sz, serial_buf); delete[] serial_buf; @@ -190,10 +193,8 @@ void can_recv(PubMaster &pm) { // create message MessageBuilder msg; auto event = msg.initEvent(); - int recv = panda->can_receive(event); - if (recv){ - pm.send("can", msg); - } + panda->can_receive(event); + pm.send("can", msg); } void can_send_thread() { @@ -254,7 +255,9 @@ void can_recv_thread() { useconds_t sleep = remaining / 1000; usleep(sleep); } else { - LOGW("missed cycles (%d) %lld", (int)-1*remaining/dt, remaining); + if (ignition){ + LOGW("missed cycles (%d) %lld", (int)-1*remaining/dt, remaining); + } next_frame_time = cur_time; } @@ -268,6 +271,7 @@ void can_health_thread() { uint32_t no_ignition_cnt = 0; bool ignition_last = false; + Params params = Params(); // Broadcast empty health message when panda is not yet connected while (!panda){ @@ -295,7 +299,7 @@ void can_health_thread() { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } - bool ignition = ((health.ignition_line != 0) || (health.ignition_can != 0)); + ignition = ((health.ignition_line != 0) || (health.ignition_can != 0)); if (ignition) { no_ignition_cnt = 0; @@ -317,9 +321,9 @@ void can_health_thread() { // clear VIN, CarParams, and set new safety on car start if (ignition && !ignition_last) { - int result = delete_db_value("CarVin"); + int result = params.delete_db_value("CarVin"); assert((result == 0) || (result == ERR_NO_VALUE)); - result = delete_db_value("CarParams"); + result = params.delete_db_value("CarParams"); assert((result == 0) || (result == ERR_NO_VALUE)); if (!safety_setter_thread_running) { @@ -384,9 +388,6 @@ void hardware_control_thread() { LOGD("start hardware control thread"); SubMaster sm({"thermal", "frontFrame"}); - // Other pandas don't have hardware to control - if (panda->hw_type != cereal::HealthData::HwType::UNO && panda->hw_type != cereal::HealthData::HwType::DOS) return; - uint64_t last_front_frame_t = 0; uint16_t prev_fan_speed = 999; uint16_t ir_pwr = 0; @@ -400,15 +401,8 @@ void hardware_control_thread() { cnt++; sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? - if (sm.updated("thermal")){ - // Fan speed - uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeed(); - if (fan_speed != prev_fan_speed || cnt % 100 == 0){ - panda->set_fan_speed(fan_speed); - prev_fan_speed = fan_speed; - } - #ifdef QCOM + if (sm.updated("thermal")){ // Charging mode bool charging_disabled = sm["thermal"].getThermal().getChargingDisabled(); if (charging_disabled != prev_charging_disabled){ @@ -421,7 +415,18 @@ void hardware_control_thread() { } prev_charging_disabled = charging_disabled; } + } #endif + + // Other pandas don't have fan/IR to control + if (panda->hw_type != cereal::HealthData::HwType::UNO && panda->hw_type != cereal::HealthData::HwType::DOS) continue; + if (sm.updated("thermal")){ + // Fan speed + uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeed(); + if (fan_speed != prev_fan_speed || cnt % 100 == 0){ + panda->set_fan_speed(fan_speed); + prev_fan_speed = fan_speed; + } } if (sm.updated("frontFrame")){ auto event = sm["frontFrame"]; diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py index 4b0859018..01ca05248 100644 --- a/selfdrive/boardd/boardd_setup.py +++ b/selfdrive/boardd/boardd_setup.py @@ -16,7 +16,7 @@ setup(name='Boardd API Implementation', ], sources=['boardd_api_impl.pyx'], language="c++", - extra_compile_args=["-std=c++11"], + extra_compile_args=["-std=c++1z", "-Wno-nullability-completeness"], ) ) ) diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 21ce0c301..f0d7026db 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -321,10 +321,10 @@ int Panda::can_receive(cereal::Event::Builder &event){ uint32_t data[RECV_SIZE/4]; int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE); - // return if length is 0 - if (recv <= 0) { - return 0; - } else if (recv == RECV_SIZE) { + // Not sure if this can happen + if (recv < 0) recv = 0; + + if (recv == RECV_SIZE) { LOGW("Receive buffer full"); } diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index 7a81b76c8..e6b672ef4 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -1,6 +1,6 @@ -Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'webcam', 'QCOM_REPLAY') +Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM', 'QCOM_REPLAY') -libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', cereal, messaging, 'czmq', 'zmq', 'capnp', 'kj', visionipc, gpucommon] +libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon] if arch == "aarch64": libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui'] @@ -9,14 +9,14 @@ if arch == "aarch64": else: cameras = ['cameras/camera_qcom.cc'] elif arch == "larch64": - libs += [] - cameras = ['cameras/camera_qcom2.c'] + libs += ['atomic'] + cameras = ['cameras/camera_qcom2.cc'] # no screen # env = env.Clone() # env.Append(CXXFLAGS = '-DNOSCREEN') # env.Append(CFLAGS = '-DNOSCREEN') else: - if webcam: + if USE_WEBCAM: libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio'] cameras = ['cameras/camera_webcam.cc'] env = env.Clone() @@ -36,6 +36,7 @@ env.SharedLibrary('snapshot/visionipc', env.Program('camerad', [ 'main.cc', + 'cameras/camera_common.cc', 'transforms/rgb_to_yuv.c', 'imgproc/utils.cc', cameras, diff --git a/selfdrive/camerad/bufs.h b/selfdrive/camerad/bufs.h deleted file mode 100644 index 71930baed..000000000 --- a/selfdrive/camerad/bufs.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef _SELFDRIVE_VISIOND_VISIOND_H_ -#define _SELFDRIVE_VISIOND_VISIOND_H_ - -#include - -typedef struct { uint8_t *y, *u, *v; } YUVBuf; - -#endif // _SELFDRIVE_VISIOND_VISIOND_H_ diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc new file mode 100644 index 000000000..ac44903f5 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -0,0 +1,413 @@ +#include +#include +#include +#include +#include + +#if defined(QCOM) && !defined(QCOM_REPLAY) +#include "cameras/camera_qcom.h" +#elif QCOM2 +#include "cameras/camera_qcom2.h" +#elif WEBCAM +#include "cameras/camera_webcam.h" +#else +#include "cameras/camera_frame_stream.h" +#endif + +#include "camera_common.h" +#include +#include + +#include "clutil.h" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "imgproc/utils.h" + +static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b) { + char args[4096]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " + "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " + "-DBAYER_FLIP=%d -DHDR=%d", + ci->frame_width, ci->frame_height, ci->frame_stride, + 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); +#else + return CLU_LOAD_FROM_FILE(context, device_id, "cameras/debayer.cl", args); +#endif +} + +void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, int frame_cnt, + const char *name, release_cb relase_callback) { + const CameraInfo *ci = &s->ci; + 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++) { + camera_bufs[i] = visionbuf_allocate_cl(frame_size, device_id, context); + } + + rgb_width = ci->frame_width; + rgb_height = ci->frame_height; +#ifndef QCOM2 + // debayering does a 2x downscale + if (ci->bayer) { + rgb_width = ci->frame_width / 2; + rgb_height = ci->frame_height / 2; + } + float db_s = 0.5; +#else + float db_s = 1.0; +#endif + + if (ci->bayer) { + yuv_transform = transform_scale_buffer(s->transform, db_s); + } else { + yuv_transform = s->transform; + } + + for (int i = 0; i < UI_BUF_COUNT; i++) { + VisionImg img = visionimg_alloc_rgb24(device_id, context, rgb_width, rgb_height, &rgb_bufs[i]); + if (i == 0) { + rgb_stride = img.stride; + } + } + tbuffer_init(&ui_tb, UI_BUF_COUNT, name); + tbuffer_init2(&camera_tb, frame_buf_count, "frame", relase_callback, s); + + // yuv back for recording and orbd + pool_init(&yuv_pool, YUV_COUNT); + yuv_tb = pool_get_tbuffer(&yuv_pool); + + yuv_width = rgb_width; + yuv_height = rgb_height; + yuv_buf_size = rgb_width * rgb_height * 3 / 2; + + for (int i = 0; i < YUV_COUNT; i++) { + yuv_ion[i] = visionbuf_allocate_cl(yuv_buf_size, device_id, context); + yuv_bufs[i].y = (uint8_t *)yuv_ion[i].addr; + yuv_bufs[i].u = yuv_bufs[i].y + (yuv_width * yuv_height); + 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); + } + + 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); +#else + const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + q = clCreateCommandQueueWithProperties(context, device_id, props, &err); +#endif + assert(err == 0); +} + +CameraBuf::~CameraBuf() { + for (int i = 0; i < frame_buf_count; i++) { + visionbuf_free(&camera_bufs[i]); + } + for (int i = 0; i < UI_BUF_COUNT; i++) { + visionbuf_free(&rgb_bufs[i]); + } + for (int i = 0; i < YUV_COUNT; i++) { + visionbuf_free(&yuv_ion[i]); + } + clReleaseKernel(krnl_debayer); + clReleaseCommandQueue(q); +} + +bool CameraBuf::acquire() { + const int buf_idx = tbuffer_acquire(&camera_tb); + if (buf_idx < 0) { + return false; + } + const FrameMetadata &frame_data = camera_bufs_metadata[buf_idx]; + if (frame_data.frame_id == -1) { + LOGE("no frame data? wtf"); + tbuffer_release(&camera_tb, buf_idx); + return false; + } + + cur_frame_data = frame_data; + + cur_rgb_idx = tbuffer_select(&ui_tb); + cur_rgb_buf = &rgb_bufs[cur_rgb_idx]; + + 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); +#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); +#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); + const size_t debayer_work_size = rgb_height; // doesn't divide evenly, is this okay? + const size_t debayer_local_work_size = 128; + assert(clEnqueueNDRangeKernel(q, krnl_debayer, 1, NULL, + &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event) == 0); +#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); + } + + clWaitForEvents(1, &debayer_event); + clReleaseEvent(debayer_event); + + tbuffer_release(&camera_tb, buf_idx); + visionbuf_sync(cur_rgb_buf, VISIONBUF_SYNC_FROM_DEVICE); + + cur_yuv_idx = pool_select(&yuv_pool); + yuv_metas[cur_yuv_idx] = frame_data; + rgb_to_yuv_queue(&rgb_to_yuv_state, q, cur_rgb_buf->buf_cl, yuv_ion[cur_yuv_idx].buf_cl); + visionbuf_sync(&yuv_ion[cur_yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); + + // keep another reference around till were done processing + pool_acquire(&yuv_pool, cur_yuv_idx); + pool_push(&yuv_pool, cur_yuv_idx); + + return true; +} + +void CameraBuf::release() { + tbuffer_dispatch(&ui_tb, cur_rgb_idx); + pool_release(&yuv_pool, cur_yuv_idx); +} + +void CameraBuf::stop() { + tbuffer_stop(&ui_tb); + tbuffer_stop(&camera_tb); + pool_stop(&yuv_pool); +} + +// common functions + +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt) { + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); +} + +void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { + const CameraBuf *b = &c->buf; + + uint8_t* thumbnail_buffer = NULL; + unsigned long thumbnail_len = 0; + + unsigned char *row = (unsigned char *)malloc(b->rgb_width/4*3); + + struct jpeg_compress_struct cinfo; + struct jpeg_error_mgr jerr; + + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); + jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len); + + cinfo.image_width = b->rgb_width / 4; + cinfo.image_height = b->rgb_height / 4; + cinfo.input_components = 3; + cinfo.in_color_space = JCS_RGB; + + jpeg_set_defaults(&cinfo); +#ifndef __APPLE__ + jpeg_set_quality(&cinfo, 50, true); + jpeg_start_compress(&cinfo, true); +#else + jpeg_set_quality(&cinfo, 50, static_cast(true) ); + jpeg_start_compress(&cinfo, static_cast(true) ); +#endif + + JSAMPROW row_pointer[1]; + for (int ii = 0; ii < b->rgb_height/4; ii+=1) { + for (int j = 0; j < b->rgb_width*3; j+=12) { + for (int k = 0; k < 3; k++) { + uint16_t dat = 0; + int i = ii * 4; + dat += bgr_ptr[b->rgb_stride*i + j + k]; + dat += bgr_ptr[b->rgb_stride*i + j+3 + k]; + dat += bgr_ptr[b->rgb_stride*(i+1) + j + k]; + dat += bgr_ptr[b->rgb_stride*(i+1) + j+3 + k]; + dat += bgr_ptr[b->rgb_stride*(i+2) + j + k]; + dat += bgr_ptr[b->rgb_stride*(i+2) + j+3 + k]; + dat += bgr_ptr[b->rgb_stride*(i+3) + j + k]; + dat += bgr_ptr[b->rgb_stride*(i+3) + j+3 + k]; + row[(j/4) + (2-k)] = dat/8; + } + } + row_pointer[0] = row; + jpeg_write_scanlines(&cinfo, row_pointer, 1); + } + free(row); + jpeg_finish_compress(&cinfo); + + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initThumbnail(); + thumbnaild.setFrameId(b->cur_frame_data.frame_id); + thumbnaild.setTimestampEof(b->cur_frame_data.timestamp_eof); + thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len)); + + if (s->pm != NULL) { + s->pm->send("thumbnail", msg); + } +} + +void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { + const CameraBuf *b = &c->buf; + + uint32_t lum_binning[256] = {0}; + for (int y = y_start; y < y_end; y += y_skip) { + for (int x = x_start; x < x_end; x += x_skip) { + if (!front) { + uint8_t lum = pix_ptr[(y * b->yuv_width) + x]; + lum_binning[lum]++; + } else { + const uint8_t *pix = &pix_ptr[y * b->rgb_width * 3 + x * 3]; + unsigned int lum = (unsigned int)(pix[0] + pix[1] + pix[2]); + lum_binning[std::min(lum / 3, 255u)]++; + } + } + } + + unsigned int lum_total = (y_end - y_start) * (x_end - x_start) / x_skip / y_skip; + unsigned int lum_cur = 0; + int lum_med = 0; + int lum_med_alt = 0; + for (lum_med=255; lum_med>=0; lum_med--) { + lum_cur += lum_binning[lum_med]; +#ifdef QCOM2 + bool reach_hlc_perc = false; + if (c->camera_num == 0) { // wide + reach_hlc_perc = lum_cur > 2*lum_total / (3*HLC_A); + } else { + reach_hlc_perc = lum_cur > lum_total / HLC_A; + } + if (reach_hlc_perc && lum_med > HLC_THRESH) { + lum_med_alt = 86; + } +#endif + if (lum_cur >= lum_total / 2) { + break; + } + } + lum_med = lum_med_alt>lum_med?lum_med_alt:lum_med; + camera_autoexposure(c, lum_med / 256.0); +} + +extern volatile sig_atomic_t do_exit; + +void *processing_thread(MultiCameraState *cameras, const char *tname, + CameraState *cs, int priority, 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; + + callback(cameras, cs, cnt); + + cs->buf.release(); + } + return NULL; +} + +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); +} + +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 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 (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; + } +#ifdef QCOM2 + x_start = 96; + x_end = 1832; + y_start = 242; + y_end = 1148; + skip = 4; +#endif + set_exposure_target(c, (const uint8_t *)b->cur_rgb_buf->addr, 1, x_start, x_end, 2, y_start, y_end, skip); + } + + MessageBuilder msg; + auto framed = msg.initEvent().initFrontFrame(); + framed.setFrameType(cereal::FrameData::FrameType::FRONT); + fill_frame_data(framed, b->cur_frame_data, cnt); + pm->send("frontFrame", msg); +} diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 1bd8ac6c7..0170db27e 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -1,8 +1,19 @@ -#ifndef CAMERA_COMMON_H -#define CAMERA_COMMON_H +#pragma once -#include #include +#include +#include +#include +#include "common/buffering.h" +#include "common/mat.h" +#include "common/swaglog.h" +#include "common/visionbuf.h" +#include "common/visionimg.h" +#include "imgproc/utils.h" +#include "messaging.hpp" +#include "transforms/rgb_to_yuv.h" + +#include "common/visionipc.h" #define CAMERA_ID_IMX298 0 #define CAMERA_ID_IMX179 1 @@ -15,9 +26,13 @@ #define CAMERA_ID_AR0231 8 #define CAMERA_ID_MAX 9 -#ifdef __cplusplus -extern "C" { -#endif +#define UI_BUF_COUNT 4 +#define YUV_COUNT 40 +#define LOG_CAMERA_ID_FCAMERA 0 +#define LOG_CAMERA_ID_DCAMERA 1 +#define LOG_CAMERA_ID_ECAMERA 2 +#define LOG_CAMERA_ID_QCAMERA 3 +#define LOG_CAMERA_ID_MAX 4 typedef struct CameraInfo { const char* name; @@ -28,6 +43,19 @@ typedef struct CameraInfo { bool hdr; } CameraInfo; +typedef struct LogCameraInfo { + const char* filename; + const char* frame_packet_name; + const char* encode_idx_name; + VisionStreamType stream_type; + int frame_width, frame_height; + int fps; + int bitrate; + bool is_h265; + bool downscale; + bool has_qcamera; +} LogCameraInfo; + typedef struct FrameMetadata { uint32_t frame_id; uint64_t timestamp_eof; @@ -41,10 +69,67 @@ typedef struct FrameMetadata { float gain_frac; } FrameMetadata; +typedef struct CameraExpInfo { + int op_id; + float grey_frac; +} CameraExpInfo; + extern CameraInfo cameras_supported[CAMERA_ID_MAX]; -#ifdef __cplusplus -} -#endif +typedef struct { + uint8_t *y, *u, *v; +} YUVBuf; -#endif +struct MultiCameraState; +struct CameraState; +typedef void (*release_cb)(void *cookie, int buf_idx); + +class CameraBuf { +public: + + CameraState *camera_state; + cl_kernel krnl_debayer; + cl_command_queue q; + + Pool yuv_pool; + VisionBuf yuv_ion[YUV_COUNT]; + YUVBuf yuv_bufs[YUV_COUNT]; + FrameMetadata yuv_metas[YUV_COUNT]; + size_t yuv_buf_size; + int yuv_width, yuv_height; + RGBToYUVState rgb_to_yuv_state; + + int rgb_width, rgb_height, rgb_stride; + VisionBuf rgb_bufs[UI_BUF_COUNT]; + + mat3 yuv_transform; + + int cur_yuv_idx, cur_rgb_idx; + FrameMetadata cur_frame_data; + VisionBuf *cur_rgb_buf; + + + std::unique_ptr camera_bufs; + std::unique_ptr camera_bufs_metadata; + TBuffer camera_tb, ui_tb; + TBuffer *yuv_tb; // only for visionserver + + CameraBuf() = default; + ~CameraBuf(); + void init(cl_device_id device_id, cl_context context, CameraState *s, int frame_cnt, + const char *name = "frame", release_cb relase_callback = nullptr); + bool acquire(); + void release(); + void stop(); + int frame_buf_count; + int frame_size; +}; + +typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); + +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt); +void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr); +void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, 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); +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 9a4263103..a2acc018e 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -23,41 +23,35 @@ extern volatile sig_atomic_t do_exit; #define FRAME_HEIGHT 874 namespace { -void camera_open(CameraState *s, VisionBuf *camera_bufs, bool rear) { - assert(camera_bufs); - s->camera_bufs = camera_bufs; +void camera_open(CameraState *s, bool rear) { } void camera_close(CameraState *s) { - tbuffer_stop(&s->camera_tb); + s->buf.stop(); } -void camera_release_buffer(void *cookie, int buf_idx) {} - -void camera_init(CameraState *s, int camera_id, unsigned int fps) { +void camera_init(CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx) { assert(camera_id < ARRAYSIZE(cameras_supported)); s->ci = cameras_supported[camera_id]; assert(s->ci.frame_width != 0); - s->frame_size = s->ci.frame_height * s->ci.frame_stride; s->fps = fps; - - tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s); + s->buf.init(device_id, ctx, s, FRAME_BUF_COUNT, "camera"); } void run_frame_stream(MultiCameraState *s) { - SubMaster sm({"frame"}); + s->sm = new SubMaster({"frame"}); CameraState *const rear_camera = &s->rear; - auto *tb = &rear_camera->camera_tb; + auto *tb = &rear_camera->buf.camera_tb; while (!do_exit) { - if (sm.update(1000) == 0) continue; + if (s->sm->update(1000) == 0) continue; - auto frame = sm["frame"].getFrame(); + auto frame = (*(s->sm))["frame"].getFrame(); const int buf_idx = tbuffer_select(tb); - rear_camera->camera_bufs_metadata[buf_idx] = { + rear_camera->buf.camera_bufs_metadata[buf_idx] = { .frame_id = frame.getFrameId(), .timestamp_eof = frame.getTimestampEof(), .frame_length = static_cast(frame.getFrameLength()), @@ -65,8 +59,8 @@ void run_frame_stream(MultiCameraState *s) { .global_gain = static_cast(frame.getGlobalGain()), }; - cl_command_queue q = rear_camera->camera_bufs[buf_idx].copy_q; - cl_mem yuv_cl = rear_camera->camera_bufs[buf_idx].buf_cl; + cl_command_queue q = rear_camera->buf.camera_bufs[buf_idx].copy_q; + cl_mem yuv_cl = rear_camera->buf.camera_bufs[buf_idx].buf_cl; clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, frame.getImage().size(), frame.getImage().begin(), 0, NULL, NULL); tbuffer_dispatch(tb, buf_idx); @@ -93,15 +87,15 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { }, }; -void cameras_init(MultiCameraState *s) { - camera_init(&s->rear, CAMERA_ID_IMX298, 20); +void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { + camera_init(&s->rear, CAMERA_ID_IMX298, 20, device_id, ctx); s->rear.transform = (mat3){{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, }}; - camera_init(&s->front, CAMERA_ID_OV8865, 10); + camera_init(&s->front, CAMERA_ID_OV8865, 10, device_id, ctx); s->front.transform = (mat3){{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, @@ -111,25 +105,27 @@ void cameras_init(MultiCameraState *s) { void camera_autoexposure(CameraState *s, float grey_frac) {} -void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, - VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, - VisionBuf *camera_bufs_front) { - assert(camera_bufs_rear); - assert(camera_bufs_front); - +void cameras_open(MultiCameraState *s) { // LOG("*** open front ***"); - camera_open(&s->front, camera_bufs_front, false); + camera_open(&s->front, false); // LOG("*** open rear ***"); - camera_open(&s->rear, camera_bufs_rear, true); + camera_open(&s->rear, true); } void cameras_close(MultiCameraState *s) { camera_close(&s->rear); } +// called by processing_thread +void camera_process_rear(MultiCameraState *s, CameraState *c, int cnt) { + // empty +} + void cameras_run(MultiCameraState *s) { + std::thread t = start_process_thread(s, "processing", &s->rear, 51, camera_process_rear); set_thread_name("frame_streaming"); run_frame_stream(s); cameras_close(s); + t.join(); } diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h index a29e40304..5d6f71b87 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.h +++ b/selfdrive/camerad/cameras/camera_frame_stream.h @@ -1,5 +1,4 @@ -#ifndef CAMERA_FRAME_STREAM_H -#define CAMERA_FRAME_STREAM_H +#pragma once #include @@ -10,50 +9,35 @@ #include #endif -#include "common/mat.h" - -#include "buffering.h" -#include "common/visionbuf.h" #include "camera_common.h" #define FRAME_BUF_COUNT 16 -#ifdef __cplusplus -extern "C" { -#endif - typedef struct CameraState { int camera_id; CameraInfo ci; - int frame_size; - - VisionBuf *camera_bufs; - FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; - TBuffer camera_tb; int fps; float digital_gain; - float cur_gain_frac; mat3 transform; -} CameraState; + CameraBuf buf; +} CameraState; typedef struct MultiCameraState { int ispif_fd; CameraState rear; CameraState front; + + SubMaster *sm; + PubMaster *pm; } MultiCameraState; -void cameras_init(MultiCameraState *s); -void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); +void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx); +void cameras_open(MultiCameraState *s); void cameras_run(MultiCameraState *s); void cameras_close(MultiCameraState *s); void camera_autoexposure(CameraState *s, float grey_frac); -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 27709ef22..d90d900cf 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -4,8 +4,11 @@ #include #include #include +#include #include #include +#include +#include #include @@ -22,6 +25,7 @@ #include "common/timing.h" #include "common/swaglog.h" #include "common/params.h" +#include "clutil.h" #include "cereal/gen/cpp/log.capnp.h" @@ -30,21 +34,12 @@ #include "camera_qcom.h" -// enable this to run the camera at 60fps and sample every third frame -// supposed to reduce 33ms of lag, but no results -//#define HIGH_FPS - -#define CAMERA_MSG_AUTOEXPOSE 0 - -typedef struct CameraMsg { - int type; - int camera_num; - - float grey_frac; -} CameraMsg; - extern volatile sig_atomic_t do_exit; +// global var for AE/AF ops +std::atomic rear_exp{{0}}; +std::atomic front_exp{{0}}; + CameraInfo cameras_supported[CAMERA_ID_MAX] = { [CAMERA_ID_IMX298] = { .frame_width = 2328, @@ -106,14 +101,13 @@ static void camera_release_buffer(void* cookie, int buf_idx) { static void camera_init(CameraState *s, int camera_id, int camera_num, uint32_t pixel_clock, uint32_t line_length_pclk, - unsigned int max_gain, unsigned int fps) { + unsigned int max_gain, unsigned int fps, cl_device_id device_id, cl_context ctx) { s->camera_num = camera_num; s->camera_id = camera_id; assert(camera_id < ARRAYSIZE(cameras_supported)); s->ci = cameras_supported[camera_id]; assert(s->ci.frame_width != 0); - s->frame_size = s->ci.frame_height * s->ci.frame_stride; s->pixel_clock = pixel_clock; s->line_length_pclk = line_length_pclk; @@ -122,12 +116,7 @@ static void camera_init(CameraState *s, int camera_id, int camera_num, s->self_recover = 0; - s->ops_sock = zsock_new_push(">inproc://cameraops"); - assert(s->ops_sock); - s->ops_sock_handle = zsock_resolve(s->ops_sock); - - tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", - camera_release_buffer, s); + s->buf.init(device_id, ctx, s, FRAME_BUF_COUNT, "frame", camera_release_buffer); pthread_mutex_init(&s->frame_info_lock, NULL); } @@ -233,12 +222,6 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li //printf("front camera: %d %d %d\n", gain, integ_lines, frame_length); int err; - if (gain > 448) { - s->digital_gain = (512.0/(512-(gain))) / 8.0; - } else { - s->digital_gain = 1.0; - } - struct msm_camera_i2c_reg_array reg_array[] = { {0x104,0x1,0}, @@ -258,7 +241,18 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li return err; } -void cameras_init(MultiCameraState *s) { +cl_program build_conv_program(cl_device_id device_id, cl_context context, int image_w, int image_h, int filter_size) { + char args[4096]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d " + "-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); +} + +void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { char project_name[1024] = {0}; property_get("ro.boot.project_name", project_name, ""); @@ -296,29 +290,29 @@ void cameras_init(MultiCameraState *s) { camera_init(&s->rear, CAMERA_ID_IMX298, 0, /*pixel_clock=*/600000000, /*line_length_pclk=*/5536, - /*max_gain=*/510, //0 (ISO 100)- 448 (ISO 800, max analog gain) - 511 (super noisy) + /*max_gain=*/510, //0 (ISO 100)- 448 (ISO 800, max analog gain) - 511 (super noisy) #ifdef HIGH_FPS - /*fps*/60 + /*fps*/ 60, #else - /*fps*/20 + /*fps*/ 20, #endif - ); + device_id, ctx); s->rear.apply_exposure = imx298_apply_exposure; if (s->device == DEVICE_OP3T) { camera_init(&s->front, CAMERA_ID_S5K3P8SP, 1, - /*pixel_clock=*/561000000, /*line_length_pclk=*/5120, - /*max_gain=*/510, 10); + /*pixel_clock=*/560000000, /*line_length_pclk=*/5120, + /*max_gain=*/510, 10, device_id, ctx); s->front.apply_exposure = imx179_s5k3p8sp_apply_exposure; } else if (s->device == DEVICE_LP3) { camera_init(&s->front, CAMERA_ID_OV8865, 1, - /*pixel_clock=*/251200000, /*line_length_pclk=*/7000, - /*max_gain=*/510, 10); + /*pixel_clock=*/72000000, /*line_length_pclk=*/1602, + /*max_gain=*/510, 10, device_id, ctx); s->front.apply_exposure = ov8865_apply_exposure; } else { camera_init(&s->front, CAMERA_ID_IMX179, 1, /*pixel_clock=*/251200000, /*line_length_pclk=*/3440, - /*max_gain=*/224, 20); + /*max_gain=*/224, 20, device_id, ctx); s->front.apply_exposure = imx179_s5k3p8sp_apply_exposure; } @@ -338,6 +332,40 @@ void cameras_init(MultiCameraState *s) { s->rear.device = s->device; s->front.device = s->device; + + s->sm = new SubMaster({"driverState", "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); + // 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; + + 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); } static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { @@ -349,7 +377,7 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { unsigned int integ_lines = s->cur_integ_lines; if (exposure_frac >= 0) { - exposure_frac = clamp(exposure_frac, 2.0 / frame_length, 1.0); + exposure_frac = std::clamp(exposure_frac, 2.0f / frame_length, 1.0f); integ_lines = frame_length * exposure_frac; // See page 79 of the datasheet, this is the max allowed (-1 for phase adjust) @@ -358,7 +386,7 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { if (gain_frac >= 0) { // ISO200 is minimum gain - gain_frac = clamp(gain_frac, 1.0/64, 1.0); + gain_frac = std::clamp(gain_frac, 1.0f/64, 1.0f); // linearize gain response // TODO: will be wrong for front camera @@ -442,21 +470,10 @@ static void do_autoexposure(CameraState *s, float grey_frac) { } } -void camera_autoexposure(CameraState *s, float grey_frac) { - CameraMsg msg = { - .type = CAMERA_MSG_AUTOEXPOSE, - .camera_num = s->camera_num, - .grey_frac = grey_frac, - }; - - zmq_send(s->ops_sock_handle, &msg, sizeof(msg), ZMQ_DONTWAIT); -} - static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) { int err; - struct msm_eeprom_cfg_data cfg; - memset(&cfg, 0, sizeof(struct msm_eeprom_cfg_data)); + struct msm_eeprom_cfg_data cfg = {}; cfg.cfgtype = CFG_EEPROM_GET_CAL_DATA; err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg); assert(err >= 0); @@ -556,8 +573,7 @@ static void sensors_init(MultiCameraState *s) { } assert(sensorinit_fd >= 0); - struct sensor_init_cfg_data sensor_init_cfg; - memset(&sensor_init_cfg, 0, sizeof(struct sensor_init_cfg_data)); + struct sensor_init_cfg_data sensor_init_cfg = {}; // init rear sensor @@ -1073,31 +1089,17 @@ static void sensors_init(MultiCameraState *s) { static void camera_open(CameraState *s, bool rear) { int err; - struct sensorb_cfg_data sensorb_cfg_data; - memset(&sensorb_cfg_data, 0, sizeof(struct sensorb_cfg_data)); - struct csid_cfg_data csid_cfg_data; - memset(&csid_cfg_data, 0, sizeof(struct csid_cfg_data)); - struct csiphy_cfg_data csiphy_cfg_data; - memset(&csiphy_cfg_data, 0, sizeof(struct csiphy_cfg_data)); - struct msm_camera_csiphy_params csiphy_params; - memset(&csiphy_params, 0, sizeof(struct msm_camera_csiphy_params)); - struct msm_camera_csid_params csid_params; - memset(&csid_params, 0, sizeof(struct msm_camera_csid_params)); - struct msm_vfe_input_cfg input_cfg; - memset(&input_cfg, 0, sizeof(struct msm_vfe_input_cfg)); - struct msm_vfe_axi_stream_update_cmd update_cmd; - memset(&update_cmd, 0, sizeof(struct msm_vfe_axi_stream_update_cmd)); - struct v4l2_event_subscription sub; - memset(&sub, 0, sizeof(struct v4l2_event_subscription)); - struct ispif_cfg_data ispif_cfg_data; - memset(&ispif_cfg_data, 0, sizeof(struct ispif_cfg_data)); - struct msm_vfe_cfg_cmd_list cfg_cmd_list; - memset(&cfg_cmd_list, 0, sizeof(struct msm_vfe_cfg_cmd_list)); + struct sensorb_cfg_data sensorb_cfg_data = {}; + struct csid_cfg_data csid_cfg_data = {}; + struct csiphy_cfg_data csiphy_cfg_data = {}; + struct msm_camera_csiphy_params csiphy_params = {}; + struct msm_camera_csid_params csid_params = {}; + struct msm_vfe_input_cfg input_cfg = {}; + struct msm_vfe_axi_stream_update_cmd update_cmd = {}; + struct v4l2_event_subscription sub = {}; - struct msm_actuator_cfg_data actuator_cfg_data; - memset(&actuator_cfg_data, 0, sizeof(struct msm_actuator_cfg_data)); - struct msm_ois_cfg_data ois_cfg_data; - memset(&ois_cfg_data, 0, sizeof(struct msm_ois_cfg_data)); + struct msm_actuator_cfg_data actuator_cfg_data = {}; + struct msm_ois_cfg_data ois_cfg_data = {}; // open devices const char *sensor_dev; @@ -1717,7 +1719,7 @@ void actuator_move(CameraState *s, uint16_t target) { } int dest_step_pos = s->cur_step_pos + step; - dest_step_pos = clamp(dest_step_pos, 0, 255); + dest_step_pos = std::clamp(dest_step_pos, 0, 255); struct msm_actuator_cfg_data actuator_cfg_data = {0}; actuator_cfg_data.cfgtype = CFG_MOVE_FOCUS; @@ -1803,8 +1805,8 @@ static void do_autofocus(CameraState *s) { } // stay off the walls - lens_true_pos = clamp(lens_true_pos, dac_down, dac_up); - int target = clamp(lens_true_pos - sag, dac_down, dac_up); + 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; /*char debug[4096]; @@ -1817,6 +1819,19 @@ static void do_autofocus(CameraState *s) { actuator_move(s, target); } +void camera_autoexposure(CameraState *s, float grey_frac) { + if (s->camera_num == 0) { + CameraExpInfo tmp = rear_exp.load(); + tmp.op_id++; + tmp.grey_frac = grey_frac; + rear_exp.store(tmp); + } else { + CameraExpInfo tmp = front_exp.load(); + tmp.op_id++; + tmp.grey_frac = grey_frac; + front_exp.store(tmp); + } +} static void front_start(CameraState *s) { int err; @@ -1827,16 +1842,10 @@ static void front_start(CameraState *s) { LOG("sensor start regs: %d", err); } - - -void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) { +void cameras_open(MultiCameraState *s) { int err; - - struct ispif_cfg_data ispif_cfg_data; - memset(&ispif_cfg_data, 0, sizeof(struct ispif_cfg_data)); - - struct msm_ispif_param_data ispif_params; - memset(&ispif_params, 0, sizeof(struct msm_ispif_param_data)); + struct ispif_cfg_data ispif_cfg_data = {}; + struct msm_ispif_param_data ispif_params = {}; ispif_params.num = 4; // rear camera ispif_params.entries[0].vfe_intf = VFE0; @@ -1863,9 +1872,6 @@ void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *c ispif_params.entries[3].cids[0] = CID2; ispif_params.entries[3].csid = CSID0; - assert(camera_bufs_rear); - assert(camera_bufs_front); - s->msmcfg_fd = open("/dev/media0", O_RDWR | O_NONBLOCK); assert(s->msmcfg_fd >= 0); @@ -1889,13 +1895,13 @@ void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *c // LOG("ispif stop: %d", err); LOG("*** open front ***"); - s->front.ss[0].bufs = camera_bufs_front; + s->front.ss[0].bufs = s->front.buf.camera_bufs.get(); camera_open(&s->front, false); LOG("*** open rear ***"); - s->rear.ss[0].bufs = camera_bufs_rear; - s->rear.ss[1].bufs = camera_bufs_focus; - s->rear.ss[2].bufs = camera_bufs_stats; + s->rear.ss[0].bufs = s->rear.buf.camera_bufs.get(); + s->rear.ss[1].bufs = s->focus_bufs; + s->rear.ss[2].bufs = s->stats_bufs; camera_open(&s->rear, true); if (getenv("CAMERA_TEST")) { @@ -1936,7 +1942,7 @@ void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *c static void camera_close(CameraState *s) { int err; - tbuffer_stop(&s->camera_tb); + s->buf.stop(); // ISP: STOP_STREAM s->stream_cfg.cmd = STOP_STREAM; @@ -1958,8 +1964,6 @@ static void camera_close(CameraState *s) { } free(s->eeprom); - - zsock_destroy(&s->ops_sock); } @@ -2006,107 +2010,152 @@ static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) { }; } -static void ops_term() { - zsock_t *ops_sock = zsock_new_push(">inproc://cameraops"); - assert(ops_sock); - - CameraMsg msg = {.type = -1}; - zmq_send(zsock_resolve(ops_sock), &msg, sizeof(msg), ZMQ_DONTWAIT); - - zsock_destroy(&ops_sock); -} - static void* ops_thread(void* arg) { - int err; MultiCameraState *s = (MultiCameraState*)arg; + int rear_op_id_last = 0; + int front_op_id_last = 0; + + CameraExpInfo rear_op; + CameraExpInfo front_op; + set_thread_name("camera_settings"); - zsock_t *cameraops = zsock_new_pull("@inproc://cameraops"); - assert(cameraops); + 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); + rear_op_id_last = rear_op.op_id; + } - zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); - assert(terminate); + front_op = front_exp.load(); + if (front_op.op_id != front_op_id_last) { + do_autoexposure(&s->front, front_op.grey_frac); + front_op_id_last = front_op.op_id; + } - zpoller_t *poller = zpoller_new(cameraops, terminate, NULL); - assert(poller); + usleep(50000); + } - SubMaster sm({"sensorEvents"}); // msgq submaster + return NULL; +} - while (!do_exit) { - // zmq handling - zsock_t *which = (zsock_t*)zpoller_wait(poller, -1); - if (which == terminate) { - break; - } else if (which != NULL) { - void* sockraw = zsock_resolve(which); +void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) { + common_camera_process_front(s->sm, s->pm, c, cnt); +} - if (which == cameraops) { - zmq_msg_t msg; - err = zmq_msg_init(&msg); - assert(err == 0); +// 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 - err = zmq_msg_recv(&msg, sockraw, 0); - if (err >= 0) { - CameraMsg cmsg; - if (zmq_msg_size(&msg) == sizeof(cmsg)) { - memcpy(&cmsg, zmq_msg_data(&msg), zmq_msg_size(&msg)); - - //LOGD("cameraops %d", cmsg.type); - - if (cmsg.type == CAMERA_MSG_AUTOEXPOSE) { - if (cmsg.camera_num == 0) { - do_autoexposure(&s->rear, cmsg.grey_frac); - do_autofocus(&s->rear); - } else { - do_autoexposure(&s->front, cmsg.grey_frac); - } - } else if (cmsg.type == -1) { - break; - } - } - } else { - // skip if zmq is interrupted by msgq - int err_no = zmq_errno(); - assert(err_no == EINTR || err_no == EAGAIN); + // gz compensation + s->sm->update(0); + if (s->sm->updated("sensorEvents")) { + float vals[3] = {0.0}; + bool got_accel = false; + auto sensor_events = (*(s->sm))["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; } - - zmq_msg_close(&msg); + for (int j = 0; j < 3; j++) { + vals[j] = v[j]; + } + got_accel = true; + break; } } - // msgq handling - if (sm.update(0) > 0) { - float vals[3] = {0.0}; - bool got_accel = false; - - auto sensor_events = sm["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; //wtf - } - 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]; - } + 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]; } } - zpoller_destroy(&poller); - zsock_destroy(&cameraops); - zsock_destroy(&terminate); + // 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); - return NULL; + 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); + 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); + } + + if (cnt % 100 == 3) { + 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, 0, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip); + } } void cameras_run(MultiCameraState *s) { @@ -2116,6 +2165,9 @@ void cameras_run(MultiCameraState *s) { err = pthread_create(&ops_thread_handle, NULL, 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)); CameraState* cameras[2] = {&s->rear, &s->front}; @@ -2173,8 +2225,8 @@ void cameras_run(MultiCameraState *s) { //printf("divert: %d %d %d\n", i, buffer, buf_idx); if (buffer == 0) { - c->camera_bufs_metadata[buf_idx] = get_frame_metadata(c, isp_event_data->frame_id); - tbuffer_dispatch(&c->camera_tb, buf_idx); + c->buf.camera_bufs_metadata[buf_idx] = get_frame_metadata(c, isp_event_data->frame_id); + tbuffer_dispatch(&c->buf.camera_tb, buf_idx); } else { uint8_t *d = (uint8_t*)(c->ss[buffer].bufs[buf_idx].addr); if (buffer == 1) { @@ -2214,14 +2266,27 @@ void cameras_run(MultiCameraState *s) { LOG(" ************** STOPPING **************"); - ops_term(); err = pthread_join(ops_thread_handle, NULL); assert(err == 0); cameras_close(s); + + for (auto &t : threads) t.join(); } void cameras_close(MultiCameraState *s) { camera_close(&s->rear); camera_close(&s->front); + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + 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); + + clReleaseProgram(s->prg_rgb_laplacian); + clReleaseKernel(s->krnl_rgb_laplacian); + delete s->sm; + delete s->pm; } diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 02e139d7c..49afb0f66 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include "messaging.hpp" @@ -40,10 +40,6 @@ #define FOCUS_RECOVER_PATIENCE 50 // 2.5 seconds of complete blur #define FOCUS_RECOVER_STEPS 240 // 6 seconds -#ifdef __cplusplus -extern "C" { -#endif - typedef struct CameraState CameraState; typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, int frame_length); @@ -59,13 +55,9 @@ typedef struct CameraState { int camera_num; int camera_id; CameraInfo ci; - int frame_size; int device; - void* ops_sock_handle; - zsock_t * ops_sock; - uint32_t pixel_clock; uint32_t line_length_pclk; unsigned int max_gain; @@ -85,8 +77,6 @@ typedef struct CameraState { uint8_t *eeprom; // uint32_t camera_bufs_ids[FRAME_BUF_COUNT]; - FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; - TBuffer camera_tb; pthread_mutex_t frame_info_lock; FrameMetadata frame_metadata[METADATA_BUF_COUNT]; @@ -121,6 +111,8 @@ typedef struct CameraState { int fps; mat3 transform; + + CameraBuf buf; } CameraState; @@ -131,19 +123,35 @@ typedef struct MultiCameraState { unique_fd msmcfg_fd; unique_fd v4l_fd; + cl_mem rgb_conv_roi_cl, rgb_conv_result_cl, rgb_conv_filter_cl; + uint16_t lapres[(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)]; + + VisionBuf focus_bufs[FRAME_BUF_COUNT]; + VisionBuf stats_bufs[FRAME_BUF_COUNT]; + + 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; + PubMaster *pm; + } MultiCameraState; -void cameras_init(MultiCameraState *s); -void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); +void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx); +void cameras_open(MultiCameraState *s); void cameras_run(MultiCameraState *s); void cameras_close(MultiCameraState *s); void camera_autoexposure(CameraState *s, float grey_frac); void actuator_move(CameraState *s, uint16_t target); int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type); - -#ifdef __cplusplus -} // extern "C" -#endif diff --git a/selfdrive/camerad/cameras/sensor_i2c.h b/selfdrive/camerad/cameras/sensor_i2c.h index c33bc3468..74f56b027 100644 --- a/selfdrive/camerad/cameras/sensor_i2c.h +++ b/selfdrive/camerad/cameras/sensor_i2c.h @@ -1810,8 +1810,8 @@ static struct msm_camera_i2c_reg_array init_array_ov8865[] = { {0x3806,0x9,0}, {0x3807,0xa3,0}, // 2467 {0x3808,0x6,0}, {0x3809,0x60,0}, // 0x660 = 1632 (width) {0x380a,0x4,0}, {0x380b,0xc8,0}, // 0x4c8 = 1224 (height) -{0x380c,0x7,0}, {0x380d,0x83,0}, // 0x783 = 1923 (stride) -{0x380e,0x4,0}, {0x380f,0xe0,0}, // 0x4e0 = 1248 (vstride) +{0x380c,0x6,0}, {0x380d,0x42,0}, // line_length_pck +{0x380e,0x5,0}, {0x380f,0xda,0}, // frame_length_lines {0x3810,0x0,0}, {0x3811,0x4,0}, {0x3813,0x4,0}, {0x3814,0x3,0}, {0x3815,0x1,0}, // H-binning {0x3820,0x6,0}, // format1 diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index c28d6edf5..712a08809 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -1,7 +1,10 @@ +#include #include #include -#include -#include +#include +#include +#include +#include #if defined(QCOM) && !defined(QCOM_REPLAY) #include "cameras/camera_qcom.h" @@ -13,44 +16,23 @@ #include "cameras/camera_frame_stream.h" #endif -#include "common/util.h" -#include "common/params.h" -#include "common/swaglog.h" - -#include "common/ipc.h" -#include "common/visionipc.h" -#include "common/visionbuf.h" -#include "common/visionimg.h" - -#include "messaging.hpp" - -#include "transforms/rgb_to_yuv.h" -#include "imgproc/utils.h" +#include #include "clutil.h" -#include "bufs.h" +#include "common/ipc.h" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "common/visionipc.h" -#include -#include -#include - -#define UI_BUF_COUNT 4 -#define DEBAYER_LOCAL_WORKSIZE 16 -#define YUV_COUNT 40 #define MAX_CLIENTS 6 -extern "C" { volatile sig_atomic_t do_exit = 0; -} -void set_do_exit(int sig) { +static void set_do_exit(int sig) { do_exit = 1; } -/* -TODO: refactor out camera specific things from here -*/ - struct VisionState; struct VisionClientState { @@ -69,820 +51,22 @@ struct VisionClientStreamState { }; struct VisionState { - int frame_width, frame_height; - int frame_stride; - int frame_size; - - int ion_fd; - - // cl state - cl_device_id device_id; - cl_context context; - - cl_program prg_debayer_rear; - cl_program prg_debayer_front; - cl_program prg_debayer_wide; - cl_kernel krnl_debayer_rear; - cl_kernel krnl_debayer_front; - cl_kernel krnl_debayer_wide; - int debayer_cl_localMemSize; - size_t debayer_cl_globalWorkSize[2]; - size_t debayer_cl_localWorkSize[2]; - - cl_program prg_rgb_laplacian; - cl_kernel krnl_rgb_laplacian; - - int conv_cl_localMemSize; - size_t conv_cl_globalWorkSize[2]; - size_t conv_cl_localWorkSize[2]; - size_t pool_cl_globalWorkSize[2]; - - // processing - TBuffer ui_tb; - TBuffer ui_front_tb; - TBuffer ui_wide_tb; - - mat3 yuv_transform; - TBuffer *yuv_tb; - TBuffer *yuv_front_tb; - TBuffer *yuv_wide_tb; - - // TODO: refactor for both cameras? - Pool yuv_pool; - VisionBuf yuv_ion[YUV_COUNT]; - cl_mem yuv_cl[YUV_COUNT]; - YUVBuf yuv_bufs[YUV_COUNT]; - FrameMetadata yuv_metas[YUV_COUNT]; - size_t yuv_buf_size; - int yuv_width, yuv_height; - RGBToYUVState rgb_to_yuv_state; - - // for front camera recording - Pool yuv_front_pool; - VisionBuf yuv_front_ion[YUV_COUNT]; - cl_mem yuv_front_cl[YUV_COUNT]; - YUVBuf yuv_front_bufs[YUV_COUNT]; - FrameMetadata yuv_front_metas[YUV_COUNT]; - size_t yuv_front_buf_size; - int yuv_front_width, yuv_front_height; - RGBToYUVState front_rgb_to_yuv_state; - - Pool yuv_wide_pool; - VisionBuf yuv_wide_ion[YUV_COUNT]; - cl_mem yuv_wide_cl[YUV_COUNT]; - YUVBuf yuv_wide_bufs[YUV_COUNT]; - FrameMetadata yuv_wide_metas[YUV_COUNT]; - size_t yuv_wide_buf_size; - int yuv_wide_width, yuv_wide_height; - RGBToYUVState wide_rgb_to_yuv_state; - - size_t rgb_buf_size; - int rgb_width, rgb_height, rgb_stride; - VisionBuf rgb_bufs[UI_BUF_COUNT]; - cl_mem rgb_bufs_cl[UI_BUF_COUNT]; - cl_mem rgb_conv_roi_cl, rgb_conv_result_cl, rgb_conv_filter_cl; - uint16_t lapres[(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)]; - - size_t rgb_front_buf_size; - int rgb_front_width, rgb_front_height, rgb_front_stride; - VisionBuf rgb_front_bufs[UI_BUF_COUNT]; - cl_mem rgb_front_bufs_cl[UI_BUF_COUNT]; - - bool rhd_front; - int front_meteringbox_xmin, front_meteringbox_xmax; - int front_meteringbox_ymin, front_meteringbox_ymax; - - size_t rgb_wide_buf_size; - int rgb_wide_width, rgb_wide_height, rgb_wide_stride; - VisionBuf rgb_wide_bufs[UI_BUF_COUNT]; - cl_mem rgb_wide_bufs_cl[UI_BUF_COUNT]; - - cl_mem camera_bufs_cl[FRAME_BUF_COUNT]; - VisionBuf camera_bufs[FRAME_BUF_COUNT]; - VisionBuf focus_bufs[FRAME_BUF_COUNT]; - VisionBuf stats_bufs[FRAME_BUF_COUNT]; - - cl_mem front_camera_bufs_cl[FRAME_BUF_COUNT]; - VisionBuf front_camera_bufs[FRAME_BUF_COUNT]; - - cl_mem wide_camera_bufs_cl[FRAME_BUF_COUNT]; - VisionBuf wide_camera_bufs[FRAME_BUF_COUNT]; - - MultiCameraState cameras; - - zsock_t *terminate_pub; - - PubMaster *pm; - pthread_mutex_t clients_lock; VisionClientState clients[MAX_CLIENTS]; }; -// frontview thread -void* frontview_thread(void *arg) { - int err; - VisionState *s = (VisionState*)arg; - s->rhd_front = read_db_bool("IsRHD"); - - set_thread_name("frontview"); - err = set_realtime_priority(51); - // we subscribe to this for placement of the AE metering box - // TODO: the loop is bad, ideally models shouldn't affect sensors - SubMaster sm({"driverState"}); - - cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); - assert(err == 0); - - for (int cnt = 0; !do_exit; cnt++) { - int buf_idx = tbuffer_acquire(&s->cameras.front.camera_tb); - if (buf_idx < 0) { - break; - } - - int ui_idx = tbuffer_select(&s->ui_front_tb); - int rgb_idx = ui_idx; - FrameMetadata frame_data = s->cameras.front.camera_bufs_metadata[buf_idx]; - - //double t1 = millis_since_boot(); - - cl_event debayer_event; - if (s->cameras.front.ci.bayer) { - err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); - assert(err == 0); - err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[rgb_idx]); - assert(err == 0); -#ifdef QCOM2 - err = clSetKernelArg(s->krnl_debayer_front, 2, s->debayer_cl_localMemSize, 0); - assert(err == 0); - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 2, NULL, - s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event); -#else - float digital_gain = 1.0; - err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); - assert(err == 0); - const size_t debayer_work_size = s->rgb_front_height; // doesn't divide evenly, is this okay? - const size_t debayer_local_work_size = 128; - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, - &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); -#endif - assert(err == 0); - } else { - assert(s->rgb_front_buf_size >= s->cameras.front.frame_size); - assert(s->rgb_front_stride == s->cameras.front.ci.frame_stride); - err = clEnqueueCopyBuffer(q, s->front_camera_bufs_cl[buf_idx], s->rgb_front_bufs_cl[rgb_idx], - 0, 0, s->rgb_front_buf_size, 0, 0, &debayer_event); - assert(err == 0); - } - clWaitForEvents(1, &debayer_event); - clReleaseEvent(debayer_event); - - tbuffer_release(&s->cameras.front.camera_tb, buf_idx); - visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE); - - sm.update(0); - -#ifdef NOSCREEN - if (frame_data.frame_id % 4 == 2) { - sendrgb(&s->cameras, (uint8_t*) s->rgb_front_bufs[rgb_idx].addr, s->rgb_front_bufs[rgb_idx].len, 2); - } -#endif - - 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 = s->rhd_front ? 0:s->rgb_front_width - 0.5 * s->rgb_front_height; - s->front_meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72; - s->front_meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72; - s->front_meteringbox_ymin = (face_position[1] + 0.5) * (s->rgb_front_height) - 72; - s->front_meteringbox_ymax = (face_position[1] + 0.5) * (s->rgb_front_height) + 72; - } else {// use default setting if no face - s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3; - s->front_meteringbox_ymax = s->rgb_front_height * 1; - s->front_meteringbox_xmin = s->rhd_front ? 0:s->rgb_front_width * 3 / 5; - s->front_meteringbox_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width; - } - } - - // auto exposure - const uint8_t *bgr_front_ptr = (const uint8_t*)s->rgb_front_bufs[ui_idx].addr; -#ifndef DEBUG_DRIVER_MONITOR - if (cnt % 3 == 0) -#endif - { - // use driver face crop for AE - int x_start; - int x_end; - int y_start; - int y_end; - int skip = 1; - - if (s->front_meteringbox_xmax > 0) - { - x_start = s->front_meteringbox_xmin<0 ? 0:s->front_meteringbox_xmin; - x_end = s->front_meteringbox_xmax>=s->rgb_front_width ? s->rgb_front_width-1:s->front_meteringbox_xmax; - y_start = s->front_meteringbox_ymin<0 ? 0:s->front_meteringbox_ymin; - y_end = s->front_meteringbox_ymax>=s->rgb_front_height ? s->rgb_front_height-1:s->front_meteringbox_ymax; - } - else - { - y_start = s->rgb_front_height * 1 / 3; - y_end = s->rgb_front_height * 1; - x_start = s->rhd_front ? 0:s->rgb_front_width * 3 / 5; - x_end = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width; - } -#ifdef QCOM2 - x_start = 0.15*s->rgb_front_width; - x_end = 0.85*s->rgb_front_width; - y_start = 0.5*s->rgb_front_height; - y_end = 0.75*s->rgb_front_height; - skip = 2; -#endif - uint32_t lum_binning[256] = {0,}; - for (int y = y_start; y < y_end; y += skip) { - for (int x = x_start; x < x_end; x += 2) { // every 2nd col - const uint8_t *pix = &bgr_front_ptr[y * s->rgb_front_stride + x * 3]; - unsigned int lum = (unsigned int)pix[0] + pix[1] + pix[2]; -#ifdef DEBUG_DRIVER_MONITOR - uint8_t *pix_rw = (uint8_t *)pix; - - // set all the autoexposure pixels to pure green (pixel format is bgr) - pix_rw[0] = pix_rw[2] = 0; - pix_rw[1] = 0xff; -#endif - lum_binning[std::min(lum / 3, 255u)]++; - } - } - const unsigned int lum_total = (y_end - y_start) * (x_end - x_start) / 2 / skip; - unsigned int lum_cur = 0; - int lum_med = 0; - for (lum_med=0; lum_med<256; lum_med++) { - lum_cur += lum_binning[lum_med]; - if (lum_cur >= lum_total / 2) { - break; - } - } - camera_autoexposure(&s->cameras.front, lum_med / 256.0); - } - - // push YUV buffer - int yuv_idx = pool_select(&s->yuv_front_pool); - s->yuv_front_metas[yuv_idx] = frame_data; - - rgb_to_yuv_queue(&s->front_rgb_to_yuv_state, q, s->rgb_front_bufs_cl[ui_idx], s->yuv_front_cl[yuv_idx]); - visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); - s->yuv_front_metas[yuv_idx] = frame_data; - - // no reference required cause we don't use this in visiond - //pool_acquire(&s->yuv_front_pool, yuv_idx); - pool_push(&s->yuv_front_pool, yuv_idx); - //pool_release(&s->yuv_front_pool, yuv_idx); - - // send frame event - { - if (s->pm != NULL) { - MessageBuilder msg; - auto framed = msg.initEvent().initFrontFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - framed.setGainFrac(frame_data.gain_frac); - framed.setFrameType(cereal::FrameData::FrameType::FRONT); - - s->pm->send("frontFrame", msg); - } - } - - /*FILE *f = fopen("/tmp/test2", "wb"); - printf("%d %d\n", s->rgb_front_height, s->rgb_front_stride); - fwrite(bgr_front_ptr, 1, s->rgb_front_stride * s->rgb_front_height, f); - fclose(f);*/ - - tbuffer_dispatch(&s->ui_front_tb, ui_idx); - - //double t2 = millis_since_boot(); - //LOGD("front process: %.2fms", t2-t1); +static CameraBuf *get_camerabuf_by_type(VisionState *s, VisionStreamType type) { + assert(type >= 0 && type < VISION_STREAM_MAX); + if (type == VISION_STREAM_RGB_BACK || type == VISION_STREAM_YUV) { + return &s->cameras.rear.buf; + } else if (type == VISION_STREAM_RGB_FRONT || type == VISION_STREAM_YUV_FRONT) { + return &s->cameras.front.buf; } - clReleaseCommandQueue(q); - - return NULL; -} - #ifdef QCOM2 -// wide -void* wideview_thread(void *arg) { - int err; - VisionState *s = (VisionState*)arg; - - set_thread_name("wideview"); - - err = set_realtime_priority(51); - LOG("setpriority returns %d", err); - - // init cl stuff - const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; - cl_command_queue q = clCreateCommandQueueWithProperties(s->context, s->device_id, props, &err); - assert(err == 0); - - // init the net - LOG("wideview start!"); - - for (int cnt = 0; !do_exit; cnt++) { - int buf_idx = tbuffer_acquire(&s->cameras.wide.camera_tb); - // int buf_idx = camera_acquire_buffer(s); - if (buf_idx < 0) { - break; - } - - double t1 = millis_since_boot(); - - FrameMetadata frame_data = s->cameras.wide.camera_bufs_metadata[buf_idx]; - uint32_t frame_id = frame_data.frame_id; - - if (frame_id == -1) { - LOGE("no frame data? wtf"); - tbuffer_release(&s->cameras.wide.camera_tb, buf_idx); - continue; - } - - int ui_idx = tbuffer_select(&s->ui_wide_tb); - int rgb_idx = ui_idx; - - cl_event debayer_event; - if (s->cameras.wide.ci.bayer) { - err = clSetKernelArg(s->krnl_debayer_wide, 0, sizeof(cl_mem), &s->wide_camera_bufs_cl[buf_idx]); - assert(err == 0); - err = clSetKernelArg(s->krnl_debayer_wide, 1, sizeof(cl_mem), &s->rgb_wide_bufs_cl[rgb_idx]); - assert(err == 0); - err = clSetKernelArg(s->krnl_debayer_wide, 2, s->debayer_cl_localMemSize, 0); - assert(err == 0); - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_wide, 2, NULL, - s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event); - assert(err == 0); - } else { - assert(s->rgb_wide_buf_size >= s->frame_size); - assert(s->rgb_stride == s->frame_stride); - err = clEnqueueCopyBuffer(q, s->wide_camera_bufs_cl[buf_idx], s->rgb_wide_bufs_cl[rgb_idx], - 0, 0, s->rgb_wide_buf_size, 0, 0, &debayer_event); - assert(err == 0); - } - - clWaitForEvents(1, &debayer_event); - clReleaseEvent(debayer_event); - - tbuffer_release(&s->cameras.wide.camera_tb, buf_idx); - - visionbuf_sync(&s->rgb_wide_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE); - -#ifdef NOSCREEN - if (frame_data.frame_id % 4 == 0) { - sendrgb(&s->cameras, (uint8_t*) s->rgb_wide_bufs[rgb_idx].addr, s->rgb_wide_bufs[rgb_idx].len, 1); - } + return &s->cameras.wide.buf; #endif - - double t2 = millis_since_boot(); - - double yt1 = millis_since_boot(); - - int yuv_idx = pool_select(&s->yuv_wide_pool); - - s->yuv_wide_metas[yuv_idx] = frame_data; - - uint8_t* yuv_ptr_y = s->yuv_wide_bufs[yuv_idx].y; - cl_mem yuv_wide_cl = s->yuv_wide_cl[yuv_idx]; - rgb_to_yuv_queue(&s->wide_rgb_to_yuv_state, q, s->rgb_wide_bufs_cl[rgb_idx], yuv_wide_cl); - visionbuf_sync(&s->yuv_wide_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); - - double yt2 = millis_since_boot(); - - // keep another reference around till were done processing - pool_acquire(&s->yuv_wide_pool, yuv_idx); - pool_push(&s->yuv_wide_pool, yuv_idx); - - // send frame event - { - if (s->pm != NULL) { - MessageBuilder msg; - auto framed = msg.initEvent().initWideFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - framed.setGainFrac(frame_data.gain_frac); - - s->pm->send("wideFrame", msg); - } - } - - tbuffer_dispatch(&s->ui_wide_tb, ui_idx); - - // auto exposure over big box - // TODO: fix this? should not use med imo - const int exposure_x = 384; - const int exposure_y = 300; - const int exposure_height = 400; - const int exposure_width = 1152; - if (cnt % 3 == 0) { - // find median box luminance for AE - uint32_t lum_binning[256] = {0,}; - for (int y=0; yyuv_wide_width) + exposure_x + x]; - lum_binning[lum]++; - } - } - const unsigned int lum_total = exposure_height * exposure_width / 4; - unsigned int lum_cur = 0; - int lum_med = 0; - for (lum_med=0; lum_med<256; lum_med++) { - // shouldn't be any values less than 16 - yuv footroom - lum_cur += lum_binning[lum_med]; - if (lum_cur >= lum_total / 2) { - break; - } - } - - camera_autoexposure(&s->cameras.wide, lum_med / 256.0); - } - - pool_release(&s->yuv_wide_pool, yuv_idx); - double t5 = millis_since_boot(); - LOGD("queued: %.2fms, yuv: %.2f, | processing: %.3fms", (t2-t1), (yt2-yt1), (t5-t1)); - } - - return NULL; -} -#endif - -// processing -void* processing_thread(void *arg) { - int err; - VisionState *s = (VisionState*)arg; - - set_thread_name("processing"); - - err = set_realtime_priority(51); - LOG("setpriority returns %d", err); - -#if defined(QCOM) && !defined(QCOM_REPLAY) - std::unique_ptr rgb_roi_buf = std::make_unique((s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y)*3); - std::unique_ptr conv_result = std::make_unique((s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y)); -#endif - - // init cl stuff -#ifdef __APPLE__ - cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); -#else - const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; - cl_command_queue q = clCreateCommandQueueWithProperties(s->context, s->device_id, props, &err); -#endif - assert(err == 0); - - // init the net - LOG("processing start!"); - - for (int cnt = 0; !do_exit; cnt++) { - int buf_idx = tbuffer_acquire(&s->cameras.rear.camera_tb); - // int buf_idx = camera_acquire_buffer(s); - if (buf_idx < 0) { - break; - } - - double t1 = millis_since_boot(); - - FrameMetadata frame_data = s->cameras.rear.camera_bufs_metadata[buf_idx]; - uint32_t frame_id = frame_data.frame_id; - - if (frame_id == -1) { - LOGE("no frame data? wtf"); - tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); - continue; - } - - int ui_idx = tbuffer_select(&s->ui_tb); - int rgb_idx = ui_idx; - - cl_event debayer_event; - if (s->cameras.rear.ci.bayer) { - err = clSetKernelArg(s->krnl_debayer_rear, 0, sizeof(cl_mem), &s->camera_bufs_cl[buf_idx]); - assert(err == 0); - err = clSetKernelArg(s->krnl_debayer_rear, 1, sizeof(cl_mem), &s->rgb_bufs_cl[rgb_idx]); - assert(err == 0); -#ifdef QCOM2 - err = clSetKernelArg(s->krnl_debayer_rear, 2, s->debayer_cl_localMemSize, 0); - assert(err == 0); - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 2, NULL, - s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event); -#else - err = clSetKernelArg(s->krnl_debayer_rear, 2, sizeof(float), &s->cameras.rear.digital_gain); - assert(err == 0); - const size_t debayer_work_size = s->rgb_height; // doesn't divide evenly, is this okay? - const size_t debayer_local_work_size = 128; - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 1, NULL, - &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); -#endif - assert(err == 0); - } else { - assert(s->rgb_buf_size >= s->frame_size); - assert(s->rgb_stride == s->frame_stride); - err = clEnqueueCopyBuffer(q, s->camera_bufs_cl[buf_idx], s->rgb_bufs_cl[rgb_idx], - 0, 0, s->rgb_buf_size, 0, 0, &debayer_event); - assert(err == 0); - } - clWaitForEvents(1, &debayer_event); - clReleaseEvent(debayer_event); - - tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); - - visionbuf_sync(&s->rgb_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE); - -#ifdef NOSCREEN - if (frame_data.frame_id % 4 == 1) { - sendrgb(&s->cameras, (uint8_t*) s->rgb_bufs[rgb_idx].addr, s->rgb_bufs[rgb_idx].len, 0); - } -#endif - -#if defined(QCOM) && !defined(QCOM_REPLAY) - /*FILE *dump_rgb_file = fopen("/tmp/process_dump.rgb", "wb"); - fwrite(s->rgb_bufs[rgb_idx].addr, s->rgb_bufs[rgb_idx].len, sizeof(uint8_t), dump_rgb_file); - fclose(dump_rgb_file); - printf("ORIGINAL SAVED!!\n");*/ - - /*double t10 = millis_since_boot();*/ - - // cache rgb roi and write to cl - int roi_id = cnt % ((ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)); // 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<(s->rgb_height/NUM_SEGMENTS_Y);r++) { - memcpy(rgb_roi_buf.get() + r * (s->rgb_width/NUM_SEGMENTS_X) * 3, - (uint8_t *) s->rgb_bufs[rgb_idx].addr + \ - (ROI_Y_MIN + roi_y_offset) * s->rgb_height/NUM_SEGMENTS_Y * FULL_STRIDE_X * 3 + \ - (ROI_X_MIN + roi_x_offset) * s->rgb_width/NUM_SEGMENTS_X * 3 + r * FULL_STRIDE_X * 3, - s->rgb_width/NUM_SEGMENTS_X * 3); - } - - err = clEnqueueWriteBuffer (q, s->rgb_conv_roi_cl, true, 0, - s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), rgb_roi_buf.get(), 0, 0, 0); - assert(err == 0); - - /*double t11 = millis_since_boot(); - printf("cache time: %f ms\n", t11 - t10); - t10 = millis_since_boot();*/ - - err = clSetKernelArg(s->krnl_rgb_laplacian, 0, sizeof(cl_mem), (void *) &s->rgb_conv_roi_cl); - assert(err == 0); - err = clSetKernelArg(s->krnl_rgb_laplacian, 1, sizeof(cl_mem), (void *) &s->rgb_conv_result_cl); - assert(err == 0); - err = clSetKernelArg(s->krnl_rgb_laplacian, 2, sizeof(cl_mem), (void *) &s->rgb_conv_filter_cl); - assert(err == 0); - err = clSetKernelArg(s->krnl_rgb_laplacian, 3, s->conv_cl_localMemSize, 0); - assert(err == 0); - - cl_event conv_event; - err = clEnqueueNDRangeKernel(q, s->krnl_rgb_laplacian, 2, NULL, - s->conv_cl_globalWorkSize, s->conv_cl_localWorkSize, 0, 0, &conv_event); - assert(err == 0); - clWaitForEvents(1, &conv_event); - clReleaseEvent(conv_event); - - err = clEnqueueReadBuffer(q, s->rgb_conv_result_cl, true, 0, - s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * sizeof(int16_t), conv_result.get(), 0, 0, 0); - assert(err == 0); - - /*t11 = millis_since_boot(); - printf("conv time: %f ms\n", t11 - t10); - t10 = millis_since_boot();*/ - - get_lapmap_one(conv_result.get(), &s->lapres[roi_id], s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y); - - /*t11 = millis_since_boot(); - printf("pool time: %f ms\n", t11 - t10); - t10 = millis_since_boot();*/ - - /*t11 = millis_since_boot(); - printf("process time: %f ms\n ----- \n", t11 - t10); - t10 = millis_since_boot();*/ - - // setup self recover - const float lens_true_pos = s->cameras.rear.lens_true_pos; - if (is_blur(&s->lapres[0]) && - (lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN)+1 || - lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP)-1) && - s->cameras.rear.self_recover < 2) { - // truly stuck, needs help - s->cameras.rear.self_recover -= 1; - if (s->cameras.rear.self_recover < -FOCUS_RECOVER_PATIENCE) { - LOGD("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", - lens_true_pos, s->cameras.rear.self_recover.load()); - s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((lens_true_pos < (s->cameras.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->cameras.device == DEVICE_LP3? LP3_AF_DAC_M - LP3_AF_DAC_3SIG:OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) || - lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M + LP3_AF_DAC_3SIG:OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) && - s->cameras.rear.self_recover < 2) { - // in suboptimal position with high prob, but may still recover by itself - s->cameras.rear.self_recover -= 1; - if (s->cameras.rear.self_recover < -(FOCUS_RECOVER_PATIENCE*3)) { - LOGD("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, s->cameras.rear.self_recover.load()); - s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); - } - } else if (s->cameras.rear.self_recover < 0) { - s->cameras.rear.self_recover += 1; // reset if fine - } - -#endif - - double t2 = millis_since_boot(); - -#ifndef QCOM2 - uint8_t *bgr_ptr = (uint8_t*)s->rgb_bufs[rgb_idx].addr; -#endif - - double yt1 = millis_since_boot(); - - int yuv_idx = pool_select(&s->yuv_pool); - - s->yuv_metas[yuv_idx] = frame_data; - - uint8_t* yuv_ptr_y = s->yuv_bufs[yuv_idx].y; - cl_mem yuv_cl = s->yuv_cl[yuv_idx]; - rgb_to_yuv_queue(&s->rgb_to_yuv_state, q, s->rgb_bufs_cl[rgb_idx], yuv_cl); - visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); - - double yt2 = millis_since_boot(); - - // keep another reference around till were done processing - pool_acquire(&s->yuv_pool, yuv_idx); - pool_push(&s->yuv_pool, yuv_idx); - - // send frame event - { - if (s->pm != NULL) { - MessageBuilder msg; - auto framed = msg.initEvent().initFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - framed.setGainFrac(frame_data.gain_frac); - -#if defined(QCOM) && !defined(QCOM_REPLAY) - kj::ArrayPtr focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS); - kj::ArrayPtr focus_confs(&s->cameras.rear.confidence[0], NUM_FOCUS); - framed.setFocusVal(focus_vals); - framed.setFocusConf(focus_confs); - kj::ArrayPtr sharpness_score(&s->lapres[0], (ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)); - framed.setSharpnessScore(sharpness_score); - framed.setRecoverState(s->cameras.rear.self_recover); -#endif - -// TODO: add this back -#if !defined(QCOM) && !defined(QCOM2) -//#ifndef QCOM - framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); -#endif - - kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); - framed.setTransform(transform_vs); - - s->pm->send("frame", msg); - } - } - -#ifndef QCOM2 - // TODO: fix on QCOM2, giving scanline error - // one thumbnail per 5 seconds (instead of %5 == 0 posenet) - if (cnt % 100 == 3) { - uint8_t* thumbnail_buffer = NULL; - unsigned long thumbnail_len = 0; - - unsigned char *row = (unsigned char *)malloc(s->rgb_width/4*3); - - struct jpeg_compress_struct cinfo; - struct jpeg_error_mgr jerr; - - cinfo.err = jpeg_std_error(&jerr); - jpeg_create_compress(&cinfo); - jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len); - - cinfo.image_width = s->rgb_width / 4; - cinfo.image_height = s->rgb_height / 4; - cinfo.input_components = 3; - cinfo.in_color_space = JCS_RGB; - - jpeg_set_defaults(&cinfo); - jpeg_set_quality(&cinfo, 50, true); - jpeg_start_compress(&cinfo, true); - - JSAMPROW row_pointer[1]; - for (int i = 0; i < s->rgb_height - 4; i+=4) { - for (int j = 0; j < s->rgb_width*3; j+=12) { - for (int k = 0; k < 3; k++) { - uint16_t dat = 0; - dat += bgr_ptr[s->rgb_stride*i + j + k]; - dat += bgr_ptr[s->rgb_stride*i + j+3 + k]; - dat += bgr_ptr[s->rgb_stride*(i+1) + j + k]; - dat += bgr_ptr[s->rgb_stride*(i+1) + j+3 + k]; - dat += bgr_ptr[s->rgb_stride*(i+2) + j + k]; - dat += bgr_ptr[s->rgb_stride*(i+2) + j+3 + k]; - dat += bgr_ptr[s->rgb_stride*(i+3) + j + k]; - dat += bgr_ptr[s->rgb_stride*(i+3) + j+3 + k]; - - row[(j/4) + (2-k)] = dat/8; - } - } - row_pointer[0] = row; - jpeg_write_scanlines(&cinfo, row_pointer, 1); - } - free(row); - jpeg_finish_compress(&cinfo); - - MessageBuilder msg; - auto thumbnaild = msg.initEvent().initThumbnail(); - thumbnaild.setFrameId(frame_data.frame_id); - thumbnaild.setTimestampEof(frame_data.timestamp_eof); - thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len)); - - if (s->pm != NULL) { - s->pm->send("thumbnail", msg); - } - - free(thumbnail_buffer); - } -#endif - - tbuffer_dispatch(&s->ui_tb, ui_idx); - - // auto exposure over big box -#ifdef QCOM2 - const int exposure_x = 384; - const int exposure_y = 300; - const int exposure_height = 400; - const int exposure_width = 1152; - const int skip = 2; -#else - const int exposure_x = 290; - const int exposure_y = 322; - const int exposure_height = 314; - const int exposure_width = 560; - const int skip = 1; -#endif - if (cnt % 3 == 0) { - // find median box luminance for AE - uint32_t lum_binning[256] = {0,}; - for (int y=0; yyuv_width) + exposure_x + x]; - lum_binning[lum]++; - } - } - const unsigned int lum_total = exposure_height * exposure_width / skip / skip; - unsigned int lum_cur = 0; - int lum_med = 0; - for (lum_med=0; lum_med<256; lum_med++) { - // shouldn't be any values less than 16 - yuv footroom - lum_cur += lum_binning[lum_med]; - if (lum_cur >= lum_total / 2) { - break; - } - } - - camera_autoexposure(&s->cameras.rear, lum_med / 256.0); - } - - pool_release(&s->yuv_pool, yuv_idx); - double t5 = millis_since_boot(); - LOGD("queued: %.2fms, yuv: %.2f, | processing: %.3fms", (t2-t1), (yt2-yt1), (t5-t1)); - } - - clReleaseCommandQueue(q); - return NULL; + assert(0); } // visionserver @@ -894,26 +78,20 @@ void* visionserver_client_thread(void* arg) { set_thread_name("clientthread"); - zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); - assert(terminate); - void* terminate_raw = zsock_resolve(terminate); - VisionClientStreamState streams[VISION_STREAM_MAX] = {{0}}; LOGW("client start fd %d", fd); while (true) { - zmq_pollitem_t polls[2+VISION_STREAM_MAX] = {{0}}; - polls[0].socket = terminate_raw; - polls[0].events = ZMQ_POLLIN; - polls[1].fd = fd; - polls[1].events = ZMQ_POLLIN; + struct pollfd polls[1+VISION_STREAM_MAX] = {{0}}; + polls[0].fd = fd; + polls[0].events = POLLIN; - int poll_to_stream[2+VISION_STREAM_MAX] = {0}; - int num_polls = 2; + int poll_to_stream[1+VISION_STREAM_MAX] = {0}; + int num_polls = 1; for (int i=0; i= 2) { continue; } @@ -925,18 +103,16 @@ void* visionserver_client_thread(void* arg) { poll_to_stream[num_polls] = i; num_polls++; } - int ret = zmq_poll(polls, num_polls, -1); + int ret = poll(polls, num_polls, -1); if (ret < 0) { if (errno == EINTR || errno == EAGAIN) continue; LOGE("poll failed (%d - %d)", ret, errno); break; } + if (do_exit) break; if (polls[0].revents) { - break; - } else if (polls[1].revents) { VisionPacket p; err = vipc_recv(fd, &p); - // printf("recv %d\n", p.type); if (err <= 0) { break; } else if (p.type == VIPC_STREAM_SUBSCRIBE) { @@ -950,98 +126,41 @@ void* visionserver_client_thread(void* arg) { stream->tb = p.d.stream_sub.tbuffer; VisionStreamBufs *stream_bufs = &rep.d.stream_bufs; - if (stream_type == VISION_STREAM_RGB_BACK) { - stream_bufs->width = s->rgb_width; - stream_bufs->height = s->rgb_height; - stream_bufs->stride = s->rgb_stride; - stream_bufs->buf_len = s->rgb_bufs[0].len; + CameraBuf *b = get_camerabuf_by_type(s, stream_type); + if (stream_type == VISION_STREAM_RGB_BACK || + stream_type == VISION_STREAM_RGB_FRONT || + stream_type == VISION_STREAM_RGB_WIDE) { + stream_bufs->width = b->rgb_width; + stream_bufs->height = b->rgb_height; + stream_bufs->stride = b->rgb_stride; + stream_bufs->buf_len = b->rgb_bufs[0].len; rep.num_fds = UI_BUF_COUNT; - for (int i=0; irgb_bufs[i].fd; + for (int i = 0; i < rep.num_fds; i++) { + rep.fds[i] = b->rgb_bufs[i].fd; } if (stream->tb) { - stream->tbuffer = &s->ui_tb; + stream->tbuffer = &b->ui_tb; } else { assert(false); } - } else if (stream_type == VISION_STREAM_RGB_FRONT) { - stream_bufs->width = s->rgb_front_width; - stream_bufs->height = s->rgb_front_height; - stream_bufs->stride = s->rgb_front_stride; - stream_bufs->buf_len = s->rgb_front_bufs[0].len; - rep.num_fds = UI_BUF_COUNT; - for (int i=0; irgb_front_bufs[i].fd; - } - if (stream->tb) { - stream->tbuffer = &s->ui_front_tb; - } else { - assert(false); - } - } else if (stream_type == VISION_STREAM_RGB_WIDE) { - stream_bufs->width = s->rgb_wide_width; - stream_bufs->height = s->rgb_wide_height; - stream_bufs->stride = s->rgb_wide_stride; - stream_bufs->buf_len = s->rgb_wide_bufs[0].len; - rep.num_fds = UI_BUF_COUNT; - for (int i=0; irgb_wide_bufs[i].fd; - } - if (stream->tb) { - stream->tbuffer = &s->ui_wide_tb; - } else { - assert(false); - } - } else if (stream_type == VISION_STREAM_YUV) { - stream_bufs->width = s->yuv_width; - stream_bufs->height = s->yuv_height; - stream_bufs->stride = s->yuv_width; - stream_bufs->buf_len = s->yuv_buf_size; - rep.num_fds = YUV_COUNT; - for (int i=0; iyuv_ion[i].fd; - } - if (stream->tb) { - stream->tbuffer = s->yuv_tb; - } else { - stream->queue = pool_get_queue(&s->yuv_pool); - } - } else if (stream_type == VISION_STREAM_YUV_FRONT) { - stream_bufs->width = s->yuv_front_width; - stream_bufs->height = s->yuv_front_height; - stream_bufs->stride = s->yuv_front_width; - stream_bufs->buf_len = s->yuv_front_buf_size; - rep.num_fds = YUV_COUNT; - for (int i=0; iyuv_front_ion[i].fd; - } - if (stream->tb) { - stream->tbuffer = s->yuv_front_tb; - } else { - stream->queue = pool_get_queue(&s->yuv_front_pool); - } - } else if (stream_type == VISION_STREAM_YUV_WIDE) { - stream_bufs->width = s->yuv_wide_width; - stream_bufs->height = s->yuv_wide_height; - stream_bufs->stride = s->yuv_wide_width; - stream_bufs->buf_len = s->yuv_wide_buf_size; - rep.num_fds = YUV_COUNT; - for (int i=0; iyuv_wide_ion[i].fd; - } - if (stream->tb) { - stream->tbuffer = s->yuv_wide_tb; - } else { - stream->queue = pool_get_queue(&s->yuv_wide_pool); - } - } else { - assert(false); + stream_bufs->width = b->yuv_width; + stream_bufs->height = b->yuv_height; + stream_bufs->stride = b->yuv_width; + stream_bufs->buf_len = b->yuv_buf_size; + rep.num_fds = YUV_COUNT; + for (int i = 0; i < rep.num_fds; i++) { + rep.fds[i] = b->yuv_ion[i].fd; + } + if (stream->tb) { + stream->tbuffer = b->yuv_tb; + } else { + stream->queue = pool_get_queue(&b->yuv_pool); + } } vipc_send(fd, &rep); streams[stream_type].subscribed = true; } else if (p.type == VIPC_STREAM_RELEASE) { - // printf("client release f %d %d\n", p.d.stream_rel.type, p.d.stream_rel.idx); int si = p.d.stream_rel.type; assert(si < VISION_STREAM_MAX); if (streams[si].tb) { @@ -1055,7 +174,7 @@ void* visionserver_client_thread(void* arg) { } } else { int stream_i = VISION_STREAM_MAX; - for (int i=2; iyuv_metas[idx].frame_id; - rep.d.stream_acq.extra.timestamp_eof = s->yuv_metas[idx].timestamp_eof; - } else if (stream_i == VISION_STREAM_YUV_FRONT) { - rep.d.stream_acq.extra.frame_id = s->yuv_front_metas[idx].frame_id; - rep.d.stream_acq.extra.timestamp_eof = s->yuv_front_metas[idx].timestamp_eof; - } else if (stream_i == VISION_STREAM_YUV_WIDE) { - rep.d.stream_acq.extra.frame_id = s->yuv_wide_metas[idx].frame_id; - rep.d.stream_acq.extra.timestamp_eof = s->yuv_wide_metas[idx].timestamp_eof; + if (stream_i == VISION_STREAM_YUV || + stream_i == VISION_STREAM_YUV_FRONT || + stream_i == VISION_STREAM_YUV_WIDE) { + CameraBuf *b = get_camerabuf_by_type(s, (VisionStreamType)stream_i); + rep.d.stream_acq.extra.frame_id = b->yuv_metas[idx].frame_id; + rep.d.stream_acq.extra.timestamp_eof = b->yuv_metas[idx].timestamp_eof; } vipc_send(fd, &rep); } @@ -1108,7 +224,6 @@ void* visionserver_client_thread(void* arg) { } close(fd); - zsock_destroy(&terminate); pthread_mutex_lock(&s->clients_lock); client->running = false; @@ -1123,27 +238,20 @@ void* visionserver_thread(void* arg) { set_thread_name("visionserver"); - zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); - assert(terminate); - void* terminate_raw = zsock_resolve(terminate); - int sock = ipc_bind(VIPC_SOCKET_PATH); while (!do_exit) { - zmq_pollitem_t polls[2] = {{0}}; - polls[0].socket = terminate_raw; - polls[0].events = ZMQ_POLLIN; - polls[1].fd = sock; - polls[1].events = ZMQ_POLLIN; + struct pollfd polls[1] = {{0}}; + polls[0].fd = sock; + polls[0].events = POLLIN; - int ret = zmq_poll(polls, ARRAYSIZE(polls), -1); + int ret = poll(polls, ARRAYSIZE(polls), 1000); if (ret < 0) { if (errno == EINTR || errno == EAGAIN) continue; LOGE("poll failed (%d - %d)", ret, errno); break; } - if (polls[0].revents) { - break; - } else if (!polls[1].revents) { + if (do_exit) break; + if (!polls[0].revents) { continue; } @@ -1188,448 +296,46 @@ void* visionserver_thread(void* arg) { } close(sock); - zsock_destroy(&terminate); return NULL; } - -////////// cl stuff - -cl_program build_debayer_program(VisionState *s, - int frame_width, int frame_height, int frame_stride, - int rgb_width, int rgb_height, int rgb_stride, - int bayer_flip, int hdr) { -#ifdef QCOM2 - assert(rgb_width == frame_width); - assert(rgb_height == frame_height); -#else - assert(rgb_width == frame_width/2); - assert(rgb_height == frame_height/2); -#endif - - char args[4096]; - snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " - "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " - "-DBAYER_FLIP=%d -DHDR=%d", - frame_width, frame_height, frame_stride, - rgb_width, rgb_height, rgb_stride, - bayer_flip, hdr); -#ifdef QCOM2 - return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/real_debayer.cl", args); -#else - return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args); -#endif -} - -cl_program build_conv_program(VisionState *s, - int image_w, int image_h, - int filter_size) { - char args[4096]; - snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d " - "-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(s->context, s->device_id, "imgproc/conv.cl", args); -} - -cl_program build_pool_program(VisionState *s, - int full_stride_x, - int x_pitch, int y_pitch, - int roi_x_min, int roi_x_max, - int roi_y_min, int roi_y_max) { - char args[4096]; - snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DFULL_STRIDE_X=%d -DX_PITCH=%d -DY_PITCH=%d " - "-DROI_X_MIN=%d -DROI_X_MAX=%d -DROI_Y_MIN=%d -DROI_Y_MAX=%d", - full_stride_x, x_pitch, y_pitch, - roi_x_min, roi_x_max, roi_y_min, roi_y_max); - return CLU_LOAD_FROM_FILE(s->context, s->device_id, "imgproc/pool.cl", args); -} - -void cl_init(VisionState *s) { - int err; - s->device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); - s->context = clCreateContext(NULL, 1, &s->device_id, NULL, NULL, &err); - assert(err == 0); -} - -void cl_free(VisionState *s) { - int err; - - err = clReleaseContext(s->context); - assert(err == 0); -} - -void init_buffers(VisionState *s) { - int err; - - // allocate camera buffers - - for (int i=0; icamera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context, - &s->camera_bufs_cl[i]); - #ifdef QCOM - // TODO: make lengths correct - s->focus_bufs[i] = visionbuf_allocate(0xb80); - s->stats_bufs[i] = visionbuf_allocate(0xb80); - #endif - } - - for (int i=0; ifront_camera_bufs[i] = visionbuf_allocate_cl(s->cameras.front.frame_size, - s->device_id, s->context, - &s->front_camera_bufs_cl[i]); - } -#ifdef QCOM2 - for (int i=0; iwide_camera_bufs[i] = visionbuf_allocate_cl(s->cameras.wide.frame_size, - s->device_id, s->context, - &s->wide_camera_bufs_cl[i]); - } -#endif - // processing buffers - if (s->cameras.rear.ci.bayer) { -#ifdef QCOM2 - s->rgb_width = s->frame_width; - s->rgb_height = s->frame_height; -#else - s->rgb_width = s->frame_width / 2; - s->rgb_height = s->frame_height / 2; -#endif - } else { - s->rgb_width = s->frame_width; - s->rgb_height = s->frame_height; - } - - for (int i=0; irgb_width, s->rgb_height, &s->rgb_bufs[i]); - s->rgb_bufs_cl[i] = visionbuf_to_cl(&s->rgb_bufs[i], s->device_id, s->context); - if (i == 0){ - s->rgb_stride = img.stride; - s->rgb_buf_size = img.size; - } - } - tbuffer_init(&s->ui_tb, UI_BUF_COUNT, "rgb"); - - if (s->cameras.front.ci.bayer) { -#ifdef QCOM2 - s->rgb_front_width = s->cameras.front.ci.frame_width; - s->rgb_front_height = s->cameras.front.ci.frame_height; -#else - s->rgb_front_width = s->cameras.front.ci.frame_width / 2; - s->rgb_front_height = s->cameras.front.ci.frame_height / 2; -#endif - } else { - s->rgb_front_width = s->cameras.front.ci.frame_width; - s->rgb_front_height = s->cameras.front.ci.frame_height; - } -#ifdef QCOM2 - s->rgb_wide_width = s->cameras.wide.ci.frame_width; - s->rgb_wide_height = s->cameras.wide.ci.frame_height; -#endif - - for (int i=0; irgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]); - s->rgb_front_bufs_cl[i] = visionbuf_to_cl(&s->rgb_front_bufs[i], s->device_id, s->context); - if (i == 0){ - s->rgb_front_stride = img.stride; - s->rgb_front_buf_size = img.size; - } - } - tbuffer_init(&s->ui_front_tb, UI_BUF_COUNT, "frontrgb"); -#ifdef QCOM2 - for (int i=0; irgb_wide_width, s->rgb_wide_height, &s->rgb_wide_bufs[i]); - s->rgb_wide_bufs_cl[i] = visionbuf_to_cl(&s->rgb_wide_bufs[i], s->device_id, s->context); - if (i == 0){ - s->rgb_wide_stride = img.stride; - s->rgb_wide_buf_size = img.size; - } - } - tbuffer_init(&s->ui_wide_tb, UI_BUF_COUNT, "widergb"); -#endif - - // yuv back for recording and orbd - pool_init(&s->yuv_pool, YUV_COUNT); - s->yuv_tb = pool_get_tbuffer(&s->yuv_pool); //only for visionserver... - - s->yuv_width = s->rgb_width; - s->yuv_height = s->rgb_height; - s->yuv_buf_size = s->rgb_width * s->rgb_height * 3 / 2; - - for (int i=0; iyuv_ion[i] = visionbuf_allocate_cl(s->yuv_buf_size, s->device_id, s->context, &s->yuv_cl[i]); - s->yuv_bufs[i].y = (uint8_t*)s->yuv_ion[i].addr; - s->yuv_bufs[i].u = s->yuv_bufs[i].y + (s->yuv_width * s->yuv_height); - s->yuv_bufs[i].v = s->yuv_bufs[i].u + (s->yuv_width/2 * s->yuv_height/2); - } - - // yuv front for recording - pool_init(&s->yuv_front_pool, YUV_COUNT); - s->yuv_front_tb = pool_get_tbuffer(&s->yuv_front_pool); - - s->yuv_front_width = s->rgb_front_width; - s->yuv_front_height = s->rgb_front_height; - s->yuv_front_buf_size = s->rgb_front_width * s->rgb_front_height * 3 / 2; - - for (int i=0; iyuv_front_ion[i] = visionbuf_allocate_cl(s->yuv_front_buf_size, s->device_id, s->context, &s->yuv_front_cl[i]); - s->yuv_front_bufs[i].y = (uint8_t*)s->yuv_front_ion[i].addr; - s->yuv_front_bufs[i].u = s->yuv_front_bufs[i].y + (s->yuv_front_width * s->yuv_front_height); - s->yuv_front_bufs[i].v = s->yuv_front_bufs[i].u + (s->yuv_front_width/2 * s->yuv_front_height/2); - } - - // yuv wide for recording -#ifdef QCOM2 - pool_init(&s->yuv_wide_pool, YUV_COUNT); - s->yuv_wide_tb = pool_get_tbuffer(&s->yuv_wide_pool); - - s->yuv_wide_width = s->rgb_wide_width; - s->yuv_wide_height = s->rgb_wide_height; - s->yuv_wide_buf_size = s->rgb_wide_width * s->rgb_wide_height * 3 / 2; - - for (int i=0; iyuv_wide_ion[i] = visionbuf_allocate_cl(s->yuv_wide_buf_size, s->device_id, s->context, &s->yuv_wide_cl[i]); - s->yuv_wide_bufs[i].y = (uint8_t*)s->yuv_wide_ion[i].addr; - s->yuv_wide_bufs[i].u = s->yuv_wide_bufs[i].y + (s->yuv_wide_width * s->yuv_wide_height); - s->yuv_wide_bufs[i].v = s->yuv_wide_bufs[i].u + (s->yuv_wide_width/2 * s->yuv_wide_height/2); - } -#endif - - if (s->cameras.rear.ci.bayer) { - // debayering does a 2x downscale - s->yuv_transform = transform_scale_buffer(s->cameras.rear.transform, 0.5); - } else { - s->yuv_transform = s->cameras.rear.transform; - } - - if (s->cameras.rear.ci.bayer) { - s->prg_debayer_rear = build_debayer_program(s, s->cameras.rear.ci.frame_width, s->cameras.rear.ci.frame_height, - s->cameras.rear.ci.frame_stride, - s->rgb_width, s->rgb_height, s->rgb_stride, - s->cameras.rear.ci.bayer_flip, s->cameras.rear.ci.hdr); - s->krnl_debayer_rear = clCreateKernel(s->prg_debayer_rear, "debayer10", &err); - assert(err == 0); - } - - if (s->cameras.front.ci.bayer) { - s->prg_debayer_front = build_debayer_program(s, s->cameras.front.ci.frame_width, s->cameras.front.ci.frame_height, - s->cameras.front.ci.frame_stride, - s->rgb_front_width, s->rgb_front_height, s->rgb_front_stride, - s->cameras.front.ci.bayer_flip, s->cameras.front.ci.hdr); - - s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err); - assert(err == 0); - } -#ifdef QCOM2 - if (s->cameras.wide.ci.bayer) { - s->prg_debayer_wide = build_debayer_program(s, s->cameras.wide.ci.frame_width, s->cameras.wide.ci.frame_height, - s->cameras.wide.ci.frame_stride, - s->rgb_wide_width, s->rgb_wide_height, s->rgb_wide_stride, - s->cameras.wide.ci.bayer_flip, s->cameras.wide.ci.hdr); - - s->krnl_debayer_wide = clCreateKernel(s->prg_debayer_wide, "debayer10", &err); - assert(err == 0); - } -#endif - s->debayer_cl_localMemSize = (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * sizeof(float); - s->debayer_cl_globalWorkSize[0] = s->rgb_width; - s->debayer_cl_globalWorkSize[1] = s->rgb_height; - s->debayer_cl_localWorkSize[0] = DEBAYER_LOCAL_WORKSIZE; - s->debayer_cl_localWorkSize[1] = DEBAYER_LOCAL_WORKSIZE; - -#ifdef QCOM - s->prg_rgb_laplacian = build_conv_program(s, s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y, - 3); - s->krnl_rgb_laplacian = clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err); - assert(err == 0); - // TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter - s->rgb_conv_roi_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE, - s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), NULL, NULL); - s->rgb_conv_result_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE, - s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * sizeof(int16_t), NULL, NULL); - s->rgb_conv_filter_cl = clCreateBuffer(s->context, 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] = s->rgb_width/NUM_SEGMENTS_X; - s->conv_cl_globalWorkSize[1] = s->rgb_height/NUM_SEGMENTS_Y; - s->conv_cl_localWorkSize[0] = CONV_LOCAL_WORKSIZE; - s->conv_cl_localWorkSize[1] = CONV_LOCAL_WORKSIZE; - - for (int i=0; i<(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1); i++) {s->lapres[i] = 16160;} -#endif - - rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride); - rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride); -#ifdef QCOM2 - rgb_to_yuv_init(&s->wide_rgb_to_yuv_state, s->context, s->device_id, s->yuv_wide_width, s->yuv_wide_height, s->rgb_wide_stride); -#endif -} - -void free_buffers(VisionState *s) { - // free bufs - for (int i=0; icamera_bufs[i]); - visionbuf_free(&s->front_camera_bufs[i]); -#ifdef QCOM - visionbuf_free(&s->focus_bufs[i]); - visionbuf_free(&s->stats_bufs[i]); -#elif defined(QCOM2) - visionbuf_free(&s->wide_camera_bufs[i]); -#endif - } - - for (int i=0; irgb_bufs[i]); - visionbuf_free(&s->rgb_front_bufs[i]); -#ifdef QCOM2 - visionbuf_free(&s->rgb_wide_bufs[i]); -#endif - } - - for (int i=0; iyuv_ion[i]); - visionbuf_free(&s->yuv_front_ion[i]); -#ifdef QCOM2 - visionbuf_free(&s->yuv_wide_ion[i]); -#endif - } - - clReleaseMemObject(s->rgb_conv_roi_cl); - clReleaseMemObject(s->rgb_conv_result_cl); - clReleaseMemObject(s->rgb_conv_filter_cl); - - clReleaseProgram(s->prg_debayer_rear); - clReleaseProgram(s->prg_debayer_front); - clReleaseKernel(s->krnl_debayer_rear); - clReleaseKernel(s->krnl_debayer_front); -#ifdef QCOM2 - clReleaseProgram(s->prg_debayer_wide); - clReleaseKernel(s->krnl_debayer_wide); -#endif - - clReleaseProgram(s->prg_rgb_laplacian); - clReleaseKernel(s->krnl_rgb_laplacian); +void party(cl_device_id device_id, cl_context context) { + VisionState state = {}; + VisionState *s = &state; -} - -void party(VisionState *s) { - int err; - - s->terminate_pub = zsock_new_pub("@inproc://terminate"); - assert(s->terminate_pub); - - pthread_t visionserver_thread_handle; - err = pthread_create(&visionserver_thread_handle, NULL, - visionserver_thread, s); - assert(err == 0); - - pthread_t proc_thread_handle; - err = pthread_create(&proc_thread_handle, NULL, - processing_thread, s); - assert(err == 0); - -#ifndef __APPLE__ - pthread_t frontview_thread_handle; - err = pthread_create(&frontview_thread_handle, NULL, - frontview_thread, s); - assert(err == 0); -#endif -#ifdef QCOM2 - pthread_t wideview_thread_handle; - err = pthread_create(&wideview_thread_handle, NULL, - wideview_thread, s); - assert(err == 0); -#endif + cameras_init(&s->cameras, device_id, context); + cameras_open(&s->cameras); + std::thread server_thread(visionserver_thread, s); + // priority for cameras - err = set_realtime_priority(51); + int err = set_realtime_priority(51); LOG("setpriority returns %d", err); cameras_run(&s->cameras); - tbuffer_stop(&s->ui_tb); - tbuffer_stop(&s->ui_front_tb); - pool_stop(&s->yuv_pool); - pool_stop(&s->yuv_front_pool); -#ifdef QCOM2 - tbuffer_stop(&s->ui_wide_tb); - pool_stop(&s->yuv_wide_pool); -#endif - - zsock_signal(s->terminate_pub, 0); - -#if (defined(QCOM) && !defined(QCOM_REPLAY)) || defined(WEBCAM) || defined(QCOM2) - LOG("joining frontview_thread"); - err = pthread_join(frontview_thread_handle, NULL); - assert(err == 0); -#endif -#ifdef QCOM2 - LOG("joining wideview_thread"); - err = pthread_join(wideview_thread_handle, NULL); - assert(err == 0); -#endif - LOG("joining visionserver_thread"); - err = pthread_join(visionserver_thread_handle, NULL); - assert(err == 0); - - LOG("joining proc_thread"); - err = pthread_join(proc_thread_handle, NULL); - assert(err == 0); - - zsock_destroy (&s->terminate_pub); + server_thread.join(); } int main(int argc, char *argv[]) { set_realtime_priority(51); -#ifdef QCOM +#if defined(QCOM) set_core_affinity(2); +#elif defined(QCOM2) + set_core_affinity(6); #endif - zsys_handler_set(NULL); signal(SIGINT, (sighandler_t)set_do_exit); signal(SIGTERM, (sighandler_t)set_do_exit); - VisionState state = {}; - VisionState *s = &state; - + int err; clu_init(); - cl_init(s); + 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); - cameras_init(&s->cameras); + party(device_id, context); - s->frame_width = s->cameras.rear.ci.frame_width; - s->frame_height = s->cameras.rear.ci.frame_height; - s->frame_stride = s->cameras.rear.ci.frame_stride; - s->frame_size = s->cameras.rear.frame_size; - - init_buffers(s); - -#if defined(QCOM) && !defined(QCOM_REPLAY) - s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"}); -#elif defined(QCOM2) - s->pm = new PubMaster({"frame", "frontFrame", "wideFrame", "thumbnail"}); -#endif - -#ifndef QCOM2 - cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]); -#else - cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0], &s->wide_camera_bufs[0]); -#endif - - party(s); - - if (s->pm != NULL) { - delete s->pm; - } - - free_buffers(s); - cl_free(s); + clReleaseContext(context); } diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 317618c0b..addf293cc 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -19,7 +19,7 @@ def snapshot(): params = Params() front_camera_allowed = int(params.get("RecordFront")) - if params.get("IsTakingSnapshot") == b"1": + if params.get("IsOffroad") != b"1" or params.get("IsTakingSnapshot") == b"1": return None params.put("IsTakingSnapshot", "1") diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 77f74681b..56e16e6d5 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -70,7 +70,7 @@ def get_can_signals(CP): ("SCM_BUTTONS", 25), ] - if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL): + if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G): checks += [ ("GEARBOX", 50), ] @@ -106,8 +106,7 @@ def get_can_signals(CP): checks += [("CRUISE_PARAMS", 10)] else: checks += [("CRUISE_PARAMS", 50)] - - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): + if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G): signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] elif CP.carFingerprint == CAR.ODYSSEY_CHN: signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)] @@ -189,7 +188,7 @@ class CarState(CarStateBase): # ******************* parse out can ******************* # TODO: find wheels moving bit in dbc - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): + if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G): ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']) elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: @@ -239,7 +238,7 @@ class CarState(CarStateBase): self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, - CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): + CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G): self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index d74db1b1f..7331c673c 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -196,7 +196,7 @@ class CarInterface(CarInterfaceBase): elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): stop_and_go = True if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch - ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.safetyParam = 1 # Accord(ICE), CRV 5G, and RDX 3G use an alternate user brake msg ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 @@ -243,7 +243,7 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CRV_5G: stop_and_go = True - ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.safetyParam = 1 # Accord(ICE), CRV 5G, and RDX 3G use an alternate user brake msg ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 @@ -265,7 +265,7 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CRV_HYBRID: stop_and_go = True - ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.safetyParam = 1 # Accord(ICE), CRV 5G, and RDX 3G use an alternate user brake msg ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 @@ -320,6 +320,21 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] + elif candidate == CAR.ACURA_RDX_3G: + stop_and_go = True + ret.safetyParam = 1 # Accord(ICE), CRV 5G, and RDX 3G use an alternate user brake msg + ret.mass = 4068. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.75 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 11.95 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + tire_stiffness_factor = 0.677 + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] + elif candidate == CAR.ODYSSEY: stop_and_go = False ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 6a92ced81..77d815301 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -40,6 +40,7 @@ class CAR: ODYSSEY = "HONDA ODYSSEY 2018 EX-L" ODYSSEY_CHN = "HONDA ODYSSEY 2019 EXCLUSIVE CHN" ACURA_RDX = "ACURA RDX 2018 ACURAWATCH PLUS" + ACURA_RDX_3G = "ACURA RDX 2020 TECH" PILOT = "HONDA PILOT 2017 TOURING" PILOT_2019 = "HONDA PILOT 2019 ELITE" RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION" @@ -65,6 +66,9 @@ FINGERPRINTS = { CAR.ACURA_RDX: [{ 57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 392: 6, 398: 3, 399: 6, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5, 1424: 5, 1729: 1 }], + CAR.ACURA_RDX_3G: [{ + 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 507: 1, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 815: 8, 825: 4, 829: 5, 846: 8, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 892: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 6, 1092: 1, 1108: 8, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8 + }], CAR.CIVIC: [{ 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 450: 8, 464: 8, 470: 2, 476: 7, 487: 4, 490: 8, 493: 5, 506: 8, 512: 6, 513: 6, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1633: 8, }], @@ -137,7 +141,7 @@ FINGERPRINTS = { } # Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection -IGNORED_FINGERPRINTS = [CAR.INSIGHT, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_EU, CAR.HRV] +IGNORED_FINGERPRINTS = [CAR.INSIGHT, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_EU, CAR.HRV, CAR.ACURA_RDX_3G] # add DIAG_MSGS to fingerprints for c in FINGERPRINTS: @@ -856,6 +860,19 @@ FW_VERSIONS = { b'57114-TGT-A530\x00\x00', ], }, + 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'], + }, CAR.RIDGELINE: { (Ecu.eps, 0x18da30f1, None): [ b'39990-T6Z-A020\x00\x00', @@ -864,6 +881,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-T6Z-A020\x00\x00', b'36161-T6Z-A310\x00\x00', + b'36161-T6Z-A420\x00\x00', b'36161-T6Z-A520\x00\x00', b'36161-TJZ-A120\x00\x00', ], @@ -874,6 +892,7 @@ FW_VERSIONS = { (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-T6Z-A420\x00\x00', b'78109-T6Z-A510\x00\x00', + b'78109-T6Z-A710\x00\x00', b'78109-TJZ-A510\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ @@ -931,6 +950,7 @@ DBC = { CAR.ACCORDH: dbc_dict('honda_accord_s2t_2018_can_generated', None), CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None), CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_civic_sedan_16_diesel_2019_can_generated', None), @@ -954,6 +974,7 @@ STEER_THRESHOLD = { CAR.ACCORDH: 1200, CAR.ACURA_ILX: 1200, CAR.ACURA_RDX: 400, + CAR.ACURA_RDX_3G: 1200, CAR.CIVIC: 1200, CAR.CIVIC_BOSCH: 1200, CAR.CIVIC_BOSCH_DIESEL: 1200, @@ -977,6 +998,7 @@ SPEED_FACTOR = { CAR.ACCORDH: 1., CAR.ACURA_ILX: 1., CAR.ACURA_RDX: 1., + CAR.ACURA_RDX_3G: 1., CAR.CIVIC: 1., CAR.CIVIC_BOSCH: 1., CAR.CIVIC_BOSCH_DIESEL: 1., @@ -1000,4 +1022,4 @@ ECU_FINGERPRINT = { Ecu.fwdCamera: [0xE4, 0x194], # steer torque cmd } -HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT]) +HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G]) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index c47545ef2..736435615 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -77,9 +77,9 @@ class CarController(): if pcm_cancel_cmd: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: - # send resume at a max freq of 5Hz - if (frame - self.last_resume_frame)*DT_CTRL > 0.2: - can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) + # send resume at a max freq of 10Hz + if (frame - self.last_resume_frame)*DT_CTRL > 0.1: + can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 20) self.last_resume_frame = frame # 20 Hz LFA MFA message diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index fd53ffc8a..b799a13fe 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -29,8 +29,8 @@ class CarState(CarStateBase): ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle'] ret.steeringRate = cp.vl["SAS11"]['SAS_Speed'] ret.yawRate = cp.vl["ESP12"]['YAW_RATE'] - ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0 - ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0 + ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]['CF_Gway_TurnSigLh'], + cp.vl["CGW1"]['CF_Gway_TurnSigRh']) ret.steeringTorque = cp.vl["MDPS12"]['CR_Mdps_StrColTq'] ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq'] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD @@ -151,9 +151,7 @@ class CarState(CarStateBase): ("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door ("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door ("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door - ("CF_Gway_TSigLHSw", "CGW1", 0), ("CF_Gway_TurnSigLh", "CGW1", 0), - ("CF_Gway_TSigRHSw", "CGW1", 0), ("CF_Gway_TurnSigRh", "CGW1", 0), ("CF_Gway_ParkBrakeSw", "CGW1", 0), diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index c36ecbd96..6b67e84fc 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -85,7 +85,7 @@ FINGERPRINTS = { 67: 8, 68: 8, 80: 4, 160: 8, 161: 8, 272: 8, 288: 4, 339: 8, 356: 8, 357: 8, 399: 8, 544: 8, 608: 8, 672: 8, 688: 5, 704: 1, 790: 8, 809: 8, 848: 8, 880: 8, 898: 8, 900: 8, 901: 8, 904: 8, 1056: 8, 1064: 8, 1065: 8, 1072: 8, 1075: 8, 1087: 8, 1088: 8, 1151: 8, 1200: 8, 1201: 8, 1232: 4, 1264: 8, 1265: 8, 1266: 8, 1296: 8, 1306: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1348: 8, 1349: 8, 1369: 8, 1370: 8, 1371: 8, 1407: 8, 1415: 8, 1419: 8, 1440: 8, 1442: 4, 1461: 8, 1470: 8 }], CAR.SONATA: [ - {67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 865: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 908: 8, 909: 8, 912: 7, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1089: 5, 1096: 8, 1107: 5, 1108: 8, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1184: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1268: 8, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1330: 8, 1339: 8, 1342: 6, 1343: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1379: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1460: 8, 1470: 8, 1485: 8, 1504: 3, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8}, + {67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 549: 8, 550: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 865: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 908: 8, 909: 8, 912: 7, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1089: 5, 1096: 8, 1107: 5, 1108: 8, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1184: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1268: 8, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1330: 8, 1339: 8, 1342: 6, 1343: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1379: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1460: 8, 1470: 8, 1485: 8, 1504: 3, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8}, ], CAR.SONATA_2019: [ {66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8}, @@ -139,7 +139,7 @@ FINGERPRINTS = { 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1265: 4, 1268: 8, 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, 1371: 8, 1407: 8, 1419: 8, 1420: 8, 1425: 2, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 }], CAR.PALISADE: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8 + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 547: 8, 548: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8 }], CAR.VELOSTER: [{ 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1181: 5, 1186: 2, 1191: 2, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1378: 4, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1872: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 @@ -164,6 +164,7 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x87391162M003\xf1\xa0000F', + b'\xf1\x87391162M003\xf1\xa00240', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101\xf1\xa01.01', @@ -202,23 +203,28 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424',], (Ecu.transmission, 0x7e1, None): [b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91",], }, - CAR.PALISADE: { + CAR.PALISADE: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04', b'\xf1\x00LX2 SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04', ], (Ecu.esp, 0x7d1, None): [ - b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330', b'\xf1\x00LX ESC \v 102\x19\x05\a 58910-S8330\xf1\xa01.02', b'\xf1\x00LX ESC \v 103\x19\t\x10 58910-S8360\xf1\xa01.03', + b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360\xf1\xa01.03', + b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640K0051\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.engine, 0x7e0, None): [b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00',], (Ecu.eps, 0x7d4, None): [b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103',], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.05 99211-S8100 190909', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', b'\xf1\x87LDLVBN560098KF26\x86fff\x87vgfg\x88\x96xfw\x86gfw\x86g\x95\xf6\xffeU_\xff\x92c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', ], diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index bd5c8eb4b..a4dcb905c 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -149,9 +149,12 @@ class CarStateBase: def __init__(self, CP): self.CP = CP self.car_fingerprint = CP.carFingerprint - self.cruise_buttons = 0 self.out = car.CarState.new_message() + self.cruise_buttons = 0 + self.left_blinker_cnt = 0 + self.right_blinker_cnt = 0 + # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], @@ -166,6 +169,11 @@ class CarStateBase: v_ego_x = self.v_ego_kf.update(v_ego_raw) return float(v_ego_x[0]), float(v_ego_x[1]) + def update_blinker(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool): + self.left_blinker_cnt = blinker_time if left_blinker_lamp else max(self.left_blinker_cnt - 1, 0) + self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0) + 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, diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 0630a8cb9..ea4af219e 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -23,10 +23,14 @@ FINGERPRINTS = { { 2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8, 643: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8 }, + # 2020 Leaf SV Plus + { + 2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8 + }, ], CAR.ROGUE: [ { - 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1821: 8, 1823: 8, 1837: 8, 1839: 8 + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }, ] } diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 5b5e23599..3be516ed3 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -10,8 +10,6 @@ from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CAR class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - self.left_blinker_cnt = 0 - self.right_blinker_cnt = 0 can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["Transmission"]['Gear'] @@ -36,10 +34,8 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw < 0.01 # continuous blinker signals for assisted lane change - self.left_blinker_cnt = 50 if cp.vl["Dashlights"]['LEFT_BLINKER'] else max(self.left_blinker_cnt - 1, 0) - ret.leftBlinker = self.left_blinker_cnt > 0 - self.right_blinker_cnt = 50 if cp.vl["Dashlights"]['RIGHT_BLINKER'] else max(self.right_blinker_cnt - 1, 0) - ret.rightBlinker = self.right_blinker_cnt > 0 + ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["Dashlights"]['LEFT_BLINKER'], + cp.vl["Dashlights"]['RIGHT_BLINKER']) ret.leftBlindspot = (cp.vl["BSD_RCTA"]['L_ADJACENT'] == 1) or (cp.vl["BSD_RCTA"]['L_APPROACHING'] == 1) ret.rightBlindspot = (cp.vl["BSD_RCTA"]['R_ADJACENT'] == 1) or (cp.vl["BSD_RCTA"]['R_APPROACHING'] == 1) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index e97cc23af..2f8c0bc9a 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -317,7 +317,7 @@ class CarInterface(CarInterfaceBase): # events events = self.create_common_events(ret) - if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera: + if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera and not self.CP.isPandaBlack: events.add(EventName.invalidGiraffeToyota) if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: events.add(EventName.lowSpeedLockout) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 93176b359..14bc40744 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -69,7 +69,7 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep values = { "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, - "BARRIERS" : 3 if left_lane_depart or right_lane_depart else 0, + "BARRIERS" : 3 if left_lane_depart else 2 if right_lane_depart else 0, "SET_ME_X0C": 0x0c, "SET_ME_X2C": 0x2c, "SET_ME_X38": 0x38, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index f1d31fb4d..89b27737e 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -914,6 +914,7 @@ FW_VERSIONS = { b'\x018966342T6000\x00\x00\x00\x00', b'\x018966342T9000\x00\x00\x00\x00', b'\x018966342U4000\x00\x00\x00\x00', + b'\x018966342U4100\x00\x00\x00\x00', b'\x018966342V3100\x00\x00\x00\x00', b'\x018966342V3200\x00\x00\x00\x00', b'\x018966342X5000\x00\x00\x00\x00', @@ -922,6 +923,7 @@ FW_VERSIONS = { b'\x01896634A19100\x00\x00\x00\x00', b'\x01896634A20000\x00\x00\x00\x00', b'\x01896634A22000\x00\x00\x00\x00', + b'\x01896634A46000\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', @@ -943,6 +945,7 @@ FW_VERSIONS = { b'8965B42171\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', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -966,6 +969,7 @@ FW_VERSIONS = { 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', + b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152642291\x00\x00\x00\x00\x00\x00', @@ -1019,6 +1023,7 @@ FW_VERSIONS = { b'\x01896630838000\x00\x00\x00\x00', b'\x01896630838100\x00\x00\x00\x00', b'\x01896630842000\x00\x00\x00\x00', + b'\x01896630843000\x00\x00\x00\x00', b'\x01896630851000\x00\x00\x00\x00', b'\x01896630851100\x00\x00\x00\x00', b'\x01896630852100\x00\x00\x00\x00', @@ -1102,6 +1107,7 @@ FW_VERSIONS = { b'\x01896630E36300\x00\x00\x00\x00', b'\x018966348R8500\x00\x00\x00\x00', b'\x018966348W1300\x00\x00\x00\x00', + b'\x01896630EA4300\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152648472\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index d511ebf41..c89144d49 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -13,33 +13,44 @@ #include #include +#include +#include #include #include "common/util.h" #include "common/utilpp.h" -namespace { - -template -T* null_coalesce(T* a, T* b) { - return a != NULL ? a : b; +std::string getenv_default(const char* env_var, const char * suffix, const char* default_val) { + const char* env_val = getenv(env_var); + if (env_val != NULL){ + return std::string(env_val) + std::string(suffix); + } else{ + return std::string(default_val); + } } -static const char* default_params_path = null_coalesce(const_cast(getenv("PARAMS_PATH")), "/data/params"); - -#ifdef QCOM -static const char* persistent_params_path = null_coalesce(const_cast(getenv("PERSISTENT_PARAMS_PATH")), "/persist/comma/params"); +#if defined(QCOM) || defined(QCOM2) +const std::string default_params_path = "/data/params"; #else -static const char* persistent_params_path = default_params_path; +const std::string default_params_path = getenv_default("HOME", "/.comma/params", "/data/params"); #endif -} //namespace +#if defined(QCOM) || defined(QCOM2) +const std::string persistent_params_path = "/persist/comma/params"; +#else +const std::string persistent_params_path = default_params_path; +#endif +volatile sig_atomic_t params_do_exit = 0; +void params_sig_handler(int signal) { + params_do_exit = 1; +} + static int fsync_dir(const char* path){ int result = 0; - int fd = open(path, O_RDONLY); + int fd = open(path, O_RDONLY, 0755); if (fd < 0){ result = -1; @@ -64,15 +75,46 @@ static int fsync_dir(const char* path){ } } -static int ensure_dir_exists(const char* path) { - struct stat st; - if (stat(path, &st) == -1) { - return mkdir(path, 0700); +static int mkdir_p(std::string path) { + char * _path = (char *)path.c_str(); + + mode_t prev_mask = umask(0); + for (char *p = _path + 1; *p; p++) { + if (*p == '/') { + *p = '\0'; // Temporarily truncate + if (mkdir(_path, 0777) != 0) { + if (errno != EEXIST) return -1; + } + *p = '/'; + } } + if (mkdir(_path, 0777) != 0) { + if (errno != EEXIST) return -1; + } + chmod(_path, 0777); + umask(prev_mask); return 0; } -int write_db_value(const char* key, const char* value, size_t value_size, bool persistent_param) { +static int ensure_dir_exists(std::string path) { + // TODO: replace by std::filesystem::create_directories + return mkdir_p(path.c_str()); +} + + +Params::Params(bool persistent_param){ + params_path = persistent_param ? persistent_params_path : default_params_path; +} + +Params::Params(std::string path) { + params_path = path; +} + +int Params::write_db_value(std::string key, std::string dat){ + return write_db_value(key.c_str(), dat.c_str(), dat.length()); +} + +int Params::write_db_value(const char* key, const char* value, size_t value_size) { // Information about safely and atomically writing a file: https://lwn.net/Articles/457667/ // 1) Create temp file // 2) Write data to temp file @@ -83,11 +125,9 @@ int write_db_value(const char* key, const char* value, size_t value_size, bool p int lock_fd = -1; int tmp_fd = -1; int result; - char tmp_path[1024]; - char path[1024]; - char *tmp_dir; + std::string path; + std::string tmp_path; ssize_t bytes_written; - const char* params_path = persistent_param ? persistent_params_path : default_params_path; // Make sure params path exists result = ensure_dir_exists(params_path); @@ -95,77 +135,61 @@ int write_db_value(const char* key, const char* value, size_t value_size, bool p goto cleanup; } - result = snprintf(path, sizeof(path), "%s/d", params_path); - if (result < 0) { - goto cleanup; - } - // See if the symlink exists, otherwise create it + path = params_path + "/d"; struct stat st; - if (stat(path, &st) == -1) { + if (stat(path.c_str(), &st) == -1) { // Create temp folder - result = snprintf(path, sizeof(path), "%s/.tmp_XXXXXX", params_path); - if (result < 0) { - goto cleanup; - } - tmp_dir = mkdtemp(path); - if (tmp_dir == NULL){ + path = params_path + "/.tmp_XXXXXX"; + + char *t = mkdtemp((char*)path.c_str()); + if (t == NULL){ goto cleanup; } + std::string tmp_dir(t); // Set permissions - result = chmod(tmp_dir, 0777); + result = chmod(tmp_dir.c_str(), 0777); if (result < 0) { goto cleanup; } // Symlink it to temp link - result = snprintf(tmp_path, sizeof(tmp_path), "%s.link", tmp_dir); - if (result < 0) { - goto cleanup; - } - result = symlink(tmp_dir, tmp_path); + tmp_path = tmp_dir + ".link"; + result = symlink(tmp_dir.c_str(), tmp_path.c_str()); if (result < 0) { goto cleanup; } // Move symlink to /d - result = snprintf(path, sizeof(path), "%s/d", params_path); + path = params_path + "/d"; + result = rename(tmp_path.c_str(), path.c_str()); if (result < 0) { goto cleanup; } - result = rename(tmp_path, path); + } else { + // Ensure permissions are correct in case we didn't create the symlink + result = chmod(path.c_str(), 0777); if (result < 0) { goto cleanup; } } // Write value to temp. - result = - snprintf(tmp_path, sizeof(tmp_path), "%s/.tmp_value_XXXXXX", params_path); - if (result < 0) { - goto cleanup; - } - - tmp_fd = mkstemp(tmp_path); + tmp_path = params_path + "/.tmp_value_XXXXXX"; + tmp_fd = mkstemp((char*)tmp_path.c_str()); bytes_written = write(tmp_fd, value, value_size); - if (bytes_written != value_size) { + if (bytes_written < 0 || (size_t)bytes_written != value_size) { result = -20; goto cleanup; } // Build lock path - result = snprintf(path, sizeof(path), "%s/.lock", params_path); - if (result < 0) { - goto cleanup; - } - lock_fd = open(path, O_CREAT); + path = params_path + "/.lock"; + lock_fd = open(path.c_str(), O_CREAT, 0775); // Build key path - result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key); - if (result < 0) { - goto cleanup; - } + path = params_path + "/d/" + std::string(key); // Take lock. result = flock(lock_fd, LOCK_EX); @@ -186,18 +210,14 @@ int write_db_value(const char* key, const char* value, size_t value_size, bool p } // Move temp into place. - result = rename(tmp_path, path); + result = rename(tmp_path.c_str(), path.c_str()); if (result < 0) { goto cleanup; } // fsync parent directory - result = snprintf(path, sizeof(path), "%s/d", params_path); - if (result < 0) { - goto cleanup; - } - - result = fsync_dir(path); + path = params_path + "/d"; + result = fsync_dir(path.c_str()); if (result < 0) { goto cleanup; } @@ -209,25 +229,25 @@ cleanup: } if (tmp_fd >= 0) { if (result < 0) { - remove(tmp_path); + remove(tmp_path.c_str()); } close(tmp_fd); } return result; } -int delete_db_value(const char* key, bool persistent_param) { +int Params::delete_db_value(std::string key) { + return delete_db_value(key.c_str()); +} + +int Params::delete_db_value(const char* key) { int lock_fd = -1; int result; - char path[1024]; - const char* params_path = persistent_param ? persistent_params_path : default_params_path; + std::string path; // Build lock path, and open lockfile - result = snprintf(path, sizeof(path), "%s/.lock", params_path); - if (result < 0) { - goto cleanup; - } - lock_fd = open(path, O_CREAT); + path = params_path + "/.lock"; + lock_fd = open(path.c_str(), O_CREAT, 0775); // Take lock. result = flock(lock_fd, LOCK_EX); @@ -235,26 +255,17 @@ int delete_db_value(const char* key, bool persistent_param) { goto cleanup; } - // Build key path - result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key); - if (result < 0) { - goto cleanup; - } - // Delete value. - result = remove(path); + path = params_path + "/d/" + std::string(key); + result = remove(path.c_str()); if (result != 0) { result = ERR_NO_VALUE; goto cleanup; } // fsync parent directory - result = snprintf(path, sizeof(path), "%s/d", params_path); - if (result < 0) { - goto cleanup; - } - - result = fsync_dir(path); + path = params_path + "/d"; + result = fsync_dir(path.c_str()); if (result < 0) { goto cleanup; } @@ -267,39 +278,58 @@ cleanup: return result; } -int read_db_value(const char* key, char** value, size_t* value_sz, bool persistent_param) { - char path[1024]; - const char* params_path = persistent_param ? persistent_params_path : default_params_path; +std::string Params::get(std::string key, bool block){ + char* value; + size_t size; + int r; - int result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key); - if (result < 0) { - return result; + if (block){ + r = read_db_value_blocking((const char*)key.c_str(), &value, &size); + } else { + r = read_db_value((const char*)key.c_str(), &value, &size); } - *value = static_cast(read_file(path, value_sz)); + if (r == 0){ + std::string s(value, size); + free(value); + return s; + } else { + return ""; + } +} + +int Params::read_db_value(const char* key, char** value, size_t* value_sz) { + std::string path = params_path + "/d/" + std::string(key); + *value = static_cast(read_file(path.c_str(), value_sz)); if (*value == NULL) { return -22; } return 0; } -void read_db_value_blocking(const char* key, char** value, size_t* value_sz, bool persistent_param) { - while (1) { - const int result = read_db_value(key, value, value_sz, persistent_param); +int Params::read_db_value_blocking(const char* key, char** value, size_t* value_sz) { + params_do_exit = 0; + void (*prev_handler_sigint)(int) = std::signal(SIGINT, params_sig_handler); + void (*prev_handler_sigterm)(int) = std::signal(SIGTERM, params_sig_handler); + + while (!params_do_exit) { + const int result = read_db_value(key, value, value_sz); if (result == 0) { - return; + break; } else { - // Sleep for 0.1 seconds. - usleep(100000); + usleep(100000); // 0.1 s } } + + std::signal(SIGINT, prev_handler_sigint); + std::signal(SIGTERM, prev_handler_sigterm); + return params_do_exit; // Return 0 if we had no interrupt } -int read_db_all(std::map *params, bool persistent_param) { +int Params::read_db_all(std::map *params) { int err = 0; - const char* params_path = persistent_param ? persistent_params_path : default_params_path; - std::string lock_path = util::string_format("%s/.lock", params_path); + std::string lock_path = params_path + "/.lock"; int lock_fd = open(lock_path.c_str(), 0); if (lock_fd < 0) return -1; @@ -310,7 +340,7 @@ int read_db_all(std::map *params, bool persistent_para return err; } - std::string key_path = util::string_format("%s/d", params_path); + std::string key_path = params_path + "/d"; DIR *d = opendir(key_path.c_str()); if (!d) { close(lock_fd); @@ -321,7 +351,7 @@ int read_db_all(std::map *params, bool persistent_para while ((de = readdir(d))) { if (!isalnum(de->d_name[0])) continue; std::string key = std::string(de->d_name); - std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str())); + std::string value = util::read_file(key_path + "/" + key); (*params)[key] = value; } @@ -332,11 +362,11 @@ int read_db_all(std::map *params, bool persistent_para return 0; } -std::vector read_db_bytes(const char* param_name, bool persistent_param) { +std::vector Params::read_db_bytes(const char* param_name) { std::vector bytes; char* value; size_t sz; - int result = read_db_value(param_name, &value, &sz, persistent_param); + int result = read_db_value(param_name, &value, &sz); if (result == 0) { bytes.assign(value, value+sz); free(value); @@ -344,7 +374,7 @@ std::vector read_db_bytes(const char* param_name, bool persistent_param) { return bytes; } -bool read_db_bool(const char* param_name, bool persistent_param) { - std::vector bytes = read_db_bytes(param_name, persistent_param); +bool Params::read_db_bool(const char* param_name) { + std::vector bytes = read_db_bytes(param_name); return bytes.size() > 0 and bytes[0] == '1'; } diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index 394c9b6ed..d8c96a8cb 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -1,44 +1,47 @@ #pragma once #include - -#ifdef __cplusplus -extern "C" { -#endif - -#define ERR_NO_VALUE -33 - -int write_db_value(const char* key, const char* value, size_t value_size, bool persistent_param = false); - -// Reads a value from the params database. -// Inputs: -// key: The key to read. -// value: A pointer where a newly allocated string containing the db value will -// be written. -// value_sz: A pointer where the size of value will be written. Does not -// include the NULL terminator. -// persistent_param: Boolean indicating if the param store in the /persist partition is to be used. -// e.g. for sensor calibration files. Will not be cleared after wipe or re-install. -// -// Returns: Negative on failure, otherwise 0. -int read_db_value(const char* key, char** value, size_t* value_sz, bool persistent_param = false); - -// Delete a value from the params database. -// Inputs are the same as read_db_value, without value and value_sz. -int delete_db_value(const char* key, bool persistent_param = false); - -// Reads a value from the params database, blocking until successful. -// Inputs are the same as read_db_value. -void read_db_value_blocking(const char* key, char** value, size_t* value_sz, bool persistent_param = false); - -#ifdef __cplusplus -} // extern "C" -#endif - -#ifdef __cplusplus #include #include #include -int read_db_all(std::map *params, bool persistent_param = false); -std::vector read_db_bytes(const char* param_name, bool persistent_param = false); -bool read_db_bool(const char* param_name, bool persistent_param = false); -#endif + +#define ERR_NO_VALUE -33 + +class Params { +private: + std::string params_path; + +public: + Params(bool persistent_param = false); + Params(std::string path); + + int write_db_value(std::string key, std::string dat); + int write_db_value(const char* key, const char* value, size_t value_size); + + // Reads a value from the params database. + // Inputs: + // key: The key to read. + // value: A pointer where a newly allocated string containing the db value will + // be written. + // value_sz: A pointer where the size of value will be written. Does not + // include the NULL terminator. + // persistent_param: Boolean indicating if the param store in the /persist partition is to be used. + // e.g. for sensor calibration files. Will not be cleared after wipe or re-install. + // + // Returns: Negative on failure, otherwise 0. + int read_db_value(const char* key, char** value, size_t* value_sz); + + // Delete a value from the params database. + // Inputs are the same as read_db_value, without value and value_sz. + int delete_db_value(std::string key); + int delete_db_value(const char* key); + + // Reads a value from the params database, blocking until successful. + // Inputs are the same as read_db_value. + int read_db_value_blocking(const char* key, char** value, size_t* value_sz); + + int read_db_all(std::map *params); + std::vector read_db_bytes(const char* param_name); + bool read_db_bool(const char* param_name); + + std::string get(std::string key, bool block=false); +}; diff --git a/selfdrive/common/util.c b/selfdrive/common/util.c index 33930f9ec..49f53db33 100644 --- a/selfdrive/common/util.c +++ b/selfdrive/common/util.c @@ -4,13 +4,16 @@ #include #include #include -#include +#include + #ifdef __linux__ #include #include +#ifndef __USE_GNU #define __USE_GNU -#include #endif +#include +#endif // __linux__ void* read_file(const char* path, size_t* out_len) { FILE* f = fopen(path, "r"); @@ -49,7 +52,7 @@ int write_file(const char* path, const void* data, size_t size) { } ssize_t n = write(fd, data, size); close(fd); - return n == size ? 0 : -1; + return (n >= 0 && (size_t)n == size) ? 0 : -1; } void set_thread_name(const char* name) { diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index 18d9619ce..c7a1dbfc8 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -21,12 +21,6 @@ typedef void (*sighandler_t)(int sig); #endif -#define clamp(a,b,c) \ - ({ __typeof__ (a) _a = (a); \ - __typeof__ (b) _b = (b); \ - __typeof__ (c) _c = (c); \ - _a < _b ? _b : (_a > _c ? _c : _a); }) - #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) #undef ALIGN diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index ddbf69550..fb8618cd8 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.7.9-release" +#define COMMA_VERSION "0.7.10-release" diff --git a/selfdrive/common/visionbuf.h b/selfdrive/common/visionbuf.h index 95f4826a9..8716b456c 100644 --- a/selfdrive/common/visionbuf.h +++ b/selfdrive/common/visionbuf.h @@ -1,5 +1,4 @@ -#ifndef IONBUF_H -#define IONBUF_H +#pragma once #define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ @@ -29,13 +28,10 @@ typedef struct VisionBuf { #define VISIONBUF_SYNC_TO_DEVICE 1 VisionBuf visionbuf_allocate(size_t len); -VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem); -cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx); +VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx); void visionbuf_sync(const VisionBuf* buf, int dir); void visionbuf_free(const VisionBuf* buf); #ifdef __cplusplus } #endif - -#endif diff --git a/selfdrive/common/visionbuf_cl.c b/selfdrive/common/visionbuf_cl.c index 21f100507..4c9ba59be 100644 --- a/selfdrive/common/visionbuf_cl.c +++ b/selfdrive/common/visionbuf_cl.c @@ -45,17 +45,8 @@ VisionBuf visionbuf_allocate(size_t len) { }; } -cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) { - // HACK because this platform is just for convenience - VisionBuf *w_buf = (VisionBuf*)buf; - cl_mem ret; - *w_buf = visionbuf_allocate_cl(buf->len, device_id, ctx, &ret); - return ret; -} - -VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) { +VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx) { int err; - assert(out_mem); #if __OPENCL_VERSION__ >= 200 void* host_ptr = @@ -72,8 +63,6 @@ VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context c cl_mem mem = clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, len, host_ptr, &err); assert(err == 0); - *out_mem = mem; - return (VisionBuf){ .len = len, .addr = host_ptr, .handle = 0, .fd = fd, .device_id = device_id, .ctx = ctx, .buf_cl = mem, diff --git a/selfdrive/common/visionbuf_ion.c b/selfdrive/common/visionbuf_ion.c index 3068994d2..62eaa3375 100644 --- a/selfdrive/common/visionbuf_ion.c +++ b/selfdrive/common/visionbuf_ion.c @@ -71,32 +71,27 @@ VisionBuf visionbuf_allocate(size_t len) { }; } -VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) { - VisionBuf r = visionbuf_allocate(len); - *out_mem = visionbuf_to_cl(&r, device_id, ctx); - r.buf_cl = *out_mem; - return r; -} +VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx) { + VisionBuf buf = visionbuf_allocate(len); + int err = 0; -cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) { - int err = 0; - - assert(((uintptr_t)buf->addr % DEVICE_PAGE_SIZE_CL) == 0); + assert(((uintptr_t)buf.addr % DEVICE_PAGE_SIZE_CL) == 0); cl_mem_ion_host_ptr ion_cl = {0}; ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM; ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM; - ion_cl.ion_filedesc = buf->fd; - ion_cl.ion_hostptr = buf->addr; + ion_cl.ion_filedesc = buf.fd; + ion_cl.ion_hostptr = buf.addr; - cl_mem mem = clCreateBuffer(ctx, + buf.buf_cl = clCreateBuffer(ctx, CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM, - buf->len, &ion_cl, &err); + buf.len, &ion_cl, &err); assert(err == 0); - return mem; + return buf; } + void visionbuf_sync(const VisionBuf* buf, int dir) { int err; diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc index e0220c18f..5f3002992 100644 --- a/selfdrive/common/visionimg.cc +++ b/selfdrive/common/visionimg.cc @@ -44,19 +44,18 @@ void visionimg_compute_aligned_width_and_height(int width, int height, int *alig #endif } -VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) { +VisionImg visionimg_alloc_rgb24(cl_device_id device_id, cl_context ctx, int width, int height, VisionBuf *out_buf) { + assert(out_buf != nullptr); int aligned_w = 0, aligned_h = 0; visionimg_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h); int stride = aligned_w * 3; size_t size = (size_t) aligned_w * aligned_h * 3; - VisionBuf buf = visionbuf_allocate(size); - - *out_buf = buf; + *out_buf = visionbuf_allocate_cl(size, device_id, ctx); return (VisionImg){ - .fd = buf.fd, + .fd = out_buf->fd, .format = VISIONIMG_FORMAT_RGB24, .width = width, .height = height, diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h index e5eb78766..14ee9f863 100644 --- a/selfdrive/common/visionimg.h +++ b/selfdrive/common/visionimg.h @@ -28,7 +28,7 @@ typedef struct VisionImg { } VisionImg; void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h); -VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf); +VisionImg visionimg_alloc_rgb24(cl_device_id device_id, cl_context ctx, int width, int height, VisionBuf *out_buf); EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph); GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph); diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d13d83cf8..8dd19a54d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -3,7 +3,7 @@ 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_rt_process, Priority, Ratekeeper, DT_CTRL +from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL from common.profiler import Profiler from common.params import Params, put_nonblocking import cereal.messaging as messaging @@ -42,7 +42,7 @@ EventName = car.CarEvent.EventName class Controls: def __init__(self, sm=None, pm=None, can_sock=None): - config_rt_process(3, Priority.CTRL_HIGH) + config_realtime_process(3, Priority.CTRL_HIGH) # Setup sockets self.pm = pm @@ -52,7 +52,7 @@ class Controls: self.sm = sm if self.sm is None: - self.sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration', + self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) self.can_sock = can_sock @@ -121,6 +121,7 @@ class Controls: self.last_blinker_frame = 0 self.saturated_count = 0 self.distance_traveled = 0 + self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] @@ -171,6 +172,14 @@ class Controls: if self.sm['thermal'].memUsedPercent > 90: self.events.add(EventName.lowMemory) + # Alert if fan isn't spinning for 5 seconds + if self.sm['health'].hwType in [HwType.uno, HwType.dos]: + if self.sm['health'].fanSpeedRpm == 0 and self.sm['thermal'].fanSpeed > 50: + if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: + self.events.add(EventName.fanMalfunction) + else: + self.last_functional_fan_frame = self.sm.frame + # Handle calibration status cal_status = self.sm['liveCalibration'].calStatus if cal_status != Calibration.CALIBRATED: @@ -218,9 +227,6 @@ class Controls: self.events.add(EventName.posenetInvalid) if not self.sm['liveLocationKalman'].deviceStable: self.events.add(EventName.deviceFalling) - if not self.sm['frame'].recoverState < 2: - # counter>=2 is active - self.events.add(EventName.focusRecoverActive) if not self.sm['plan'].radarValid: self.events.add(EventName.radarFault) if self.sm['plan'].radarCanError: @@ -438,9 +444,10 @@ class Controls: if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: self.events.add(EventName.ldw) + clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) self.AM.add_many(self.sm.frame, alerts, self.enabled) - self.AM.process_alerts(self.sm.frame) + self.AM.process_alerts(self.sm.frame, clear_event) CC.hudControl.visualAlert = self.AM.visual_alert if not self.read_only: diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 6eaa86d3e..44d6f9877 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -32,9 +32,6 @@ class AlertManager: self.activealerts: List[Alert] = [] self.clear_current_alert() - def alert_present(self) -> bool: - return len(self.activealerts) > 0 - def clear_current_alert(self) -> None: self.alert_type: str = "" self.alert_text_1: str = "" @@ -46,34 +43,32 @@ class AlertManager: self.alert_rate: float = 0. def add_many(self, frame: int, alerts: List[Alert], enabled: bool = True) -> None: - for a in alerts: - self.add(frame, a, enabled=enabled) + for alert in alerts: + added_alert = copy.copy(alert) + added_alert.start_time = frame * DT_CTRL - def add(self, frame: int, alert: Alert, enabled: bool = True) -> None: - added_alert = copy.copy(alert) - added_alert.start_time = frame * DT_CTRL + # if new alert is higher priority, log it + if not len(self.activealerts) or added_alert.alert_priority > self.activealerts[0].alert_priority: + cloudlog.event('alert_add', alert_type=added_alert.alert_type, enabled=enabled) - # if new alert is higher priority, log it - if not self.alert_present() or added_alert.alert_priority > self.activealerts[0].alert_priority: - cloudlog.event('alert_add', alert_type=added_alert.alert_type, enabled=enabled) + self.activealerts.append(added_alert) - self.activealerts.append(added_alert) + def process_alerts(self, frame: int, clear_event_type=None) -> None: + cur_time = frame * DT_CTRL + + # first get rid of all the expired alerts + self.activealerts = [a for a in self.activealerts if a.event_type != clear_event_type and + a.start_time + max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time] # sort by priority first and then by start_time self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True) - def process_alerts(self, frame: int) -> None: - cur_time = frame * DT_CTRL - - # first get rid of all the expired alerts - self.activealerts = [a for a in self.activealerts if a.start_time + - max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time] - # start with assuming no alerts self.clear_current_alert() - current_alert = self.activealerts[0] if self.alert_present() else None - if current_alert is not None: + if len(self.activealerts): + current_alert = self.activealerts[0] + self.alert_type = current_alert.alert_type if current_alert.start_time + current_alert.duration_sound > cur_time: diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 67a6838d7..be23d3b0f 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1,4 +1,4 @@ -from functools import total_ordering +from enum import IntEnum from typing import Dict, Union, Callable, Any from cereal import log, car @@ -14,7 +14,7 @@ AudibleAlert = car.CarControl.HUDControl.AudibleAlert EventName = car.CarEvent.EventName # Alert priorities -class Priority: +class Priority(IntEnum): LOWEST = 0 LOWER = 1 LOW = 2 @@ -79,6 +79,7 @@ class Events: if DT_CTRL * (self.events_prev[e] + 1) >= alert.creation_delay: alert.alert_type = f"{EVENT_NAME[e]}/{et}" + alert.event_type = et ret.append(alert) return ret @@ -96,23 +97,21 @@ class Events: ret.append(event) return ret -@total_ordering class Alert: def __init__(self, alert_text_1: str, alert_text_2: str, - alert_status, - alert_size, - alert_priority, - visual_alert, - audible_alert, + alert_status: log.ControlsState.AlertStatus, + alert_size: log.ControlsState.AlertSize, + alert_priority: Priority, + visual_alert: car.CarControl.HUDControl.VisualAlert, + audible_alert: car.CarControl.HUDControl.AudibleAlert, duration_sound: float, duration_hud_alert: float, duration_text: float, alert_rate: float = 0., creation_delay: float = 0.): - self.alert_type = "" self.alert_text_1 = alert_text_1 self.alert_text_2 = alert_text_2 self.alert_status = alert_status @@ -125,24 +124,19 @@ class Alert: self.duration_hud_alert = duration_hud_alert self.duration_text = duration_text - self.start_time = 0. self.alert_rate = alert_rate self.creation_delay = creation_delay - # typecheck that enums are valid on startup - tst = car.CarControl.new_message() - tst.hudControl.visualAlert = self.visual_alert + self.start_time = 0. + self.alert_type = "" + self.event_type = None def __str__(self) -> str: - return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str( - self.visual_alert) + " " + str(self.audible_alert) + return f"{self.alert_text_1}/{self.alert_text_2} {self.alert_priority} {self.visual_alert} {self.audible_alert}" def __gt__(self, alert2) -> bool: return self.alert_priority > alert2.alert_priority - def __eq__(self, alert2) -> bool: - return self.alert_priority == alert2.alert_priority - class NoEntryAlert(Alert): def __init__(self, alert_text_2, audible_alert=AudibleAlert.chimeError, visual_alert=VisualAlert.none, duration_hud_alert=2.): @@ -173,6 +167,11 @@ class EngagementAlert(Alert): Priority.MID, VisualAlert.none, audible_alert, .2, 0., 0.), +class NormalPermanentAlert(Alert): + def __init__(self, alert_text_1, alert_text_2): + super().__init__(alert_text_1, alert_text_2, + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), # ********** alert callback functions ********** @@ -263,11 +262,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.whitePandaUnsupported: { ET.PERMANENT: Alert( - "White Panda Is No Longer Supported", + "White Panda No Longer Supported", "Upgrade to comma two or black panda", AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - ET.NO_ENTRY: NoEntryAlert("White panda is no longer supported"), + ET.NO_ENTRY: NoEntryAlert("Unsupported Hardware"), }, EventName.invalidLkasSetting: { @@ -358,13 +357,13 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo "KEEP EYES ON ROAD: Driver Distracted", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1), }, EventName.promptDriverDistracted: { ET.WARNING: Alert( "KEEP EYES ON ROAD", - "Driver Appears Distracted", + "Driver Distracted", AlertStatus.userPrompt, AlertSize.mid, Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2Repeat, .1, .1, .1), }, @@ -372,7 +371,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.driverDistracted: { ET.WARNING: Alert( "DISENGAGE IMMEDIATELY", - "Driver Was Distracted", + "Driver Distracted", AlertStatus.critical, AlertSize.full, Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), }, @@ -388,7 +387,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.promptDriverUnresponsive: { ET.WARNING: Alert( "TOUCH STEERING WHEEL", - "Driver Is Unresponsive", + "Driver Unresponsive", AlertStatus.userPrompt, AlertSize.mid, Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2Repeat, .1, .1, .1), }, @@ -396,7 +395,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.driverUnresponsive: { ET.WARNING: Alert( "DISENGAGE IMMEDIATELY", - "Driver Was Unresponsive", + "Driver Unresponsive", AlertStatus.critical, AlertSize.full, Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), }, @@ -404,7 +403,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.driverMonitorLowAcc: { ET.WARNING: Alert( "CHECK DRIVER FACE VISIBILITY", - "Driver Monitor Model Output Uncertain", + "Driver Monitoring Uncertain", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 0., 1.5), }, @@ -466,7 +465,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo "TAKE CONTROL", "Turn Exceeds Steering Limit", AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.), + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 1., 1.), + }, + + EventName.fanMalfunction: { + ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Contact Support"), }, # ********** events that affect controls state transitions ********** @@ -523,15 +526,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo duration_hud_alert=0.), }, - EventName.focusRecoverActive: { - ET.WARNING: Alert( - "TAKE CONTROL", - "Attempting Refocus: Camera Focus Invalid", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3., creation_delay=3.1), - }, - EventName.outOfSpace: { + ET.PERMANENT: Alert( + "Out of Storage", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), ET.NO_ENTRY: NoEntryAlert("Out of Storage Space", duration_hud_alert=0.), }, @@ -540,15 +540,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo ET.NO_ENTRY: NoEntryAlert("Speed Too Low"), }, - EventName.neosUpdateRequired: { - ET.PERMANENT: Alert( - "NEOS Update Required", - "Please Wait for Update", - AlertStatus.normal, AlertSize.mid, - Priority.HIGHEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - ET.NO_ENTRY: NoEntryAlert("NEOS Update Required"), - }, - EventName.sensorDataInvalid: { ET.PERMANENT: Alert( "No Data from Device Sensors", @@ -563,11 +554,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo }, EventName.soundsUnavailable: { - ET.PERMANENT: Alert( - "Speaker not found", - "Reboot your Device", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + ET.PERMANENT: NormalPermanentAlert("Speaker not found", "Reboot your Device"), ET.NO_ENTRY: NoEntryAlert("Speaker not found"), }, @@ -576,8 +563,13 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo }, EventName.overheat: { + ET.PERMANENT: Alert( + "System Overheated", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), ET.SOFT_DISABLE: SoftDisableAlert("System Overheated"), - ET.NO_ENTRY: NoEntryAlert("System overheated"), + ET.NO_ENTRY: NoEntryAlert("System Overheated"), }, EventName.wrongGear: { @@ -586,29 +578,25 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo }, EventName.calibrationInvalid: { - ET.PERMANENT: Alert( - "Calibration Invalid", - "Reposition Device and Recalibrate", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Reposition Device & Recalibrate"), - ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Reposition Device & Recalibrate"), + ET.PERMANENT: NormalPermanentAlert("Calibration Invalid", "Remount Device and Recalibrate"), + ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Remount Device & Recalibrate"), + ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"), }, EventName.calibrationIncomplete: { - ET.SOFT_DISABLE: SoftDisableAlert("Calibration in Progress"), ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: SoftDisableAlert("Calibration in Progress"), ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), }, EventName.doorOpen: { ET.SOFT_DISABLE: SoftDisableAlert("Door Open"), - ET.NO_ENTRY: NoEntryAlert("Door open"), + ET.NO_ENTRY: NoEntryAlert("Door Open"), }, EventName.seatbeltNotLatched: { ET.SOFT_DISABLE: SoftDisableAlert("Seatbelt Unlatched"), - ET.NO_ENTRY: NoEntryAlert("Seatbelt unlatched"), + ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"), }, EventName.espDisabled: { @@ -649,8 +637,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo }, EventName.posenetInvalid: { - ET.SOFT_DISABLE: SoftDisableAlert("Vision Model Output Uncertain"), - ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"), + ET.SOFT_DISABLE: SoftDisableAlert("Model Output Uncertain"), + ET.NO_ENTRY: NoEntryAlert("Model Output Uncertain"), }, EventName.deviceFalling: { @@ -660,11 +648,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.lowMemory: { ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"), - ET.PERMANENT: Alert( - "RAM Critically Low", - "Reboot your Device", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"), ET.NO_ENTRY : NoEntryAlert("Low Memory: Reboot Your Device", audible_alert=AudibleAlert.chimeDisengage), }, @@ -708,11 +692,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), }, - EventName.gasUnavailable: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Gas Fault: Restart the Car"), - ET.NO_ENTRY: NoEntryAlert("Gas Error: Restart the Car"), - }, - EventName.reverseGear: { ET.PERMANENT: Alert( "Reverse\nGear", @@ -734,11 +713,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.relayMalfunction: { ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Malfunction"), - ET.PERMANENT: Alert( - "Harness Malfunction", - "Please Check Hardware", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + ET.PERMANENT: NormalPermanentAlert("Harness Malfunction", "Check Hardware"), ET.NO_ENTRY: NoEntryAlert("Harness Malfunction"), }, @@ -764,7 +739,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo "Speed Too High", "Slow down to resume operation", AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarning2Repeat, 2.2, 3., 4.), + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.none, 2.2, 3., 4.), ET.NO_ENTRY: Alert( "Speed Too High", "Slow down to engage", @@ -772,13 +747,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), }, + # TODO: this is unclear, update check only happens offroad EventName.internetConnectivityNeeded: { - ET.PERMANENT: Alert( - "Please connect to Internet", - "An Update Check Is Required to Engage", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - ET.NO_ENTRY: NoEntryAlert("Please Connect to Internet", + ET.PERMANENT: NormalPermanentAlert("Connect to Internet", "An Update Check Is Required to Engage"), + ET.NO_ENTRY: NoEntryAlert("Connect to Internet", audible_alert=AudibleAlert.chimeDisengage), }, diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index b7e53b9cc..e582f54f5 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -1,4 +1,4 @@ -import numpy as np +import math from collections import defaultdict from common.numpy_fast import interp @@ -32,16 +32,16 @@ class FCWChecker(): # then limit ARel so that v_lead will get to zero in no sooner than t_decel. # This helps underweighting ARel when v_lead is close to zero. t_decel = 2. - a_rel = np.minimum(a_rel, v_lead / t_decel) + a_rel = min(a_rel, v_lead / t_decel) # delta of the quadratic equation to solve for ttc delta = v_rel**2 + 2 * x_lead * a_rel # assign an arbitrary high ttc value if there is no solution to ttc - if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1): + if delta < 0.1 or (math.sqrt(delta) + v_rel < 0.1): ttc = max_ttc else: - ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc) + ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc) return ttc def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 8371d5ad3..51872f6d0 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -13,24 +13,24 @@ class LatControlINDI(): def __init__(self, CP): self.angle_steers_des = 0. - A = np.matrix([[1.0, DT_CTRL, 0.0], - [0.0, 1.0, DT_CTRL], - [0.0, 0.0, 1.0]]) - C = np.matrix([[1.0, 0.0, 0.0], - [0.0, 1.0, 0.0]]) + A = np.array([[1.0, DT_CTRL, 0.0], + [0.0, 1.0, DT_CTRL], + [0.0, 0.0, 1.0]]) + C = np.array([[1.0, 0.0, 0.0], + [0.0, 1.0, 0.0]]) # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]]) # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]]) # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) # K = np.transpose(K) - K = np.matrix([[7.30262179e-01, 2.07003658e-04], - [7.29394177e+00, 1.39159419e-02], - [1.71022442e+01, 3.38495381e-02]]) + K = np.array([[7.30262179e-01, 2.07003658e-04], + [7.29394177e+00, 1.39159419e-02], + [1.71022442e+01, 3.38495381e-02]]) self.K = K self.A_K = A - np.dot(K, C) - self.x = np.matrix([[0.], [0.], [0.]]) + self.x = np.array([[0.], [0.], [0.]]) self.enforce_rate_limit = CP.carName == "toyota" @@ -64,7 +64,7 @@ class LatControlINDI(): def update(self, active, CS, CP, path_plan): # Update Kalman filter - y = np.matrix([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]]) + y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) indi_log = log.ControlsState.LateralINDIState.new_message() diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 375dea473..594147e11 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -106,7 +106,7 @@ class PathPlanner(): elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right - if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): + if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 818739967..3861bf574 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,3 +1,4 @@ +from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D from selfdrive.config import RADAR_TO_CAMERA @@ -54,9 +55,6 @@ class Track(): self.aLeadK = aLeadK self.aLeadTau = aLeadTau -def mean(l): - return sum(l) / len(l) - class Cluster(): def __init__(self): diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 44dd1ec62..1f19266f1 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from common.params import Params -from common.realtime import Priority, config_rt_process +from common.realtime import Priority, config_realtime_process from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.planner import Planner from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -11,7 +11,7 @@ import cereal.messaging as messaging def plannerd_thread(sm=None, pm=None): - config_rt_process(2, Priority.CTRL_LOW) + config_realtime_process(2, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index c2643d76e..6cb5f36a0 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -7,7 +7,7 @@ import cereal.messaging as messaging from cereal import car from common.numpy_fast import interp from common.params import Params -from common.realtime import Ratekeeper, Priority, set_realtime_priority +from common.realtime import Ratekeeper, Priority, config_realtime_process from selfdrive.config import RADAR_TO_CAMERA from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid from selfdrive.controls.lib.radar_helpers import Cluster, Track @@ -91,19 +91,16 @@ class RadarD(): self.tracks = defaultdict(dict) self.kalman_params = KalmanParams(radar_ts) - self.active = 0 - # v_ego self.v_ego = 0. self.v_ego_hist = deque([0], maxlen=delay+1) self.ready = False - def update(self, frame, sm, rr, enable_lead): - self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) + def update(self, sm, rr, enable_lead): + self.current_time = 1e-9*max(sm.logMonoTime.values()) if sm.updated['controlsState']: - self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo self.v_ego_hist.append(self.v_ego) if sm.updated['model']: @@ -160,21 +157,22 @@ class RadarD(): # *** publish radarState *** dat = messaging.new_message('radarState') - dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model']) - dat.radarState.mdMonoTime = sm.logMonoTime['model'] - dat.radarState.canMonoTimes = list(rr.canMonoTimes) - dat.radarState.radarErrors = list(rr.errors) - dat.radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] + dat.valid = sm.all_alive_and_valid() + radarState = dat.radarState + radarState.mdMonoTime = sm.logMonoTime['model'] + radarState.canMonoTimes = list(rr.canMonoTimes) + radarState.radarErrors = list(rr.errors) + radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] if enable_lead: - dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) - dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) return dat # fuses camera and radar data for best lead detection def radard_thread(sm=None, pm=None, can_sock=None): - set_realtime_priority(Priority.CTRL_LOW) + config_realtime_process(2, Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") @@ -185,13 +183,11 @@ def radard_thread(sm=None, pm=None, can_sock=None): cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface + # *** setup messaging if can_sock is None: can_sock = messaging.sub_sock('can') - if sm is None: - sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) - - # *** publish radarState and liveTracks + sm = messaging.SubMaster(['model', 'controlsState']) if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) @@ -212,7 +208,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): sm.update(0) - dat = RD.update(rk.frame, sm, rr, enable_lead) + dat = RD.update(sm, rr, enable_lead) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index 9b5681e89..76f14afa0 100755 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -36,6 +36,8 @@ monitored_proc_names = [ 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'dmonitoringd', 'dmonitoringmodeld', 'camerad', 'sensord', 'updated', 'gpsd', 'athena', 'locationd', 'paramsd', + 'ai.comma.plus.offroad', + # android procs 'SurfaceFlinger', 'sensors.qcom' ] diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 6e1029620..8322e9108 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -49,7 +49,7 @@ if __name__ == "__main__": if sm.updated['thermal']: t = sm['thermal'] - last_temp = np.mean([t.cpu0, t.cpu1, t.cpu2, t.cpu3]) / 10. + last_temp = np.mean(t.cpu) last_mem = t.memUsedPercent if sm.updated['procLog']: diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index 6ca5436e9..f0ffabdf7 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -37,7 +37,7 @@ if __name__ == "__main__": for route in tqdm(routes): route = route.rstrip() dongle_id, time = route.split('|') - qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2" + qlog_path = f"cd:/{dongle_id}/{time}/1/qlog.bz2" if dongle_id in dongles: continue diff --git a/selfdrive/debug/toyota_eps_factor.py b/selfdrive/debug/toyota_eps_factor.py new file mode 100755 index 000000000..b948bcc1f --- /dev/null +++ b/selfdrive/debug/toyota_eps_factor.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python3 +import sys +import numpy as np +import matplotlib.pyplot as plt +from sklearn import linear_model # pylint: disable=import-error + +from tools.lib.route import Route +from tools.lib.logreader import MultiLogIterator + + +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): + + engaged = False + torque_cmd, eps_torque = None, None + cmds, eps = [], [] + + for msg in lr: + if msg.which() != 'can': + continue + + for m in msg.can: + if m.address == 0x2e4 and m.src == 128: + engaged = bool(m.dat[0] & 1) + 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) + + if engaged and torque_cmd is not None and eps_torque is not None: + cmds.append(torque_cmd) + eps.append(eps_torque) + else: + if len(cmds) > MIN_SAMPLES: + break + cmds, eps = [], [] + + if len(cmds) < MIN_SAMPLES: + raise Exception("too few samples found in route") + + lm = linear_model.LinearRegression(fit_intercept=False) + lm.fit(np.array(cmds).reshape(-1, 1), eps) + scale_factor = 1./lm.coef_[0] + + if plot: + 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) + n = get_eps_factor(lr, plot="--plot" in sys.argv) + print("EPS torque factor: ", n) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index be7817684..b6acc1209 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -2,6 +2,7 @@ import numpy as np import sympy as sp import cereal.messaging as messaging +from cereal import log import common.transformations.coordinates as coord from common.transformations.orientation import ecef_euler_from_ned, \ euler_from_quat, \ @@ -19,6 +20,8 @@ from selfdrive.swaglog import cloudlog from sympy.utilities.lambdify import lambdify from rednose.helpers.sympy_helpers import euler_rotate +SensorSource = log.SensorEventData.SensorSource + VISION_DECIMATION = 2 SENSOR_DECIMATION = 10 @@ -235,6 +238,10 @@ class Localizer(): def handle_sensors(self, current_time, log): # TODO does not yet account for double sensor readings in the log for sensor_reading in log: + # TODO: handle messages from two IMUs at the same time + if sensor_reading.source == SensorSource.lsm6ds3: + continue + # Gyro Uncalibrated if sensor_reading.sensor == 5 and sensor_reading.type == 16: self.gyro_counter += 1 diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 1d1d35331..c0afe1697 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -77,6 +77,8 @@ def main(sm=None, pm=None): CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) cloudlog.info("paramsd got CarParams") + min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio + params = params_reader.get("LiveParameters") # Check if car model matches @@ -86,6 +88,12 @@ def main(sm=None, pm=None): cloudlog.info("Parameter learner found parameters for wrong car.") params = None + if (params is not None) and not all(( + abs(params['angleOffsetAverage']) < 10.0, + min_sr <= params['steerRatio'] <= max_sr)): + cloudlog.info(f"Invalid starting values found {params}") + params = None + if params is None: params = { 'carFingerprint': CP.carFingerprint, @@ -100,7 +108,6 @@ def main(sm=None, pm=None): params['stiffnessFactor'] = 1.0 learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage'])) - min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio while True: sm.update() diff --git a/selfdrive/logcatd/SConscript b/selfdrive/logcatd/SConscript index 7f87cbadd..2646537ad 100644 --- a/selfdrive/logcatd/SConscript +++ b/selfdrive/logcatd/SConscript @@ -1,6 +1,6 @@ Import('env', 'cereal', 'messaging', 'arch') if arch == "aarch64": - env.Program('logcatd', 'logcatd_android.cc', LIBS=[cereal, messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj']) + env.Program('logcatd', 'logcatd_android.cc', LIBS=[cereal, messaging, 'cutils', 'zmq', 'capnp', 'kj']) else: env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, 'zmq', 'capnp', 'kj', 'systemd', 'json11']) diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 2e5dae555..5d59e929e 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -1,7 +1,7 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc') src = ['loggerd.cc', 'logger.cc'] -libs = ['zmq', 'czmq', 'capnp', 'kj', 'z', +libs = ['zmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', 'yuv', 'bz2', common, cereal, messaging, visionipc] diff --git a/selfdrive/loggerd/encoder.c b/selfdrive/loggerd/encoder.c index 4939a4853..714293c78 100644 --- a/selfdrive/loggerd/encoder.c +++ b/selfdrive/loggerd/encoder.c @@ -5,8 +5,8 @@ #include #include #include - -#include +#include +#include #include @@ -397,15 +397,6 @@ static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { memcpy(s->codec_config, buf_data, out_buf->nFilledLen); } - if (s->stream_sock_raw) { - //uint64_t current_time = nanos_since_boot(); - //uint64_t diff = current_time - out_buf->nTimeStamp*1000LL; - //double msdiff = (double) diff / 1000000.0; - // printf("encoded latency to tsEof: %f\n", msdiff); - zmq_send(s->stream_sock_raw, &out_buf->nTimeStamp, sizeof(out_buf->nTimeStamp), ZMQ_SNDMORE); - zmq_send(s->stream_sock_raw, buf_data, out_buf->nFilledLen, 0); - } - if (s->of) { //printf("write %d flags 0x%x\n", out_buf->nFilledLen, out_buf->nFlags); fwrite(buf_data, out_buf->nFilledLen, 1, s->of); @@ -564,6 +555,9 @@ 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); diff --git a/selfdrive/loggerd/encoder.h b/selfdrive/loggerd/encoder.h index 18054d20d..abd9d18c5 100644 --- a/selfdrive/loggerd/encoder.h +++ b/selfdrive/loggerd/encoder.h @@ -56,15 +56,11 @@ typedef struct EncoderState { Queue free_in; Queue done_out; - void *stream_sock_raw; - AVFormatContext *ofmt_ctx; AVCodecContext *codec_ctx; AVStream *out_stream; bool remuxing; - void *zmq_ctx; - bool downscale; uint8_t *y_ptr2, *u_ptr2, *v_ptr2; } EncoderState; diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index c7487677a..3a9e64e46 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -19,10 +19,10 @@ #include #include #include +#include #include #include -#include #ifdef QCOM #include #endif @@ -34,7 +34,7 @@ #include "common/visionipc.h" #include "common/utilpp.h" #include "common/util.h" - +#include "camerad/cameras/camera_common.h" #include "logger.h" #include "messaging.hpp" #include "services.h" @@ -44,28 +44,73 @@ #define DISABLE_ENCODER #endif - #ifndef DISABLE_ENCODER #include "encoder.h" #include "raw_logger.h" #endif -#include "cereal/gen/cpp/log.capnp.h" - -#define CAM_IDX_FCAM 0 -#define CAM_IDX_DCAM 1 -#define CAM_IDX_ECAM 2 - -#define CAMERA_FPS 20 -#define SEGMENT_LENGTH 60 - #define MAIN_BITRATE 5000000 +#define QCAM_BITRATE 128000 +#define MAIN_FPS 20 #ifndef QCOM2 +#define MAX_CAM_IDX LOG_CAMERA_ID_DCAMERA #define DCAM_BITRATE 2500000 #else +#define MAX_CAM_IDX LOG_CAMERA_ID_ECAMERA #define DCAM_BITRATE MAIN_BITRATE #endif +#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead + +LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { + [LOG_CAMERA_ID_FCAMERA] = { + .stream_type = VISION_STREAM_YUV, + .filename = "fcamera.hevc", + .frame_packet_name = "frame", + .encode_idx_name = "encodeIdx", + .fps = MAIN_FPS, + .bitrate = MAIN_BITRATE, + .is_h265 = true, + .downscale = false, + .has_qcamera = true + }, + [LOG_CAMERA_ID_DCAMERA] = { + .stream_type = VISION_STREAM_YUV_FRONT, + .filename = "dcamera.hevc", + .frame_packet_name = "frontFrame", + .encode_idx_name = "frontEncodeIdx", + .fps = MAIN_FPS, // on EONs, more compressed this way + .bitrate = DCAM_BITRATE, + .is_h265 = true, + .downscale = false, + .has_qcamera = false + }, + [LOG_CAMERA_ID_ECAMERA] = { + .stream_type = VISION_STREAM_YUV_WIDE, + .filename = "ecamera.hevc", + .frame_packet_name = "wideFrame", + .encode_idx_name = "wideEncodeIdx", + .fps = MAIN_FPS, + .bitrate = MAIN_BITRATE, + .is_h265 = true, + .downscale = false, + .has_qcamera = false + }, + [LOG_CAMERA_ID_QCAMERA] = { + .filename = "qcamera.ts", + .fps = MAIN_FPS, + .bitrate = QCAM_BITRATE, + .is_h265 = false, + .downscale = true, +#ifndef QCOM2 + .frame_width = 480, .frame_height = 360 +#else + .frame_width = 526, .frame_height = 330 // keep pixel count the same? +#endif + }, +}; +#define SEGMENT_LENGTH 60 + #define LOG_ROOT "/data/media/0/realdata" #define RAW_CLIP_LENGTH 100 // 5 seconds at 20fps @@ -86,44 +131,94 @@ volatile sig_atomic_t do_exit = 0; static void set_do_exit(int sig) { do_exit = 1; } + +class RotateState { +public: + SubSocket* fpkt_sock; + uint32_t stream_frame_id, log_frame_id, last_rotate_frame_id; + bool enabled, should_rotate, initialized; + + RotateState() : fpkt_sock(nullptr), stream_frame_id(0), log_frame_id(0), + last_rotate_frame_id(UINT32_MAX), enabled(false), should_rotate(false), initialized(false) {}; + + void waitLogThread() { + std::unique_lock lk(fid_lock); + while (stream_frame_id > log_frame_id //if the log camera is older, wait for it to catch up. + && (stream_frame_id - log_frame_id) < 8 // but if its too old then there probably was a discontinuity (visiond restarted) + && !do_exit) { + cv.wait(lk); + } + } + + void cancelWait() { cv.notify_one(); } + + void setStreamFrameId(uint32_t frame_id) { + fid_lock.lock(); + stream_frame_id = frame_id; + fid_lock.unlock(); + cv.notify_one(); + } + + void setLogFrameId(uint32_t frame_id) { + fid_lock.lock(); + log_frame_id = frame_id; + fid_lock.unlock(); + cv.notify_one(); + } + + void rotate() { + if (!enabled) { return; } + std::unique_lock lk(fid_lock); + should_rotate = true; + last_rotate_frame_id = stream_frame_id; + } + + void finish_rotate() { + std::unique_lock lk(fid_lock); + should_rotate = false; + } + +private: + std::mutex fid_lock; + std::condition_variable cv; +}; + struct LoggerdState { Context *ctx; LoggerState logger; - - std::mutex lock; - std::condition_variable cv; char segment_path[4096]; - uint32_t last_frame_id; - uint32_t rotate_last_frame_id; int rotate_segment; - int rotate_seq_id; - pthread_mutex_t rotate_lock; int num_encoder; - int should_close; - int finish_close; + std::atomic rotate_seq_id; + std::atomic should_close; + std::atomic finish_close; + + RotateState rotate_state[LOG_CAMERA_ID_MAX-1]; }; LoggerdState s; #ifndef DISABLE_ENCODER -void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { - int err; +void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { - // 0:f, 1:d, 2:e - if (cam_idx == CAM_IDX_DCAM) { - // TODO: add this back - #ifndef QCOM2 - if (!read_db_bool("RecordFront")) return; - LOGW("recording front camera"); - #endif - set_thread_name("FrontCameraEncoder"); - } else if (cam_idx == CAM_IDX_FCAM) { - set_thread_name("RearCameraEncoder"); - } else if (cam_idx == CAM_IDX_ECAM) { - set_thread_name("WideCameraEncoder"); - } else { - LOGE("unexpected camera index provided"); - assert(false); + switch (cam_idx) { + case LOG_CAMERA_ID_DCAMERA: { + LOGW("recording front camera"); + set_thread_name("FrontCameraEncoder"); + break; + } + case LOG_CAMERA_ID_FCAMERA: { + set_thread_name("RearCameraEncoder"); + break; + } + case LOG_CAMERA_ID_ECAMERA: { + set_thread_name("WideCameraEncoder"); + break; + } + default: { + LOGE("unexpected camera index provided"); + assert(false); + } } VisionStream stream; @@ -131,30 +226,23 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { bool encoder_inited = false; EncoderState encoder; EncoderState encoder_alt; - bool has_encoder_alt = false; + bool has_encoder_alt = cameras_logged[cam_idx].has_qcamera; + int encoder_segment = -1; + int cnt = 0; pthread_mutex_lock(&s.rotate_lock); int my_idx = s.num_encoder; s.num_encoder += 1; pthread_mutex_unlock(&s.rotate_lock); - int encoder_segment = -1; - int cnt = 0; - - PubSocket *idx_sock = PubSocket::create(s.ctx, cam_idx == CAM_IDX_DCAM ? "frontEncodeIdx" : (cam_idx == CAM_IDX_ECAM ? "wideEncodeIdx" : "encodeIdx")); + PubSocket *idx_sock = PubSocket::create(s.ctx, cameras_logged[cam_idx].encode_idx_name); assert(idx_sock != NULL); LoggerHandle *lh = NULL; while (!do_exit) { VisionStreamBufs buf_info; - if (cam_idx == CAM_IDX_DCAM) { - err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, false, &buf_info); - } else if (cam_idx == CAM_IDX_FCAM) { - err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info); - } else if (cam_idx == CAM_IDX_ECAM) { - err = visionstream_init(&stream, VISION_STREAM_YUV_WIDE, false, &buf_info); - } + int err = visionstream_init(&stream, cameras_logged[cam_idx].stream_type, false, &buf_info); if (err != 0) { LOGD("visionstream connect fail"); usleep(100000); @@ -163,22 +251,25 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { if (!encoder_inited) { LOGD("encoder init %dx%d", buf_info.width, buf_info.height); - encoder_init(&encoder, cam_idx == CAM_IDX_DCAM ? "dcamera.hevc" : (cam_idx == CAM_IDX_ECAM ? "ecamera.hevc" : "fcamera.hevc"), buf_info.width, buf_info.height, CAMERA_FPS, cam_idx == CAM_IDX_DCAM ? DCAM_BITRATE:MAIN_BITRATE, true, false); + encoder_init(&encoder, cameras_logged[cam_idx].filename, + buf_info.width, + buf_info.height, + cameras_logged[cam_idx].fps, + cameras_logged[cam_idx].bitrate, + cameras_logged[cam_idx].is_h265, + cameras_logged[cam_idx].downscale); - #ifndef QCOM2 - // TODO: fix qcamera on tici - if (cam_idx == CAM_IDX_FCAM) { - encoder_init(&encoder_alt, "qcamera.ts", 480, 360, CAMERA_FPS, 128000, false, true); - has_encoder_alt = true; + if (has_encoder_alt) { + encoder_init(&encoder_alt, cameras_logged[LOG_CAMERA_ID_QCAMERA].filename, + cameras_logged[LOG_CAMERA_ID_QCAMERA].frame_width, + cameras_logged[LOG_CAMERA_ID_QCAMERA].frame_height, + cameras_logged[LOG_CAMERA_ID_QCAMERA].fps, + cameras_logged[LOG_CAMERA_ID_QCAMERA].bitrate, + cameras_logged[LOG_CAMERA_ID_QCAMERA].is_h265, + cameras_logged[LOG_CAMERA_ID_QCAMERA].downscale); } - #endif + encoder_inited = true; - if (is_streaming) { - encoder.zmq_ctx = zmq_ctx_new(); - encoder.stream_sock_raw = zmq_socket(encoder.zmq_ctx, ZMQ_PUB); - assert(encoder.stream_sock_raw); - zmq_bind(encoder.stream_sock_raw, "tcp://*:9002"); - } } // dont log a raw clip in the first minute @@ -187,7 +278,7 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { RawLogger *rawlogger = NULL; if (raw_clips) { - rawlogger = new RawLogger("prcamera", buf_info.width, buf_info.height, CAMERA_FPS); + rawlogger = new RawLogger("prcamera", buf_info.width, buf_info.height, MAIN_FPS); } while (!do_exit) { @@ -203,66 +294,47 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { //double msdiff = (double) diff / 1000000.0; // printf("logger latency to tsEof: %f\n", msdiff); - uint8_t *y = (uint8_t*)buf->addr; - uint8_t *u = y + (buf_info.width*buf_info.height); - uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2); + { // all the rotation stuff + + pthread_mutex_lock(&s.rotate_lock); + pthread_mutex_unlock(&s.rotate_lock); + + // wait if camera pkt id is older than stream + rotate_state->waitLogThread(); - { - // all the rotation stuff - bool should_rotate = false; - std::unique_lock lk(s.lock); - if (cam_idx == CAM_IDX_FCAM) { // TODO: should wait for three cameras on tici? - // wait if log camera is older on back camera - while ( extra.frame_id > s.last_frame_id //if the log camera is older, wait for it to catch up. - && (extra.frame_id-s.last_frame_id) < 8 // but if its too old then there probably was a discontinuity (visiond restarted) - && !do_exit) { - s.cv.wait(lk); - } - should_rotate = extra.frame_id > s.rotate_last_frame_id && encoder_segment < s.rotate_segment && s.rotate_seq_id == my_idx; - } else { - // front camera is best effort - should_rotate = encoder_segment < s.rotate_segment && s.rotate_seq_id == my_idx; - } if (do_exit) break; // rotate the encoder if the logger is on a newer segment - if (should_rotate) { - LOG("rotate encoder to %s", s.segment_path); - + if (rotate_state->should_rotate) { + if (!rotate_state->initialized) { + rotate_state->last_rotate_frame_id = extra.frame_id - 1; + rotate_state->initialized = true; + } + while (s.rotate_seq_id != my_idx && !do_exit) { usleep(1000); } + LOGW("camera %d rotate encoder to %s.", cam_idx, s.segment_path); encoder_rotate(&encoder, s.segment_path, s.rotate_segment); s.rotate_seq_id = (my_idx + 1) % s.num_encoder; - 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); } lh = logger_get_handle(&s.logger); - } - if (encoder.rotating) { pthread_mutex_lock(&s.rotate_lock); s.should_close += 1; pthread_mutex_unlock(&s.rotate_lock); - while(s.should_close > 0 && s.should_close < s.num_encoder) { - // printf("%d waiting for others to reach close, %d/%d \n", my_idx, s.should_close, s.num_encoder); - s.cv.wait(lk); - } + while(s.should_close > 0 && s.should_close < s.num_encoder && !do_exit) { usleep(1000); } pthread_mutex_lock(&s.rotate_lock); - if (s.should_close == s.num_encoder) { - s.should_close = 1 - s.num_encoder; - } else { - s.should_close += 1; - } + s.should_close = s.should_close == s.num_encoder ? 1 - s.num_encoder : s.should_close + 1; + encoder_close(&encoder); encoder_open(&encoder, encoder.next_path); encoder.segment = encoder.next_segment; @@ -276,13 +348,18 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { s.finish_close += 1; pthread_mutex_unlock(&s.rotate_lock); - while(s.finish_close > 0 && s.finish_close < s.num_encoder) { - // printf("%d waiting for others to actually close, %d/%d \n", my_idx, s.finish_close, s.num_encoder); - s.cv.wait(lk); - } + while(s.finish_close > 0 && s.finish_close < s.num_encoder && !do_exit) { usleep(1000); } s.finish_close = 0; + + rotate_state->finish_rotate(); } } + + rotate_state->setStreamFrameId(extra.frame_id); + + uint8_t *y = (uint8_t*)buf->addr; + uint8_t *u = y + (buf_info.width*buf_info.height); + uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2); { // encode hevc int out_segment = -1; @@ -302,11 +379,12 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { MessageBuilder msg; auto eidx = msg.initEvent().initEncodeIdx(); eidx.setFrameId(extra.frame_id); -#ifdef QCOM2 + #ifdef QCOM2 eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C); -#else - eidx.setType(cam_idx == CAM_IDX_DCAM ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); -#endif + #else + eidx.setType(cam_idx == LOG_CAMERA_ID_DCAMERA ? cereal::EncodeIndex::Type::FRONT:cereal::EncodeIndex::Type::FULL_H_E_V_C); + #endif + eidx.setEncodeId(cnt); eidx.setSegmentNum(out_segment); eidx.setSegmentId(out_id); @@ -444,30 +522,31 @@ kj::Array gen_init_data() { if (!clean) { init.setDirty(true); } + Params params = Params(); - std::vector git_commit = read_db_bytes("GitCommit"); + std::vector git_commit = params.read_db_bytes("GitCommit"); if (git_commit.size() > 0) { init.setGitCommit(capnp::Text::Reader(git_commit.data(), git_commit.size())); } - std::vector git_branch = read_db_bytes("GitBranch"); + std::vector git_branch = params.read_db_bytes("GitBranch"); if (git_branch.size() > 0) { init.setGitBranch(capnp::Text::Reader(git_branch.data(), git_branch.size())); } - std::vector git_remote = read_db_bytes("GitRemote"); + std::vector git_remote = params.read_db_bytes("GitRemote"); if (git_remote.size() > 0) { init.setGitRemote(capnp::Text::Reader(git_remote.data(), git_remote.size())); } - init.setPassive(read_db_bool("Passive")); + init.setPassive(params.read_db_bool("Passive")); { // log params - std::map params; - read_db_all(¶ms); - auto lparams = init.initParams().initEntries(params.size()); + std::map params_map; + params.read_db_all(¶ms_map); + auto lparams = init.initParams().initEntries(params_map.size()); int i = 0; - for (auto& kv : params) { + for (auto& kv : params_map) { auto lentry = lparams[i]; lentry.setKey(kv.first); lentry.setValue(kv.second); @@ -514,6 +593,9 @@ static void bootlog() { std::string lastPmsg = util::read_file("/sys/fs/pstore/pmsg-ramoops-0"); boot.setLastPmsg(capnp::Data::Reader((const kj::byte*)lastPmsg.data(), lastPmsg.size())); + std::string launchLog = util::read_file("/tmp/launch_log"); + boot.setLaunchLog(capnp::Text::Reader(launchLog.data(), launchLog.size())); + auto bytes = msg.toBytes(); logger_log(&s.logger, bytes.begin(), bytes.size(), false); } @@ -533,6 +615,10 @@ int main(int argc, char** argv) { if (getenv("LOGGERD_TEST")) { segment_length = atoi(getenv("LOGGERD_SEGMENT_LENGTH")); } + bool record_front = true; +#ifndef QCOM2 + record_front = Params().read_db_bool("RecordFront"); +#endif setpriority(PRIO_PROCESS, 0, -12); @@ -546,7 +632,6 @@ int main(int argc, char** argv) { // subscribe to all services - SubSocket *frame_sock = NULL; std::vector socks; std::map qlog_counter; @@ -558,12 +643,11 @@ int main(int argc, char** argv) { if (it.should_log) { SubSocket * sock = SubSocket::create(s.ctx, name); assert(sock != NULL); - poller->registerSocket(sock); socks.push_back(sock); - if (name == "frame") { - frame_sock = sock; + for (int cid=0;cid<=MAX_CAM_IDX;cid++) { + if (name == cameras_logged[cid].frame_packet_name) { s.rotate_state[cid].fpkt_sock = sock; } } qlog_counter[sock] = (it.decimation == -1) ? -1 : 0; @@ -571,31 +655,12 @@ int main(int argc, char** argv) { } } - { auto words = gen_init_data(); auto bytes = words.asBytes(); logger_init(&s.logger, "rlog", bytes.begin(), bytes.size(), true); } - bool is_streaming = false; - bool is_logging = true; - - if (argc > 1 && strcmp(argv[1], "--stream") == 0) { - is_streaming = true; - } else if (argc > 1 && strcmp(argv[1], "--only-stream") == 0) { - is_streaming = true; - is_logging = false; - } - - if (is_logging) { - err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); - assert(err == 0); - LOGW("logging to %s", s.segment_path); - } - - double start_ts = seconds_since_boot(); - double last_rotate_ts = start_ts; s.rotate_seq_id = 0; s.should_close = 0; s.finish_close = 0; @@ -603,74 +668,120 @@ 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, is_streaming, false, CAM_IDX_FCAM); - + std::thread encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_FCAMERA], false, LOG_CAMERA_ID_FCAMERA); + s.rotate_state[LOG_CAMERA_ID_FCAMERA].enabled = true; // front camera - std::thread front_encoder_thread_handle(encoder_thread, false, false, CAM_IDX_DCAM); - + 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); + s.rotate_state[LOG_CAMERA_ID_DCAMERA].enabled = true; + } #ifdef QCOM2 // wide camera - std::thread wide_encoder_thread_handle(encoder_thread, false, false, CAM_IDX_ECAM); + std::thread wide_encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_ECAMERA], false, LOG_CAMERA_ID_ECAMERA); + s.rotate_state[LOG_CAMERA_ID_ECAMERA].enabled = true; #endif #endif uint64_t msg_count = 0; uint64_t bytes_count = 0; + kj::Array buf = kj::heapArray(1024); + double start_ts = seconds_since_boot(); + double last_rotate_tms = millis_since_boot(); + double last_camera_seen_tms = millis_since_boot(); while (!do_exit) { - for (auto sock : poller->poll(100 * 1000)) { + for (auto sock : poller->poll(100 * 1000)) { + Message * last_msg = nullptr; while (true) { Message * msg = sock->receive(true); if (msg == NULL){ break; } + delete last_msg; + last_msg = msg; - uint8_t* data = (uint8_t*)msg->getData(); - size_t len = msg->getSize(); - - if (sock == frame_sock) { - // track camera frames to sync to encoder - auto amsg = kj::heapArray((len / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), data, len); - - capnp::FlatArrayMessageReader cmsg(amsg); - cereal::Event::Reader event = cmsg.getRoot(); - if (event.isFrame()) { - std::unique_lock lk(s.lock); - s.last_frame_id = event.getFrame().getFrameId(); - lk.unlock(); - s.cv.notify_all(); - } - } - - logger_log(&s.logger, data, len, qlog_counter[sock] == 0); - delete msg; + logger_log(&s.logger, (uint8_t*)msg->getData(), msg->getSize(), qlog_counter[sock] == 0); if (qlog_counter[sock] != -1) { //printf("%p: %d/%d\n", socks[i], qlog_counter[socks[i]], qlog_freqs[socks[i]]); qlog_counter[sock]++; qlog_counter[sock] %= qlog_freqs[sock]; } - - bytes_count += len; + bytes_count += msg->getSize(); msg_count++; } + + if (last_msg) { + int fpkt_id = -1; + for (int cid=0;cid<=MAX_CAM_IDX;cid++) { + if (sock == s.rotate_state[cid].fpkt_sock) {fpkt_id=cid; break;} + } + if (fpkt_id>=0) { + // track camera frames to sync to encoder + // only process last frame + const uint8_t* data = (uint8_t*)last_msg->getData(); + const size_t len = last_msg->getSize(); + const size_t size = len / sizeof(capnp::word) + 1; + if (buf.size() < size) { + buf = kj::heapArray(size); + } + memcpy(buf.begin(), data, len); + + capnp::FlatArrayMessageReader cmsg(buf); + cereal::Event::Reader event = cmsg.getRoot(); + + if (fpkt_id == LOG_CAMERA_ID_FCAMERA) { + s.rotate_state[fpkt_id].setLogFrameId(event.getFrame().getFrameId()); + } else if (fpkt_id == LOG_CAMERA_ID_DCAMERA) { + s.rotate_state[fpkt_id].setLogFrameId(event.getFrontFrame().getFrameId()); + } else if (fpkt_id == LOG_CAMERA_ID_ECAMERA) { + s.rotate_state[fpkt_id].setLogFrameId(event.getWideFrame().getFrameId()); + } + last_camera_seen_tms = millis_since_boot(); + } + delete last_msg; + } } double ts = seconds_since_boot(); - if (ts - last_rotate_ts > segment_length) { - // rotate the log + double tms = millis_since_boot(); - last_rotate_ts += segment_length; + bool new_segment = false; - std::lock_guard guard(s.lock); - s.rotate_last_frame_id = s.last_frame_id; - - if (is_logging) { - err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); - assert(err == 0); - LOGW("rotated to %s", s.segment_path); + if (s.logger.part > -1) { + new_segment = true; + if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE) { + for (int cid=0;cid<=MAX_CAM_IDX;cid++) { + // this *should* be redundant on tici since all camera frames are synced + new_segment &= (((s.rotate_state[cid].stream_frame_id >= s.rotate_state[cid].last_rotate_frame_id + segment_length * MAIN_FPS) && + (!s.rotate_state[cid].should_rotate) && (s.rotate_state[cid].initialized)) || + (!s.rotate_state[cid].enabled)); +#ifndef QCOM2 + break; // only look at fcamera frame id if not QCOM2 +#endif + } + } else { + new_segment &= tms - last_rotate_tms > segment_length * 1000; + if (new_segment) { LOGW("no camera packet seen. auto rotated"); } } + } else if (s.logger.part == -1) { + // always starts first segment immediately + new_segment = true; + } + + if (new_segment) { + pthread_mutex_lock(&s.rotate_lock); + last_rotate_tms = millis_since_boot(); + + err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); + assert(err == 0); + if (s.logger.part == 0) { LOGW("logging to %s", s.segment_path); } + LOGW("rotated to %s", s.segment_path); + + // rotate the encoders + for (int cid=0;cid<=MAX_CAM_IDX;cid++) { s.rotate_state[cid].rotate(); } + pthread_mutex_unlock(&s.rotate_lock); } if ((msg_count%1000) == 0) { @@ -679,14 +790,15 @@ int main(int argc, char** argv) { } LOGW("joining threads"); - s.cv.notify_all(); - + for (int cid=0;cid<=MAX_CAM_IDX;cid++) { s.rotate_state[cid].cancelWait(); } #ifndef DISABLE_ENCODER - #ifdef QCOM2 +#ifdef QCOM2 wide_encoder_thread_handle.join(); - #endif - front_encoder_thread_handle.join(); +#endif + if (record_front) { + front_encoder_thread_handle.join(); + } encoder_thread_handle.join(); LOGW("encoder joined"); #endif diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 08c1f323e..456712f2b 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -4,14 +4,11 @@ import inspect import json import os import random -import re -import subprocess +import requests import threading import time import traceback -import requests - from cereal import log from common.hardware import HARDWARE from common.api import Api @@ -74,16 +71,6 @@ def clear_locks(root): def is_on_wifi(): return HARDWARE.get_network_type() == NetworkType.wifi -def is_on_hotspot(): - try: - result = subprocess.check_output(["ifconfig", "wlan0"], stderr=subprocess.STDOUT, encoding='utf8') - result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0] - return (result.startswith('192.168.43.') or # android - result.startswith('172.20.10.') or # ios - result.startswith('10.0.2.')) # toyota entune - except Exception: - return False - class Uploader(): def __init__(self, dongle_id, root): self.dongle_id = dongle_id @@ -238,25 +225,22 @@ def uploader_fn(exit_event): backoff = 0.1 counter = 0 - should_upload = False + on_wifi = False while not exit_event.is_set(): offroad = params.get("IsOffroad") == b'1' allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") and offroad - check_network = (counter % 12 == 0 if offroad else True) - if check_network: - on_hotspot = is_on_hotspot() + if offroad and counter % 12 == 0: on_wifi = is_on_wifi() - should_upload = on_wifi and not on_hotspot - - d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload) counter += 1 + + d = uploader.next_file_to_upload(with_raw=allow_raw_upload and on_wifi and offroad) if d is None: # Nothing to upload time.sleep(60 if offroad else 5) continue key, fn = d - cloudlog.event("uploader_netcheck", is_on_hotspot=on_hotspot, is_on_wifi=on_wifi) + cloudlog.event("uploader_netcheck", is_on_wifi=on_wifi) cloudlog.info("to upload %r", d) success = uploader.upload(key, fn) if success: diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 898297d64..879d9aa32 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -13,13 +13,13 @@ from typing import Dict, List from selfdrive.swaglog import cloudlog, add_logentries_handler -from common.basedir import BASEDIR, PARAMS +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")) os.environ['BASEDIR'] = BASEDIR -TOTAL_SCONS_NODES = 1005 +TOTAL_SCONS_NODES = 1040 prebuilt = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) # Create folders needed for msgq @@ -208,9 +208,6 @@ interrupt_processes: List[str] = [] # processes to end with SIGKILL instead of SIGTERM kill_processes = ['sensord'] -# processes to end if thermal conditions exceed Green parameters -green_temp_processes = ['uploader'] - persistent_processes = [ 'thermald', 'logmessaged', @@ -221,13 +218,10 @@ persistent_processes = [ if not PC: persistent_processes += [ + 'updated', 'logcatd', 'tombstoned', - ] - -if ANDROID: - persistent_processes += [ - 'updated', + 'sensord', ] car_started_processes = [ @@ -258,7 +252,6 @@ if WEBCAM: if not PC: car_started_processes += [ 'ubloxd', - 'sensord', 'dmonitoringd', 'dmonitoringmodeld', ] @@ -485,16 +478,6 @@ def manager_thread(): while 1: msg = messaging.recv_sock(thermal_sock, wait=True) - # heavyweight batch processes are gated on favorable thermal conditions - if msg.thermal.thermalStatus >= ThermalStatus.yellow: - for p in green_temp_processes: - if p in persistent_processes: - kill_managed_process(p) - else: - for p in green_temp_processes: - if p in persistent_processes: - start_managed_process(p) - if msg.thermal.freeSpace < 0.05: logger_dead = True @@ -553,8 +536,6 @@ def uninstall(): HARDWARE.reboot(reason="recovery") def main(): - os.environ['PARAMS_PATH'] = PARAMS - if ANDROID: # the flippening! os.system('LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1') diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld index e68c13103..e84bbbf37 100755 --- a/selfdrive/modeld/dmonitoringmodeld +++ b/selfdrive/modeld/dmonitoringmodeld @@ -3,11 +3,10 @@ if [ -d /system ]; then if [ -f /TICI ]; then # QCOM2 export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/phonelibs/snpe/larch64:$LD_LIBRARY_PATH" - export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/larch64/" else # QCOM export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH" - export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/" fi + export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/dsp/" else # PC export LD_LIBRARY_PATH="$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:/openpilot/phonelibs/snpe/x86_64:$HOME/openpilot/phonelibs/snpe/x86_64:$LD_LIBRARY_PATH" diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 0f1c22c34..c8c637b3f 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -1,6 +1,7 @@ #include #include #include +#include #include #include "common/visionbuf.h" @@ -43,20 +44,21 @@ void* live_thread(void *arg) { 910.0, 0.0, 582.0, 0.0, 910.0, 437.0, 0.0, 0.0, 1.0; + float db_s = 0.5; // debayering does a 2x downscale #else Eigen::Matrix eon_intrinsics; eon_intrinsics << 2648.0, 0.0, 1928.0/2, 0.0, 2648.0, 1208.0/2, 0.0, 0.0, 1.0; + float db_s = 1.0; #endif - // debayering does a 2x downscale mat3 yuv_transform = transform_scale_buffer((mat3){{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, - }}, 0.5); + }}, db_s); while (!do_exit) { if (sm.update(10) > 0){ @@ -151,8 +153,7 @@ int main(int argc, char **argv) { float frames_dropped = 0; // one frame in memory - cl_mem yuv_cl; - VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl); + VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context); uint32_t frame_id = 0, last_vipc_frame_id = 0; double last = 0; @@ -190,7 +191,7 @@ int main(int argc, char **argv) { memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len); ModelDataRaw model_buf = - model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height, + model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height, model_transform, NULL, vec_desire); mt2 = millis_since_boot(); diff --git a/selfdrive/modeld/models/commonmodel.c b/selfdrive/modeld/models/commonmodel.c index 0bdc61bbe..c156ad39d 100644 --- a/selfdrive/modeld/models/commonmodel.c +++ b/selfdrive/modeld/models/commonmodel.c @@ -1,6 +1,7 @@ +#include +#include #include "commonmodel.h" -#include #include "common/mat.h" #include "common/timing.h" diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 14f45dd90..6be44c926 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -28,7 +28,7 @@ void dmonitoring_init(DMonitoringModelState* s) { int runtime = USE_DSP_RUNTIME; #endif s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, runtime); - s->is_rhd = read_db_bool("IsRHD"); + s->is_rhd = Params().read_db_bool("IsRHD"); } template diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 14c661616..9303245d7 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -54,7 +54,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t s->traffic_convention = std::make_unique(TRAFFIC_CONVENTION_LEN); s->m->addTrafficConvention(s->traffic_convention.get(), TRAFFIC_CONVENTION_LEN); - bool is_rhd = read_db_bool("IsRHD"); + bool is_rhd = Params().read_db_bool("IsRHD"); if (is_rhd) { s->traffic_convention[1] = 1.0; } else { diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 52e7401a3..170982db1 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -14,7 +14,6 @@ #include "commonmodel.h" #include "runners/run.h" -#include #include #include "messaging.hpp" diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 1122e1b7a..515838075 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -14,9 +14,9 @@ EventName = car.CarEvent.EventName # We recommend that you do not change these numbers from the defaults. # ****************************************************************************************** -_AWARENESS_TIME = 70. # one minute limit without user touching steering wheels make the car enter a terminal status -_AWARENESS_PRE_TIME_TILL_TERMINAL = 15. # a first alert is issued 15s before expiration -_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. # a second alert is issued 6s before start decelerating the car +_AWARENESS_TIME = 35. # passive wheel touch total timeout +_AWARENESS_PRE_TIME_TILL_TERMINAL = 7. +_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5. _DISTRACTED_TIME = 11. _DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index a3ddaf484..524440404 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -40,7 +40,11 @@ def get_expected_signature(fw_fn=None): if fw_fn is None: fw_fn = get_firmware_fn() - return Panda.get_signature_from_firmware(fw_fn) + try: + return Panda.get_signature_from_firmware(fw_fn) + except Exception: + cloudlog.exception("Error computing expected signature") + return b"" def update_panda(): diff --git a/selfdrive/proclogd/SConscript b/selfdrive/proclogd/SConscript index e7677099e..b80d17200 100644 --- a/selfdrive/proclogd/SConscript +++ b/selfdrive/proclogd/SConscript @@ -1,2 +1,2 @@ Import('env', 'cereal', 'messaging') -env.Program('proclogd.cc', LIBS=[cereal, messaging, 'pthread', 'zmq', 'czmq', 'capnp', 'kj']) +env.Program('proclogd.cc', LIBS=[cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']) diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index b5c8b78f9..facbf6403 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -13,5 +13,8 @@ else: 'sensors/bmx055_gyro.cc', 'sensors/bmx055_magn.cc', 'sensors/bmx055_temp.cc', + 'sensors/lsm6ds3_accel.cc', + 'sensors/lsm6ds3_gyro.cc', + 'sensors/lsm6ds3_temp.cc', ] env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index 1c23ceb6a..bc742e435 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -6,22 +6,24 @@ #include "bmx055_magn.hpp" -int16_t parse_xy(uint8_t lsb, uint8_t msb){ +static int16_t parse_xy(uint8_t lsb, uint8_t msb){ // 13 bit uint16_t combined = (uint16_t(msb) << 5) | uint16_t(lsb >> 3); return int16_t(combined << 3) / (1 << 3); } -int16_t parse_z(uint8_t lsb, uint8_t msb){ +static int16_t parse_z(uint8_t lsb, uint8_t msb){ // 15 bit uint16_t combined = (uint16_t(msb) << 7) | uint16_t(lsb >> 1); return int16_t(combined << 1) / (1 << 1); } -uint16_t parse_rhall(uint8_t lsb, uint8_t msb){ +/* +static uint16_t parse_rhall(uint8_t lsb, uint8_t msb){ // 14 bit return (uint16_t(msb) << 6) | uint16_t(lsb >> 2); } +*/ BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc new file mode 100644 index 000000000..a7253b0a6 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -0,0 +1,62 @@ +#include +#include "common/swaglog.h" +#include "common/timing.h" + +#include "lsm6ds3_accel.hpp" + + +LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus) : I2CSensor(bus) {} + +int LSM6DS3_Accel::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID); + ret = -1; + goto fail; + } + + // TODO: set scale and bandwith. Default is +- 2G, 50 Hz + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ); + if (ret < 0){ + goto fail; + } + + +fail: + return ret; +} + +void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event){ + + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = 9.81 * 2.0f / (1 << 15); + float x = read_16_bit(buffer[0], buffer[1]) * scale; + float y = read_16_bit(buffer[2], buffer[3]) * scale; + float z = read_16_bit(buffer[4], buffer[5]) * scale; + + event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(start_time); + + float xyz[] = {y, -x, z}; + kj::ArrayPtr vs(&xyz[0], 3); + + auto svec = event.initAcceleration(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.hpp b/selfdrive/sensord/sensors/lsm6ds3_accel.hpp new file mode 100644 index 000000000..314dcf8b5 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define LSM6DS3_ACCEL_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F +#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 +#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28 + +// Constants +#define LSM6DS3_ACCEL_CHIP_ID 0x69 +#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) + + +class LSM6DS3_Accel : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} +public: + LSM6DS3_Accel(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc new file mode 100644 index 000000000..93cb24ea2 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -0,0 +1,65 @@ +#include +#include +#include "common/swaglog.h" +#include "common/timing.h" + +#include "lsm6ds3_gyro.hpp" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + + +LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus) : I2CSensor(bus) {} + +int LSM6DS3_Gyro::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != LSM6DS3_GYRO_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID); + ret = -1; + goto fail; + } + + // TODO: set scale. Default is +- 250 deg/s + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ); + if (ret < 0){ + goto fail; + } + + +fail: + return ret; +} + +void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event){ + + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = 250.0f / (1 << 15); + float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); + float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); + float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + + event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3); + event.setVersion(1); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(start_time); + + float xyz[] = {y, -x, z}; + kj::ArrayPtr vs(&xyz[0], 3); + + auto svec = event.initGyroUncalibrated(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.hpp b/selfdrive/sensord/sensors/lsm6ds3_gyro.hpp new file mode 100644 index 000000000..13cec204e --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define LSM6DS3_GYRO_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_GYRO_I2C_REG_ID 0x0F +#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 +#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 + +// Constants +#define LSM6DS3_GYRO_CHIP_ID 0x69 +#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) + + +class LSM6DS3_Gyro : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} +public: + LSM6DS3_Gyro(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc new file mode 100644 index 000000000..04b0d3d4f --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -0,0 +1,45 @@ +#include +#include "common/swaglog.h" +#include "common/timing.h" + +#include "lsm6ds3_temp.hpp" + + +LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {} + +int LSM6DS3_Temp::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(LSM6DS3_TEMP_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != LSM6DS3_TEMP_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID); + ret = -1; + goto fail; + } + +fail: + return ret; +} + +void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event){ + + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[2]; + int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / 16.0f; + + event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.hpp b/selfdrive/sensord/sensors/lsm6ds3_temp.hpp new file mode 100644 index 000000000..eea5ff4a6 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define LSM6DS3_TEMP_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_TEMP_I2C_REG_ID 0x0F +#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20 + +// Constants +#define LSM6DS3_TEMP_CHIP_ID 0x69 + + +class LSM6DS3_Temp : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;} +public: + LSM6DS3_Temp(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc index 4119d8ede..58295679e 100644 --- a/selfdrive/sensord/sensors_qcom.cc +++ b/selfdrive/sensord/sensors_qcom.cc @@ -48,7 +48,12 @@ void sigpipe_handler(int sig) { void sensor_loop() { LOG("*** sensor loop"); + + uint64_t frame = 0; + bool low_power_mode = false; + while (!do_exit) { + SubMaster sm({"thermal"}); PubMaster pm({"sensorEvents"}); struct sensors_poll_device_t* device; @@ -90,6 +95,14 @@ void sensor_loop() { {SENSOR_LIGHT, ms2ns(100)} }; + // sensors needed while offroad + std::set offroad_sensors = { + SENSOR_LIGHT, + SENSOR_ACCELEROMETER, + SENSOR_GYRO_UNCALIBRATED, + }; + + // init all the sensors for (auto &s : sensors) { device->activate(device, s.first, 0); device->activate(device, s.first, 1); @@ -189,6 +202,22 @@ void sensor_loop() { re_init_sensors = false; break; } + + // Check whether to go into low power mode at 5Hz + if (frame % 20 == 0 && sm.update(0) > 0) { + bool offroad = !sm["thermal"].getThermal().getStarted(); + if (low_power_mode != offroad) { + for (auto &s : sensors) { + device->activate(device, s.first, 0); + if (!offroad || offroad_sensors.find(s.first) != offroad_sensors.end()) { + device->activate(device, s.first, 1); + } + } + low_power_mode = offroad; + } + } + + frame++; } sensors_close(device); } diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index d9da48605..1dd5ed10a 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -11,10 +11,16 @@ #include "sensors/sensor.hpp" #include "sensors/constants.hpp" + #include "sensors/bmx055_accel.hpp" #include "sensors/bmx055_gyro.hpp" #include "sensors/bmx055_magn.hpp" #include "sensors/bmx055_temp.hpp" + +#include "sensors/lsm6ds3_accel.hpp" +#include "sensors/lsm6ds3_gyro.hpp" +#include "sensors/lsm6ds3_temp.hpp" + #include "sensors/light_sensor.hpp" volatile sig_atomic_t do_exit = 0; @@ -36,18 +42,28 @@ int sensor_loop() { return -1; } - BMX055_Accel accel(i2c_bus_imu); - BMX055_Gyro gyro(i2c_bus_imu); - BMX055_Magn magn(i2c_bus_imu); - BMX055_Temp temp(i2c_bus_imu); + BMX055_Accel bmx055_accel(i2c_bus_imu); + BMX055_Gyro bmx055_gyro(i2c_bus_imu); + BMX055_Magn bmx055_magn(i2c_bus_imu); + BMX055_Temp bmx055_temp(i2c_bus_imu); + + LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu); + LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu); + LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu); + LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw"); // Sensor init std::vector sensors; - sensors.push_back(&accel); - sensors.push_back(&gyro); - sensors.push_back(&magn); - sensors.push_back(&temp); + sensors.push_back(&bmx055_accel); + sensors.push_back(&bmx055_gyro); + sensors.push_back(&bmx055_magn); + sensors.push_back(&bmx055_temp); + + sensors.push_back(&lsm6ds3_accel); + sensors.push_back(&lsm6ds3_gyro); + sensors.push_back(&lsm6ds3_temp); + sensors.push_back(&light); diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 6ee6caf08..ff4645574 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -5,11 +5,11 @@ from nose.tools import nottest from common.hardware import PC from common.apk import update_apks, start_offroad, pm_apply_packages, android_packages -from common.params import Params from selfdrive.version import training_version, terms_version from selfdrive.manager import start_managed_process, kill_managed_process, get_running def set_params_enabled(): + from common.params import Params params = Params() params.put("HasAcceptedTerms", terms_version) params.put("HasCompletedSetup", "1") diff --git a/selfdrive/test/test_cpu_usage.py b/selfdrive/test/test_cpu_usage.py index 0e06e0ab4..a86472105 100755 --- a/selfdrive/test/test_cpu_usage.py +++ b/selfdrive/test/test_cpu_usage.py @@ -21,13 +21,13 @@ def print_cpu_usage(first_proc, last_proc): ("selfdrive.controls.plannerd", 11.84), ("selfdrive.locationd.paramsd", 10.5), ("./_modeld", 7.12), - ("selfdrive.controls.radard", 9.54), ("./camerad", 7.07), ("./_sensord", 6.17), ("./_ui", 5.82), + ("selfdrive.controls.radard", 5.67), ("./boardd", 3.63), ("./_dmonitoringmodeld", 2.67), - ("selfdrive.logmessaged", 2.71), + ("selfdrive.logmessaged", 1.7), ("selfdrive.thermald.thermald", 2.41), ("selfdrive.locationd.calibrationd", 2.0), ("selfdrive.monitoring.dmonitoringd", 1.90), diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index fc963242d..39e07c0dd 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -3,6 +3,7 @@ import datetime import os import time from collections import namedtuple +from typing import Dict, Optional, Tuple import psutil from smbus2 import SMBus @@ -12,7 +13,7 @@ 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, put_nonblocking +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.loggerd.config import get_available_percent @@ -39,6 +40,8 @@ DAYS_NO_CONNECTIVITY_MAX = 7 # do not allow to engage after a week without inte DAYS_NO_CONNECTIVITY_PROMPT = 4 # send an offroad prompt after 4 days with no internet DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert +prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {} + LEON = False last_eon_fan_val = None @@ -156,6 +159,13 @@ def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition): return new_speed +def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: Optional[str]=None): + if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text): + return + prev_offroad_states[offroad_alert] = (show_alert, extra_text) + set_offroad_alert(offroad_alert, show_alert, extra_text) + + def thermald_thread(): health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency @@ -164,17 +174,19 @@ def thermald_thread(): health_sock = messaging.sub_sock('health', timeout=health_timeout) location_sock = messaging.sub_sock('gpsLocation') - ignition = False fan_speed = 0 count = 0 + startup_conditions = { + "ignition": False, + } + startup_conditions_prev = startup_conditions.copy() + off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green - thermal_status_prev = ThermalStatus.green usb_power = True - usb_power_prev = True current_branch = get_git_branch() network_type = NetworkType.none @@ -183,9 +195,6 @@ def thermald_thread(): current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) health_prev = None - fw_version_match_prev = True - current_update_alert = None - time_valid_prev = True should_start_prev = False handle_fan = None is_uno = False @@ -210,12 +219,12 @@ def thermald_thread(): if health.health.hwType == log.HealthData.HwType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: - if ignition: + if startup_conditions["ignition"]: cloudlog.error("Lost panda connection while onroad") - ignition = False + startup_conditions["ignition"] = False else: no_panda_cnt = 0 - ignition = health.health.ignitionLine or health.health.ignitionCan + startup_conditions["ignition"] = health.health.ignitionLine or health.health.ignitionCan # Setup fan handler on first connect to panda if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: @@ -270,7 +279,7 @@ def thermald_thread(): bat_temp = msg.thermal.bat if handle_fan is not None: - fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition) + fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) msg.thermal.fanSpeed = fan_speed # If device is offroad we want to cool down before going onroad @@ -302,12 +311,8 @@ def thermald_thread(): now = datetime.datetime.utcnow() # show invalid date/time alert - time_valid = now.year >= 2019 - if time_valid and not time_valid_prev: - set_offroad_alert("Offroad_InvalidTime", False) - if not time_valid and time_valid_prev: - set_offroad_alert("Offroad_InvalidTime", True) - time_valid_prev = time_valid + startup_conditions["time_valid"] = now.year >= 2019 + set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # Show update prompt try: @@ -326,72 +331,41 @@ def thermald_thread(): else: extra_text = last_update_exception - if current_update_alert != "update" + extra_text: - current_update_alert = "update" + extra_text - set_offroad_alert("Offroad_ConnectivityNeeded", False) - set_offroad_alert("Offroad_ConnectivityNeededPrompt", False) - set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text) + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) + set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: - if current_update_alert != "expired": - current_update_alert = "expired" - set_offroad_alert("Offroad_UpdateFailed", False) - set_offroad_alert("Offroad_ConnectivityNeededPrompt", False) - set_offroad_alert("Offroad_ConnectivityNeeded", True) + set_offroad_alert_if_changed("Offroad_UpdateFailed", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) - if current_update_alert != "prompt" + remaining_time: - current_update_alert = "prompt" + remaining_time - set_offroad_alert("Offroad_UpdateFailed", False) - set_offroad_alert("Offroad_ConnectivityNeeded", False) - set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") - elif current_update_alert is not None: - current_update_alert = None - set_offroad_alert("Offroad_UpdateFailed", False) - set_offroad_alert("Offroad_ConnectivityNeeded", False) - set_offroad_alert("Offroad_ConnectivityNeededPrompt", False) + set_offroad_alert_if_changed("Offroad_UpdateFailed", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") + else: + set_offroad_alert_if_changed("Offroad_UpdateFailed", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) - do_uninstall = params.get("DoUninstall") == b"1" - accepted_terms = params.get("HasAcceptedTerms") == terms_version + startup_conditions["not_uninstalling"] = not params.get("DoUninstall") == b"1" + startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version completed_training = params.get("CompletedTrainingVersion") == training_version panda_signature = params.get("PandaFirmware") - fw_version_match = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None) - - should_start = ignition + startup_conditions["fw_version_match"] = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None) + set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # with 2% left, we killall, otherwise the phone will take a long time to boot - should_start = should_start and msg.thermal.freeSpace > 0.02 - - # confirm we have completed training and aren't uninstalling - should_start = should_start and accepted_terms and (not do_uninstall) and \ - (completed_training or current_branch in ['dashcam', 'dashcam-staging']) - - # check for firmware mismatch - should_start = should_start and fw_version_match - - # check if system time is valid - should_start = should_start and time_valid - - # don't start while taking snapshot - if not should_start_prev: - is_viewing_driver = params.get("IsDriverViewEnabled") == b"1" - is_taking_snapshot = params.get("IsTakingSnapshot") == b"1" - should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver) - - if fw_version_match and not fw_version_match_prev: - set_offroad_alert("Offroad_PandaFirmwareMismatch", False) - if not fw_version_match and fw_version_match_prev: - set_offroad_alert("Offroad_PandaFirmwareMismatch", True) - + startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02 + startup_conditions["completed_training"] = completed_training or (current_branch in ['dashcam', 'dashcam-staging']) + startup_conditions["not_driver_view"] = not params.get("IsDriverViewEnabled") == b"1" + startup_conditions["not_taking_snapshot"] = not params.get("IsTakingSnapshot") == b"1" # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 - if thermal_status >= ThermalStatus.danger: - should_start = False - if thermal_status_prev < ThermalStatus.danger: - set_offroad_alert("Offroad_TemperatureTooHigh", True) - else: - if thermal_status_prev >= ThermalStatus.danger: - set_offroad_alert("Offroad_TemperatureTooHigh", False) + startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger + set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) + should_start = all(startup_conditions.values()) if should_start: if not should_start_prev: @@ -403,8 +377,10 @@ def thermald_thread(): started_seen = True os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor') else: + if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): + cloudlog.event("Startup blocked", startup_conditions=startup_conditions) if should_start_prev or (count == 0): - put_nonblocking("IsOffroad", "1") + params.put("IsOffroad", "1") started_ts = None if off_ts is None: @@ -433,15 +409,10 @@ def thermald_thread(): msg.thermal.thermalStatus = thermal_status thermal_sock.send(msg.to_bytes()) - if usb_power_prev and not usb_power: - set_offroad_alert("Offroad_ChargeDisabled", True) - elif usb_power and not usb_power_prev: - set_offroad_alert("Offroad_ChargeDisabled", False) + set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) - thermal_status_prev = thermal_status - usb_power_prev = usb_power - fw_version_match_prev = fw_version_match should_start_prev = should_start + startup_conditions_prev = startup_conditions.copy() # report to server once per minute if (count % int(60. / DT_TRML)) == 0: diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index fbebddbd7..e43f98371 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -17,10 +17,12 @@ def get_tombstones(): files = [] for folder in ["/data/tombstones/", "/var/crash/"]: if os.path.exists(folder): - for fn in os.listdir(folder): - if fn.startswith("tombstone") or fn.endswith(".crash"): - path = os.path.join(folder, fn) - files.append((path, int(os.stat(path).st_ctime))) + with os.scandir(folder) as d: + + # Loop over first 1000 directory entries + for _, f in zip(range(1000), d): + if f.name.startswith("tombstone") or f.name.endswith(".crash"): + files.append((f.path, int(f.stat().st_ctime))) return files diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 2207123a9..55f269cb5 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,7 +1,7 @@ Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] -libs = [common, 'zmq', 'czmq', 'capnp', 'kj', 'm', cereal, messaging, gpucommon, visionipc] +libs = [common, 'zmq', 'capnp', 'kj', 'm', cereal, messaging, gpucommon, visionipc] if qt_env is None: diff --git a/selfdrive/ui/android/sl_sound.cc b/selfdrive/ui/android/sl_sound.cc index 95cebab9c..52604df93 100644 --- a/selfdrive/ui/android/sl_sound.cc +++ b/selfdrive/ui/android/sl_sound.cc @@ -15,7 +15,6 @@ struct SLSound::Player { SLObjectItf player; SLPlayItf playItf; - // slplay_callback runs on a background thread,use atomic to ensure thread safe. std::atomic repeat; }; @@ -57,8 +56,8 @@ bool SLSound::init() { void SLAPIENTRY slplay_callback(SLPlayItf playItf, void *context, SLuint32 event) { SLSound::Player *s = reinterpret_cast(context); - if (event == SL_PLAYEVENT_HEADATEND && s->repeat > 1) { - --s->repeat; + if (event == SL_PLAYEVENT_HEADATEND && s->repeat != 0) { + if (s->repeat > 0) --s->repeat; (*playItf)->SetPlayState(playItf, SL_PLAYSTATE_STOPPED); (*playItf)->SetMarkerPosition(playItf, 0); (*playItf)->SetPlayState(playItf, SL_PLAYSTATE_PLAYING); @@ -69,10 +68,13 @@ bool SLSound::play(AudibleAlert alert) { if (currentSound_ != AudibleAlert::NONE) { stop(); } + auto player = player_.at(alert); SLPlayItf playItf = player->playItf; - player->repeat = sound_map[alert].second; - if (player->repeat > 0) { + + int loops = sound_map[alert].second; + player->repeat = loops > 0 ? loops - 1 : loops; + if (player->repeat != 0) { ReturnOnError((*playItf)->RegisterCallback(playItf, slplay_callback, player), "Failed to register callback"); ReturnOnError((*playItf)->SetCallbackEventsMask(playItf, SL_PLAYEVENT_HEADATEND), "Failed to set callback event mask"); } diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc index e64ab6cde..24a0db2a2 100644 --- a/selfdrive/ui/android/ui.cc +++ b/selfdrive/ui/android/ui.cc @@ -3,6 +3,8 @@ #include #include +#include + #include "common/util.h" #include "common/utilpp.h" #include "common/params.h" @@ -13,65 +15,11 @@ #include "paint.hpp" #include "android/sl_sound.hpp" -// Includes for light sensor -#include -#include -#include - volatile sig_atomic_t do_exit = 0; static void set_do_exit(int sig) { do_exit = 1; } - -static void* light_sensor_thread(void *args) { - set_thread_name("light_sensor"); - - int err; - UIState *s = (UIState*)args; - s->light_sensor = 0.0; - - struct sensors_poll_device_t* device; - struct sensors_module_t* module; - - hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module); - sensors_open(&module->common, &device); - - // need to do this - struct sensor_t const* list; - module->get_sensors_list(module, &list); - - int SENSOR_LIGHT = 7; - - err = device->activate(device, SENSOR_LIGHT, 0); - if (err != 0) goto fail; - err = device->activate(device, SENSOR_LIGHT, 1); - if (err != 0) goto fail; - - device->setDelay(device, SENSOR_LIGHT, ms2ns(100)); - - while (!do_exit) { - static const size_t numEvents = 1; - sensors_event_t buffer[numEvents]; - - int n = device->poll(device, buffer, numEvents); - if (n < 0) { - LOG_100("light_sensor_poll failed: %d", n); - } - if (n > 0) { - s->light_sensor = buffer[0].light; - } - } - sensors_close(device); - return NULL; - -fail: - LOGE("LIGHT SENSOR IS MISSING"); - s->light_sensor = 255; - - return NULL; -} - static void ui_set_brightness(UIState *s, int brightness) { static int last_brightness = -1; if (last_brightness != brightness && (s->awake || brightness == 0)) { @@ -81,36 +29,36 @@ static void ui_set_brightness(UIState *s, int brightness) { } } -int event_processing_enabled = -1; -static void enable_event_processing(bool yes) { - if (event_processing_enabled != 1 && yes) { - system("service call window 18 i32 1"); // enable event processing - event_processing_enabled = 1; - } else if (event_processing_enabled != 0 && !yes) { - system("service call window 18 i32 0"); // disable event processing - event_processing_enabled = 0; - } -} +static void handle_display_state(UIState *s, bool user_input) { -static void set_awake(UIState *s, bool awake) { - if (awake) { - // 30 second timeout - s->awake_timeout = 30*UI_FREQ; - } - if (s->awake != awake) { - s->awake = awake; + static int awake_timeout = 0; + awake_timeout = std::max(awake_timeout-1, 0); - // TODO: replace command_awake and command_sleep with direct calls to android - if (awake) { - LOGW("awake normal"); - framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL); - enable_event_processing(true); - } else { - LOGW("awake off"); - ui_set_brightness(s, 0); - framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF); - enable_event_processing(false); - } + // tap detection while display is off + const float accel_samples = 5*UI_FREQ; + static float accel_prev, gyro_prev = 0; + + bool accel_trigger = abs(s->accel_sensor - accel_prev) > 0.2; + bool gyro_trigger = abs(s->gyro_sensor - gyro_prev) > 0.15; + user_input = user_input || (accel_trigger && gyro_trigger); + gyro_prev = s->gyro_sensor; + accel_prev = (accel_prev*(accel_samples - 1) + s->accel_sensor) / accel_samples; + + // determine desired state + bool should_wake = s->awake; + if (user_input || s->ignition || s->started) { + should_wake = true; + awake_timeout = 30*UI_FREQ; + } else if (awake_timeout == 0){ + should_wake = false; + } + + // handle state transition + if (s->awake != should_wake) { + s->awake = should_wake; + int display_mode = s->awake ? HWC_POWER_MODE_NORMAL : HWC_POWER_MODE_OFF; + LOGW("setting display mode %d", display_mode); + framebuffer_set_power(s->fb, display_mode); } } @@ -120,7 +68,7 @@ static void handle_vision_touch(UIState *s, int touch_x, int touch_y) { if (!s->scene.frontview) { s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed; } else { - write_db_value("IsDriverViewEnabled", "0", 1); + Params().write_db_value("IsDriverViewEnabled", "0", 1); } } } @@ -161,9 +109,7 @@ static void update_offroad_layout_state(UIState *s, PubMaster *pm) { } int main(int argc, char* argv[]) { - int err; setpriority(PRIO_PROCESS, 0, -14); - signal(SIGINT, (sighandler_t)set_do_exit); SLSound sound; @@ -172,17 +118,11 @@ int main(int argc, char* argv[]) { ui_init(s); s->sound = &sound; - set_awake(s, true); - enable_event_processing(true); - - PubMaster *pm = new PubMaster({"offroadLayout"}); - pthread_t light_sensor_thread_handle; - err = pthread_create(&light_sensor_thread_handle, NULL, - light_sensor_thread, s); - assert(err == 0); - TouchState touch = {0}; touch_init(&touch); + handle_display_state(s, true); + + PubMaster *pm = new PubMaster({"offroadLayout"}); // light sensor scaling and volume params const bool LEON = util::read_file("/proc/cmdline").find("letv") != std::string::npos; @@ -203,9 +143,8 @@ int main(int argc, char* argv[]) { s->sound->setVolume(MIN_VOLUME); while (!do_exit) { - if (!s->started || !s->vision_connected) { - // Delay a while to avoid 9% cpu usage while car is not started and user is keeping touching on the screen. - usleep(30 * 1000); + if (!s->started) { + usleep(50 * 1000); } double u1 = millis_since_boot(); @@ -215,23 +154,12 @@ int main(int argc, char* argv[]) { int touch_x = -1, touch_y = -1; int touched = touch_poll(&touch, &touch_x, &touch_y, 0); if (touched == 1) { - set_awake(s, true); handle_sidebar_touch(s, touch_x, touch_y); handle_vision_touch(s, touch_x, touch_y); } - // manage wakefulness - if (s->started || s->ignition) { - set_awake(s, true); - } - - if (s->awake_timeout > 0) { - s->awake_timeout--; - } else { - set_awake(s, false); - } - // Don't waste resources on drawing in case screen is off + handle_display_state(s, touched == 1); if (!s->awake) { continue; } @@ -255,10 +183,7 @@ int main(int argc, char* argv[]) { framebuffer_swap(s->fb); } - set_awake(s, true); - - err = pthread_join(light_sensor_thread_handle, NULL); - assert(err == 0); + handle_display_state(s, true); delete s->sm; delete pm; return 0; diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 881074d8f..16db6cad6 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -16,30 +16,25 @@ extern "C"{ #include "paint.hpp" #include "sidebar.hpp" + // TODO: this is also hardcoded in common/transformations/camera.py // TODO: choose based on frame input size -#ifdef QCOM2 -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 +#ifdef QCOM2 +const float zoom = 1.5; const mat3 intrinsic_matrix = (mat3){{ - 910., 0., 582., - 0., 910., 437., + 2648.0, 0.0, 1928.0/2, + 0.0, 2648.0, 1208.0/2, + 0.0, 0.0, 1.0 +}}; +#else +const float zoom = 2.35; +const mat3 intrinsic_matrix = (mat3){{ + 910., 0., 1164.0/2, + 0., 910., 874.0/2, 0., 0., 1. }}; #endif -const uint8_t alert_colors[][4] = { - [STATUS_OFFROAD] = {0x07, 0x23, 0x39, 0xf1}, - [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xc8}, - [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1}, - [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1}, - [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1}, -}; - // Projects a point in car to space to the corresponding point in full frame // image space. vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) { @@ -233,13 +228,13 @@ static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { NVGpaint track_bg; if (is_mpc) { // Draw colored MPC track - const Color clr = bg_colors[s->status]; + const NVGcolor clr = bg_colors[s->status]; track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4, - nvgRGBA(clr.r, clr.g, clr.b, 255), nvgRGBA(clr.r, clr.g, clr.b, 255/2)); + nvgRGBA(clr.r, clr.g, clr.b, 255), nvgRGBA(clr.r, clr.g, clr.b, 255/2)); } else { // Draw white vision track track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4, - COLOR_WHITE, COLOR_WHITE_ALPHA(0)); + COLOR_WHITE, COLOR_WHITE_ALPHA(0)); } nvgFillPaint(s->vg, track_bg); nvgFill(s->vg); @@ -347,21 +342,19 @@ static void ui_draw_vision_lanes(UIState *s) { // Draw all world space objects. static void ui_draw_world(UIState *s) { const UIScene *scene = &s->scene; - const Rect &viz_rect = scene->viz_rect; - const int viz_w = s->fb_w - bdr_s * 2; - const int inner_height = float(viz_w) * s->fb_h / s->fb_w; + nvgSave(s->vg); - nvgScissor(s->vg, viz_rect.x, viz_rect.y, viz_rect.w, viz_rect.h); + nvgScissor(s->vg, s->video_rect.x, s->video_rect.y, s->video_rect.w, s->video_rect.h); - nvgTranslate(s->vg, viz_rect.x+scene->ui_viz_ro, viz_rect.y + (viz_rect.h-inner_height)/2.0); - nvgScale(s->vg, (float)viz_w / s->fb_w, (float)inner_height / s->fb_h); + // 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); - float w = 1440.0f; // Why 1440? - nvgTranslate(s->vg, (s->fb_w - w) / 2.0f, 0.0); + // 2) Apply same scaling as video + nvgScale(s->vg, zoom, zoom); - nvgTranslate(s->vg, -w / 2, -1080.0f / 2); - nvgScale(s->vg, 2.0, 2.0); - nvgScale(s->vg, w / s->stream.bufs_info.width, 1080.0f / s->stream.bufs_info.height); + // 3) Put (0, 0) in top left corner of video + nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); // Draw lane edges and vision/mpc tracks ui_draw_vision_lanes(s); @@ -443,23 +436,14 @@ static void ui_draw_vision_event(UIState *s) { // draw winding road sign const int img_turn_size = 160*1.5; ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); - } else { + } else if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size); const int bg_wheel_y = viz_event_y + (bg_wheel_size/2); - NVGcolor color = COLOR_BLACK_ALPHA(0); - if (s->status == STATUS_ENGAGED) { - color = nvgRGBA(23, 134, 68, 255); - } else if (s->status == STATUS_WARNING) { - color = COLOR_OCHRE; - } else { - color = nvgRGBA(23, 51, 73, 255); - } + const NVGcolor color = bg_colors[s->status]; - if (s->scene.controls_state.getEngageable()){ - ui_draw_circle_image(s->vg, bg_wheel_x, bg_wheel_y, bg_wheel_size, s->img_wheel, color, 1.0f, bg_wheel_y - 25); - } + ui_draw_circle_image(s->vg, bg_wheel_x, bg_wheel_y, bg_wheel_size, s->img_wheel, color, 1.0f, bg_wheel_y - 25); } } @@ -541,7 +525,7 @@ 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, int va_color, +void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, UIStatus va_color, const char* va_text1, const char* va_text2) { static std::map alert_size_map = { {cereal::ControlsState::AlertSize::NONE, 0}, @@ -552,7 +536,8 @@ void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, const UIScene *scene = &s->scene; bool longAlert1 = strlen(va_text1) > 15; - const uint8_t *color = alert_colors[va_color]; + NVGcolor color = bg_colors[va_color]; + color.a *= s->alert_blinking_alpha; int alr_s = alert_size_map[va_size]; const int alr_x = scene->viz_rect.x - bdr_s; @@ -560,10 +545,10 @@ void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, const int alr_h = alr_s+(va_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, nvgRGBA(color[0],color[1],color[2],(color[3]*s->alert_blinking_alpha))); + ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, color); NVGpaint gradient = nvgLinearGradient(s->vg, alr_x, alr_y, alr_x, alr_y+alr_h, - nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35)); + nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35)); ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, gradient); nvgFillColor(s->vg, COLOR_WHITE); @@ -589,9 +574,10 @@ void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, static void ui_draw_vision(UIState *s) { const UIScene *scene = &s->scene; const Rect &viz_rect = scene->viz_rect; + // Draw video frames glEnable(GL_SCISSOR_TEST); - glViewport(viz_rect.x+scene->ui_viz_ro, viz_rect.y, s->fb_w - bdr_s*2, viz_rect.h); + glViewport(s->video_rect.x, s->video_rect.y, s->video_rect.w, s->video_rect.h); glScissor(viz_rect.x, viz_rect.y, viz_rect.w, viz_rect.h); draw_frame(s); glDisable(GL_SCISSOR_TEST); @@ -618,8 +604,8 @@ static void ui_draw_vision(UIState *s) { } static void ui_draw_background(UIState *s) { - const Color color = bg_colors[s->status]; - glClearColor(color.r/256.0, color.g/256.0, color.b/256.0, 1.0); + const NVGcolor color = bg_colors[s->status]; + glClearColor(color.r, color.g, color.b, 1.0); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); } @@ -656,7 +642,7 @@ void ui_draw_image(NVGcontext *vg, float x, float y, float w, float h, int image void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r, int width) { nvgBeginPath(vg); - r > 0? nvgRoundedRect(vg, x, y, w, h, r) : nvgRect(vg, x, y, w, h); + r > 0 ? nvgRoundedRect(vg, x, y, w, h, r) : nvgRect(vg, x, y, w, h); if (width) { nvgStrokeColor(vg, color); nvgStrokeWidth(vg, width); @@ -814,10 +800,13 @@ void ui_nvg_init(UIState *s) { glBindVertexArray(0); } - // frame from 4/3 to box size with a 2x zoom + s->video_rect = Rect{bdr_s * 3, bdr_s, s->fb_w - 4 * 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 = {{ - (float)(2*(4./3.)/((float)(s->fb_w-(bdr_s*2))/(s->fb_h-(bdr_s*2)))), 0.0, 0.0, 0.0, - 0.0, 2.0, 0.0, 0.0, + zx, 0.0, 0.0, 0.0, + 0.0, zy, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; diff --git a/selfdrive/ui/qt/qt_sound.cc b/selfdrive/ui/qt/qt_sound.cc index 76842c67e..c44e27ec9 100644 --- a/selfdrive/ui/qt/qt_sound.cc +++ b/selfdrive/ui/qt/qt_sound.cc @@ -9,7 +9,7 @@ QtSound::QtSound() { } bool QtSound::play(AudibleAlert alert) { - sounds[alert].setLoopCount(sound_map[alert].second); + sounds[alert].setLoopCount(sound_map[alert].second>-1 ? sound_map[alert].second : QSoundEffect::Infinite); sounds[alert].setVolume(0.9); sounds[alert].play(); return true; diff --git a/selfdrive/ui/qt/settings.cc b/selfdrive/ui/qt/settings.cc index 612fe2af3..728b3d84b 100644 --- a/selfdrive/ui/qt/settings.cc +++ b/selfdrive/ui/qt/settings.cc @@ -43,7 +43,7 @@ ParamsToggle::ParamsToggle(QString param, QString title, QString description, QS setLayout(hlayout); - checkbox->setChecked(read_db_bool(param.toStdString().c_str())); + checkbox->setChecked(Params().read_db_bool(param.toStdString().c_str())); setStyleSheet(R"( QCheckBox { font-size: 70px } @@ -58,7 +58,7 @@ ParamsToggle::ParamsToggle(QString param, QString title, QString description, QS void ParamsToggle::checkboxClicked(int state){ char value = state ? '1': '0'; - write_db_value(param.toStdString().c_str(), &value, 1); + Params().write_db_value(param.toStdString().c_str(), &value, 1); } SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index 60a8e28cd..ceea1219b 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -15,9 +16,21 @@ #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 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; @@ -58,11 +71,14 @@ 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 = 0.0; - brightness_m = 5.0; + brightness_b = 200.0; + brightness_m = 10.0; } smooth_brightness = 512; } @@ -85,37 +101,37 @@ void GLWindow::initializeGL() { ui_state->fb_h = vwp_h; ui_init(ui_state); - timer->start(50); + timer->start(0); + backlight_timer->start(BACKLIGHT_DT * 100); } -void GLWindow::timerUpdate(){ +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 * 0.01f + smooth_brightness * 0.99f; - - std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness"); - if (brightness_control.is_open()){ - brightness_control << int(smooth_brightness) << "\n"; - brightness_control.close(); - } - - ui_update(ui_state); + smooth_brightness = clipped_brightness * k + smooth_brightness * (1.0f - k); + int brightness = smooth_brightness; #ifdef QCOM2 - if (ui_state->started != onroad){ - onroad = ui_state->started; - timer->setInterval(onroad ? 50 : 1000); - - int brightness = onroad ? 1023 : 0; - std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness"); - if (brightness_control.is_open()){ - brightness_control << int(brightness) << "\n"; - brightness_control.close(); - } + if (!ui_state->started){ + brightness = 0; } #endif - update(); + 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 + + ui_update(ui_state); + repaint(); } void GLWindow::resizeGL(int w, int h) { diff --git a/selfdrive/ui/qt/window.hpp b/selfdrive/ui/qt/window.hpp index cc525a6e0..8c82e6023 100644 --- a/selfdrive/ui/qt/window.hpp +++ b/selfdrive/ui/qt/window.hpp @@ -51,6 +51,8 @@ protected: private: QTimer * timer; + QTimer * backlight_timer; + UIState * ui_state; QtSound sound; @@ -62,6 +64,7 @@ private: public slots: void timerUpdate(); + void backlightUpdate(); signals: void openSettings(); diff --git a/selfdrive/ui/sound.hpp b/selfdrive/ui/sound.hpp index 931578306..bdb859080 100644 --- a/selfdrive/ui/sound.hpp +++ b/selfdrive/ui/sound.hpp @@ -11,7 +11,7 @@ static std::map> sound_map { {AudibleAlert::CHIME_WARNING1, {"../assets/sounds/warning_1.wav", 0}}, {AudibleAlert::CHIME_WARNING2, {"../assets/sounds/warning_2.wav", 0}}, {AudibleAlert::CHIME_WARNING2_REPEAT, {"../assets/sounds/warning_2.wav", 3}}, - {AudibleAlert::CHIME_WARNING_REPEAT, {"../assets/sounds/warning_repeat.wav", 3}}, + {AudibleAlert::CHIME_WARNING_REPEAT, {"../assets/sounds/warning_repeat.wav", -1}}, {AudibleAlert::CHIME_ERROR, {"../assets/sounds/error.wav", 0}}, {AudibleAlert::CHIME_PROMPT, {"../assets/sounds/error.wav", 0}} }; diff --git a/selfdrive/ui/spinner/Makefile b/selfdrive/ui/spinner/Makefile index 8d80f69a8..49401067f 100644 --- a/selfdrive/ui/spinner/Makefile +++ b/selfdrive/ui/spinner/Makefile @@ -10,7 +10,7 @@ WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=format-extra-args CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++1z -fPIC -O2 $(WARN_FLAGS) NANOVG_FLAGS = -I$(PHONELIBS)/nanovg diff --git a/selfdrive/ui/text/Makefile b/selfdrive/ui/text/Makefile index fbde1077d..ac0d720d5 100644 --- a/selfdrive/ui/text/Makefile +++ b/selfdrive/ui/text/Makefile @@ -10,7 +10,7 @@ WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=format-extra-args CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++1z -fPIC -O2 $(WARN_FLAGS) NANOVG_FLAGS = -I$(PHONELIBS)/nanovg diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8ca5d0170..4ebc50f06 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -19,7 +20,7 @@ extern volatile sig_atomic_t do_exit; int write_param_float(float param, const char* param_name, bool persistent_param) { char s[16]; int size = snprintf(s, sizeof(s), "%f", param); - return write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s), persistent_param); + return Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s)); } void ui_init(UIState *s) { @@ -217,16 +218,17 @@ void update_sockets(UIState *s) { } else if ((sm.frame - sm.rcv_frame("dMonitoringState")) > UI_FREQ/2) { scene.frontview = false; } - -#ifdef QCOM2 // TODO: use this for QCOM too if (sm.updated("sensorEvents")) { for (auto sensor : sm["sensorEvents"].getSensorEvents()) { if (sensor.which() == cereal::SensorEventData::LIGHT) { s->light_sensor = sensor.getLight(); + } else if (!s->started && sensor.which() == cereal::SensorEventData::ACCELERATION) { + s->accel_sensor = sensor.getAcceleration().getV()[2]; + } else if (!s->started && sensor.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) { + s->gyro_sensor = sensor.getGyroUncalibrated().getV()[1]; } } } -#endif s->started = scene.thermal.getStarted() || scene.frontview; } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 1255e54b7..1e2d91136 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -33,7 +33,6 @@ #define COLOR_WHITE_ALPHA(x) nvgRGBA(255, 255, 255, x) #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) -#define COLOR_OCHRE nvgRGBA(218, 111, 37, 255) #define UI_BUF_COUNT 4 @@ -62,10 +61,6 @@ const int TRACK_POINTS_MAX_CNT = 50 * 2; const int SET_SPEED_NA = 255; -typedef struct Color { - uint8_t r, g, b; -} Color; - typedef enum NetStatus { NET_CONNECTED, NET_DISCONNECTED, @@ -80,12 +75,12 @@ typedef enum UIStatus { STATUS_ALERT, } UIStatus; -static std::map bg_colors = { - {STATUS_OFFROAD, {0x07, 0x23, 0x39}}, - {STATUS_DISENGAGED, {0x17, 0x33, 0x49}}, - {STATUS_ENGAGED, {0x17, 0x86, 0x44}}, - {STATUS_WARNING, {0xDA, 0x6F, 0x25}}, - {STATUS_ALERT, {0xC9, 0x22, 0x31}}, +static std::map bg_colors = { + {STATUS_OFFROAD, nvgRGBA(0x07, 0x23, 0x39, 0xf1)}, + {STATUS_DISENGAGED, nvgRGBA(0x17, 0x33, 0x49, 0xc8)}, + {STATUS_ENGAGED, nvgRGBA(0x17, 0x86, 0x44, 0xf1)}, + {STATUS_WARNING, nvgRGBA(0xDA, 0x6F, 0x25, 0xf1)}, + {STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)}, }; typedef struct UIScene { @@ -183,8 +178,7 @@ typedef struct UIState { // device state bool awake; - int awake_timeout; - std::atomic light_sensor; + float light_sensor, accel_sensor, gyro_sensor; bool started; bool ignition; @@ -198,6 +192,8 @@ typedef struct UIState { track_vertices_data track_vertices[2]; model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2]; + + Rect video_rect; } UIState; void ui_init(UIState *s); @@ -210,7 +206,7 @@ int read_param(T* param, const char *param_name, bool persistent_param = false){ char *value; size_t sz; - int result = read_db_value(param_name, &value, &sz, persistent_param); + int result = Params(persistent_param).read_db_value(param_name, &value, &sz); if (result == 0){ std::string s = std::string(value, sz); // value is not null terminated free(value); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 1788816d3..bf2427e3a 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -34,7 +34,7 @@ import threading from pathlib import Path from typing import List, Tuple, Optional -from common.hardware import ANDROID +from common.hardware import ANDROID, TICI from common.basedir import BASEDIR from common.params import Params from selfdrive.swaglog import cloudlog @@ -142,7 +142,10 @@ def setup_git_options(cwd: str) -> None: def dismount_overlay() -> None: if os.path.ismount(OVERLAY_MERGED): cloudlog.info("unmounting existing overlay") - run(["umount", "-l", OVERLAY_MERGED]) + args = ["umount", "-l", OVERLAY_MERGED] + if TICI: + args = ["sudo"] + args + run(args) def init_overlay() -> None: @@ -185,7 +188,13 @@ def init_overlay() -> None: os.sync() overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}" - run(["mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED]) + + mount_cmd = ["mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED] + if TICI: + run(["sudo"] + mount_cmd) + run(["sudo", "chmod", "755", os.path.join(OVERLAY_METADATA, "work")]) + else: + run(mount_cmd) def finalize_update() -> None: