diff --git a/.gitignore b/.gitignore index f1f1d431c..f4932ed4f 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,7 @@ venv/ .vscode* model2.png a.out +.hypothesis *.dylib *.DSYM @@ -59,6 +60,7 @@ notebooks xx hyperthneed panda_jungle +provisioning .coverage* coverage.xml diff --git a/Jenkinsfile b/Jenkinsfile index 78a3a0fa6..97633da80 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -36,7 +36,7 @@ EOF""" def phone_steps(String device_type, steps) { lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) { - timeout(time: 60, unit: 'MINUTES') { + timeout(time: 90, unit: 'MINUTES') { phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),) steps.each { item -> phone(device_ip, item[0], item[1]) @@ -83,7 +83,6 @@ pipeline { } } - stages { /* @@ -113,6 +112,10 @@ pipeline { stage('On-device Tests') { agent { docker { + /* + filename 'Dockerfile.ondevice_ci' + args "--privileged -v /dev:/dev --shm-size=1G --user=root" + */ image 'python:3.7.3' args '--user=root' } @@ -121,17 +124,12 @@ pipeline { stages { stage('parallel tests') { parallel { - stage('Devel Build') { - environment { - CI_PUSH = "${env.BRANCH_NAME == 'master' ? 'master-ci' : ' '}" - } + stage('Devel Tests') { steps { phone_steps("eon-build", [ - ["build", "SCONS_CACHE=1 scons -j4"], - ["test athena", "nosetests -s selfdrive/athena/tests/test_athenad_old.py"], + ["build devel", "cd release && SCONS_CACHE=1 DEVEL_TEST=1 ./build_devel.sh"], ["test manager", "python selfdrive/manager/test/test_manager.py"], ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], - ["build devel", "cd release && CI_PUSH=${env.CI_PUSH} ./build_devel.sh"], ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"], ]) } @@ -150,6 +148,7 @@ pipeline { steps { phone_steps("eon", [ ["build", "SCONS_CACHE=1 scons -j4"], + ["test athena", "nosetests -s selfdrive/athena/tests/test_athenad_old.py"], ["test sounds", "nosetests -s selfdrive/test/test_sounds.py"], ["test boardd loopback", "nosetests -s selfdrive/boardd/tests/test_boardd_loopback.py"], ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], @@ -160,6 +159,27 @@ pipeline { } } + /* + stage('Power Consumption Tests') { + steps { + lock(resource: "", label: "c2-zookeeper", inversePrecedence: true, variable: 'device_ip', quantity: 1) { + timeout(time: 90, unit: 'MINUTES') { + sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device" + phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),) + phone(device_ip, "build", "SCONS_CACHE=1 scons -j4 && sync") + sh script: "/home/batman/tools/zookeeper/disable.py $device_ip", label: "turn off device" + sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device" + sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 3", label: "idle power consumption after boot" + sh script: "/home/batman/tools/zookeeper/ignition.py 1", label: "go onroad" + sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 10", label: "onroad power consumption" + sh script: "/home/batman/tools/zookeeper/ignition.py 0", label: "go offroad" + sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 2", label: "idle power consumption offroad" + } + } + } + } + */ + stage('Tici Build') { environment { R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}" @@ -169,6 +189,7 @@ pipeline { ["build", "SCONS_CACHE=1 scons -j16"], ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], + ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], //["build release3-staging", "cd release && PUSH=${env.R3_PUSH} ./build_release3.sh"], ]) } @@ -196,6 +217,18 @@ pipeline { } } + + stage('Push master-ci') { + when { + branch 'master' + } + steps { + phone_steps("eon-build", [ + ["push devel", "cd release && CI_PUSH='master-ci' ./build_devel.sh"], + ]) + } + } + } post { diff --git a/README.md b/README.md index 52c06169e..95a0897c1 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ Supported Cars | ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------| | Acura | ILX 2016-19 | AcuraWatch Plus | openpilot | 25mph1 | 25mph | | Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 12mph | -| Acura | RDX 2020-21 | All | Stock | 0mph | 3mph | +| Acura | RDX 2019-21 | All | Stock | 0mph | 3mph | | Honda | Accord 2018-20 | All | Stock | 0mph | 3mph | | Honda | Accord Hybrid 2018-20 | All | Stock | 0mph | 3mph | | Honda | Civic Hatchback 2017-21 | Honda Sensing | Stock | 0mph | 12mph | @@ -86,18 +86,18 @@ Supported Cars | Hyundai | Palisade 2020-21 | All | Stock | 0mph | 0mph | | Hyundai | Sonata 2020-21 | All | Stock | 0mph | 0mph | | Lexus | CT Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph | -| Lexus | ES 2019-20 | All | openpilot | 0mph | 0mph | -| Lexus | ES Hybrid 2018 | LSS | Stock3| 0mph | 0mph | -| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | +| Lexus | ES 2019-21 | All | openpilot | 0mph | 0mph | +| Lexus | ES Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph | +| Lexus | ES Hybrid 2019-21 | All | openpilot | 0mph | 0mph | | Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph | -| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph | | Lexus | NX 2018 | All | Stock3| 0mph | 0mph | +| Lexus | NX 2020 | All | openpilot | 0mph | 0mph | | Lexus | NX Hybrid 2018 | All | Stock3| 0mph | 0mph | | Lexus | RX 2016-18 | All | Stock3| 0mph | 0mph | | Lexus | RX 2020-21 | All | openpilot | 0mph | 0mph | | Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph | | Lexus | RX Hybrid 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Avalon 2016-18, 2020-21 | TSS-P | Stock3| 20mph1 | 0mph | +| Toyota | Avalon 2016-21 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph | | Toyota | Camry 2021 | All | openpilot | 0mph | 0mph | | Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph4 | 0mph | @@ -112,6 +112,7 @@ Supported Cars | Toyota | Highlander 2020-21 | All | openpilot | 0mph | 0mph | | Toyota | Highlander Hybrid 2017-19 | All | Stock3| 0mph | 0mph | | Toyota | Highlander Hybrid 2020-21 | All | openpilot | 0mph | 0mph | +| Toyota | Mirai 2021 | All | openpilot | 0mph | 0mph | | Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph | | Toyota | Prius 2021 | All | openpilot | 0mph | 0mph | | Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph | @@ -132,7 +133,8 @@ Community Maintained Cars and Features | Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | | ----------| ------------------------------| ------------------| -----------------| -------------------| -------------| -| Audi | A3 2015, 2017 | Prestige | Stock | 0mph | 0mph | +| Audi | A3 2014-17 | Prestige | Stock | 0mph | 0mph | +| Audi | A3 Sportback e-tron 2017-18 | Prestige | Stock | 0mph | 0mph | | Buick | Regal 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | | Cadillac | ATS 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | | Chevrolet | Malibu 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | @@ -157,45 +159,48 @@ Community Maintained Cars and Features | 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 | -| Kia | Forte 2018-19, 2021 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Forte 2018-2021 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Niro EV 2020 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph | | Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Seltos 2021 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Sorento 2018 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph | | Nissan | Altima 2020 | ProPILOT | Stock | 0mph | 0mph | | Nissan | Leaf 2018-20 | ProPILOT | Stock | 0mph | 0mph | -| Nissan | Rogue 2018-19 | ProPILOT | Stock | 0mph | 0mph | +| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph | | Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph | | SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Kodiaq 2018 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Superb 2018 | Driver Assistance | Stock | 0mph | 0mph | +| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph | | Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | | Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Forester 2019-20 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph | | Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | -| Volkswagen| e-Golf 2014, 2020 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Atlas 2018-19 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Golf 2015-19 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf Alltrack 2017-18 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Golf GTE 2016 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Golf GTI 2018-19 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Golf R 2016-19 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Golf SportsVan 2016 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Jetta 2018-21 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Passat 2016-172 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph | 1Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
2Only includes the MQB Passat sold outside of North America. The NMS Passat made in Chattanooga TN is not yet supported. -Although it's not upstream, there's a community of people getting openpilot to run on Tesla's [here](https://tinkla.us/) - Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`. To promote a car from community maintained, it must meet a few requirements. We must own one from the brand, we must sell the harness for it, has full ISO26262 in both panda and openpilot, there must be a path forward for longitudinal control, it must have AEB still enabled, and it must support fingerprinting 2.0 +Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). + Installation Instructions ------ @@ -327,7 +332,7 @@ Directory Structure . ├── cereal # The messaging spec and libs used for all logs ├── common # Library like functionality we've developed here - ├── installer/updater # Manages auto-updates of openpilot + ├── installer/updater # Manages auto-updates of NEOS ├── opendbc # Files showing how to interpret data from cars ├── panda # Code used to communicate on CAN ├── phonelibs # Libraries used on NEOS devices diff --git a/RELEASES.md b/RELEASES.md index 42b6379b4..bf764dd59 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,13 @@ +Version 0.8.4 (2021-05-17) +======================== + * Delay controls start until system is ready + * Fuzzy car identification, enabled with Community Features toggle + * Localizer optimized for increased precision and less CPU usage + * Retuned lateral control to be more aggressive when model is confident + * Toyota Mirai 2021 support + * Lexus NX 300 2020 support thanks to goesreallyfast! + * Volkswagen Atlas 2018-19 support thanks to jyoung8607! + Version 0.8.3 (2021-04-01) ======================== * New model diff --git a/SConstruct b/SConstruct index fc1ddfd66..138c6ab3f 100644 --- a/SConstruct +++ b/SConstruct @@ -13,6 +13,10 @@ AddOption('--test', action='store_true', help='build test files') +AddOption('--kaitai', + action='store_true', + help='Regenerate kaitai struct parsers') + AddOption('--asan', action='store_true', help='turn on ASAN') @@ -79,6 +83,9 @@ if arch == "aarch64" or arch == "larch64": "#phonelibs/libyuv/larch64/lib", "/usr/lib/aarch64-linux-gnu" ] + cpppath += [ + "#selfdrive/camerad/include", + ] cflags = ["-DQCOM2", "-mcpu=cortex-a57"] cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"] rpath = ["/usr/local/lib"] @@ -101,17 +108,22 @@ else: cpppath = [] if arch == "Darwin": + yuv_dir = "mac" if real_arch != "arm64" else "mac_arm64" libpath = [ - "#phonelibs/libyuv/mac/lib", - "#cereal", - "#selfdrive/common", + f"#phonelibs/libyuv/{yuv_dir}/lib", "/usr/local/lib", + "/opt/homebrew/lib", "/usr/local/opt/openssl/lib", + "/opt/homebrew/opt/openssl/lib", "/System/Library/Frameworks/OpenGL.framework/Libraries", ] cflags += ["-DGL_SILENCE_DEPRECATION"] cxxflags += ["-DGL_SILENCE_DEPRECATION"] - cpppath += ["/usr/local/opt/openssl/include"] + cpppath += [ + "/opt/homebrew/include", + "/usr/local/opt/openssl/include", + "/opt/homebrew/opt/openssl/include" + ] else: libpath = [ "#phonelibs/snpe/x86_64-linux-clang", @@ -141,6 +153,10 @@ else: ccflags = [] ldflags = [] +# no --as-needed on mac linker +if arch != "Darwin": + ldflags += ["-Wl,--as-needed"] + # change pythonpath to this lenv["PYTHONPATH"] = Dir("#").path @@ -162,7 +178,6 @@ env = Environment( CPPPATH=cpppath + [ "#", - "#selfdrive", "#phonelibs/catch2/include", "#phonelibs/bzip2", "#phonelibs/libyuv/include", @@ -177,14 +192,7 @@ env = Environment( "#phonelibs/snpe/include", "#phonelibs/nanovg", "#phonelibs/qrcode", - "#selfdrive/boardd", - "#selfdrive/common", - "#selfdrive/camerad", - "#selfdrive/camerad/include", - "#selfdrive/loggerd/include", - "#selfdrive/modeld", - "#selfdrive/sensord", - "#selfdrive/ui", + "#phonelibs", "#cereal", "#cereal/messaging", "#cereal/visionipc", @@ -270,7 +278,10 @@ if arch != "aarch64": qt_libs = [] if arch == "Darwin": - qt_env['QTDIR'] = "/usr/local/opt/qt@5" + if real_arch == "arm64": + qt_env['QTDIR'] = "/opt/homebrew/opt/qt@5" + else: + qt_env['QTDIR'] = "/usr/local/opt/qt@5" qt_dirs = [ os.path.join(qt_env['QTDIR'], "include"), ] @@ -326,12 +337,8 @@ if GetOption("clazy"): qt_env['CXX'] = 'clazy' qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0] qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks) -Export('qt_env') - -# still needed for apks -zmq = 'zmq' -Export('env', 'arch', 'real_arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY') +Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY') # cereal and messaging are shared with the system SConscript(['cereal/SConscript']) @@ -356,6 +363,28 @@ else: Export('common', 'gpucommon', 'visionipc') +# Build rednose library and ekf models + +rednose_config = { + 'generated_folder': '#selfdrive/locationd/models/generated', + 'to_build': { + 'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']), + 'car': ('#selfdrive/locationd/models/car_kf.py', True, []), + }, +} + +if arch != "aarch64": + rednose_config['to_build'].update({ + 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), + 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []), + 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []), + 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []), + 'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, []), + 'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, []), + }) + +Export('rednose_config') +SConscript(['rednose/SConscript']) # Build openpilot @@ -384,16 +413,12 @@ SConscript(['selfdrive/clocksd/SConscript']) SConscript(['selfdrive/loggerd/SConscript']) SConscript(['selfdrive/locationd/SConscript']) -SConscript(['selfdrive/locationd/models/SConscript']) SConscript(['selfdrive/sensord/SConscript']) SConscript(['selfdrive/ui/SConscript']) if arch != "Darwin": SConscript(['selfdrive/logcatd/SConscript']) -if real_arch == "x86_64": - SConscript(['tools/nui/SConscript']) - external_sconscript = GetOption('external_sconscript') if external_sconscript: SConscript([external_sconscript]) diff --git a/cereal/SConscript b/cereal/SConscript index bd2cde9c6..a671aeecc 100644 --- a/cereal/SConscript +++ b/cereal/SConscript @@ -1,4 +1,4 @@ -Import('env', 'envCython', 'arch', 'zmq', 'QCOM_REPLAY') +Import('env', 'envCython', 'arch', 'QCOM_REPLAY') import shutil @@ -40,13 +40,6 @@ messaging_objects = env.SharedObject([ messaging_lib = env.Library('messaging', messaging_objects) Depends('messaging/impl_zmq.cc', services_h) -# note, this rebuilds the deps shared, zmq is statically linked to make APK happy -# TODO: get APK to load system zmq to remove the static link -if arch == "aarch64": - zmq_static = FindFile("libzmq.a", "/usr/lib") - shared_lib_shared_lib = [zmq_static, 'm', 'stdc++', "gnustl_shared", "kj", "capnp"] - env.SharedLibrary('messaging_shared', messaging_objects, LIBS=shared_lib_shared_lib) - env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[messaging_lib, 'zmq']) Depends('messaging/bridge.cc', services_h) diff --git a/cereal/car.capnp b/cereal/car.capnp index d5726f7c5..c98928190 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -53,7 +53,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { lowSpeedLockout @31; plannerError @32; debugAlert @34; - steerTempUnavailableMute @35; + steerTempUnavailableUserOverride @35; resumeRequired @36; preDriverDistracted @37; promptDriverDistracted @38; @@ -64,7 +64,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { belowSteerSpeed @46; lowBattery @48; vehicleModelInvalid @50; - controlsFailed @51; + accFaulted @51; sensorDataInvalid @52; commIssue @53; tooDistracted @54; @@ -89,6 +89,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { startupNoCar @76; startupNoControl @77; startupMaster @78; + startupFuzzyFingerprint @97; fcw @79; steerSaturated @80; belowEngageSpeed @84; @@ -101,6 +102,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { gpsMalfunction @94; processNotRunning @95; dashcamMode @96; + controlsInitializing @98; radarCanErrorDEPRECATED @15; radarCommIssueDEPRECATED @67; @@ -351,12 +353,14 @@ struct CarControl { struct CarParams { carName @0 :Text; carFingerprint @1 :Text; + fuzzyFingerprint @55 :Bool; enableGasInterceptor @2 :Bool; enableCruise @3 :Bool; enableCamera @4 :Bool; enableDsu @5 :Bool; # driving support unit enableApgs @6 :Bool; # advanced parking guidance system + enableBsm @56 :Bool; # blind spot monitoring minEnableSpeed @7 :Float32; minSteerSpeed @8 :Float32; @@ -412,6 +416,7 @@ struct CarParams { dashcamOnly @41: Bool; transmissionType @43 :TransmissionType; carFw @44 :List(CarFw); + radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard communityFeature @46: Bool; # true if a community maintained feature is detected fingerprintSource @49: FingerprintSource; diff --git a/cereal/log.capnp b/cereal/log.capnp index aca7a9b35..41de42009 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1070,6 +1070,7 @@ struct UbloxGnss { aStatus @2 :AntennaSupervisorState; aPower @3 :AntennaPowerStatus; jamInd @4 :UInt8; + flags @5 :UInt8; enum AntennaSupervisorState { init @0; @@ -1192,9 +1193,11 @@ struct DriverMonitoringState @0xb83cda094a1da284 { struct Boot { wallTimeNanos @0 :UInt64; - lastKmsg @1 :Data; - lastPmsg @2 :Data; + pstore @4 :Map(Text, Data); launchLog @3 :Text; + + lastKmsgDEPRECATED @1 :Data; + lastPmsgDEPRECATED @2 :Data; } struct LiveParametersData { diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py index 0b1204b47..1628dd524 100644 --- a/cereal/messaging/__init__.py +++ b/cereal/messaging/__init__.py @@ -1,9 +1,11 @@ # must be build with scons from .messaging_pyx import Context, Poller, SubSocket, PubSocket # pylint: disable=no-name-in-module, import-error from .messaging_pyx import MultiplePublishersError, MessagingError # pylint: disable=no-name-in-module, import-error +import os import capnp from typing import Optional, List, Union +from collections import deque from cereal import log from cereal.services import service_list @@ -11,6 +13,9 @@ from cereal.services import service_list assert MultiplePublishersError assert MessagingError +AVG_FREQ_HISTORY = 100 +SIMULATION = "SIMULATION" in os.environ + # sec_since_boot is faster, but allow to run standalone too try: from common.realtime import sec_since_boot @@ -126,12 +131,14 @@ def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader: class SubMaster(): def __init__(self, services: List[str], poll: Optional[List[str]] = None, - ignore_alive: Optional[List[str]] = None, addr:str ="127.0.0.1"): + ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None, + addr: str = "127.0.0.1"): self.frame = -1 self.updated = {s: False for s in services} self.rcv_time = {s: 0. for s in services} self.rcv_frame = {s: 0 for s in services} self.alive = {s: False for s in services} + self.recv_dts = {s: deque([0.0] * AVG_FREQ_HISTORY, maxlen=AVG_FREQ_HISTORY) for s in services} self.sock = {} self.freq = {} self.data = {} @@ -142,10 +149,8 @@ class SubMaster(): self.non_polled_services = [s for s in services if poll is not None and len(poll) and s not in poll] - if ignore_alive is not None: - self.ignore_alive = ignore_alive - else: - self.ignore_alive = [] + self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq + self.ignore_alive = [] if ignore_alive is None else ignore_alive for s in services: if addr is not None: @@ -184,20 +189,34 @@ class SubMaster(): s = msg.which() self.updated[s] = True + + if self.rcv_time[s] > 1e-5 and self.freq[s] > 1e-5 and (s not in self.non_polled_services) \ + and (s not in self.ignore_average_freq): + self.recv_dts[s].append(cur_time - self.rcv_time[s]) + self.rcv_time[s] = cur_time self.rcv_frame[s] = self.frame self.data[s] = getattr(msg, s) self.logMonoTime[s] = msg.logMonoTime self.valid[s] = msg.valid - for s in self.data: - # arbitrary small number to avoid float comparison. If freq is 0, we can skip the check - if self.freq[s] > 1e-5: - # alive if delay is within 10x the expected frequency - self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s]) - else: + if SIMULATION: self.alive[s] = True + if not SIMULATION: + for s in self.data: + # arbitrary small number to avoid float comparison. If freq is 0, we can skip the check + if self.freq[s] > 1e-5: + # alive if delay is within 10x the expected frequency + self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s]) + + # alive if average frequency is higher than 90% of expected frequency + avg_dt = sum(self.recv_dts[s]) / AVG_FREQ_HISTORY + expected_dt = 1 / (self.freq[s] * 0.90) + self.alive[s] = self.alive[s] and (avg_dt < expected_dt) + else: + self.alive[s] = True + def all_alive(self, service_list=None) -> bool: if service_list is None: # check all service_list = self.alive.keys() @@ -223,3 +242,6 @@ class PubMaster(): if not isinstance(dat, bytes): dat = dat.to_bytes() self.sock[s].send(dat) + + def all_readers_updated(self, s: str) -> bool: + return self.sock[s].all_readers_updated() diff --git a/cereal/messaging/bridge.cc b/cereal/messaging/bridge.cc index 8e29566ca..da923835a 100644 --- a/cereal/messaging/bridge.cc +++ b/cereal/messaging/bridge.cc @@ -8,8 +8,8 @@ typedef void (*sighandler_t)(int sig); #include "services.h" -#include "impl_msgq.hpp" -#include "impl_zmq.hpp" +#include "impl_msgq.h" +#include "impl_zmq.h" void sigpipe_handler(int sig) { assert(sig == SIGPIPE); diff --git a/cereal/messaging/impl_msgq.cc b/cereal/messaging/impl_msgq.cc index e211d6abb..862c94cb3 100644 --- a/cereal/messaging/impl_msgq.cc +++ b/cereal/messaging/impl_msgq.cc @@ -6,7 +6,7 @@ #include #include "services.h" -#include "impl_msgq.hpp" +#include "impl_msgq.h" volatile sig_atomic_t msgq_do_exit = 0; @@ -196,6 +196,10 @@ int MSGQPubSocket::send(char *data, size_t size){ return msgq_msg_send(&msg, q); } +bool MSGQPubSocket::all_readers_updated() { + return msgq_all_readers_updated(q); +} + MSGQPubSocket::~MSGQPubSocket(){ if (q != NULL){ msgq_close_queue(q); diff --git a/cereal/messaging/impl_msgq.hpp b/cereal/messaging/impl_msgq.h similarity index 95% rename from cereal/messaging/impl_msgq.hpp rename to cereal/messaging/impl_msgq.h index e89994852..b67aae622 100644 --- a/cereal/messaging/impl_msgq.hpp +++ b/cereal/messaging/impl_msgq.h @@ -1,6 +1,6 @@ #pragma once -#include "messaging.hpp" -#include "msgq.hpp" +#include "messaging.h" +#include "msgq.h" #include #include @@ -48,6 +48,7 @@ public: int connect(Context *context, std::string endpoint, bool check_endpoint=true); int sendMessage(Message *message); int send(char *data, size_t size); + bool all_readers_updated(); ~MSGQPubSocket(); }; diff --git a/cereal/messaging/impl_zmq.cc b/cereal/messaging/impl_zmq.cc index 605165fcf..aeed176ae 100644 --- a/cereal/messaging/impl_zmq.cc +++ b/cereal/messaging/impl_zmq.cc @@ -7,7 +7,7 @@ #include #include "services.h" -#include "impl_zmq.hpp" +#include "impl_zmq.h" static int get_port(std::string endpoint) { int port = -1; @@ -131,6 +131,11 @@ int ZMQPubSocket::send(char *data, size_t size){ return zmq_send(sock, data, size, ZMQ_DONTWAIT); } +bool ZMQPubSocket::all_readers_updated() { + assert(false); // TODO not implemented + return false; +} + ZMQPubSocket::~ZMQPubSocket(){ zmq_close(sock); } diff --git a/cereal/messaging/impl_zmq.hpp b/cereal/messaging/impl_zmq.h similarity index 96% rename from cereal/messaging/impl_zmq.hpp rename to cereal/messaging/impl_zmq.h index f9f6deedb..bb232049e 100644 --- a/cereal/messaging/impl_zmq.hpp +++ b/cereal/messaging/impl_zmq.h @@ -1,5 +1,5 @@ #pragma once -#include "messaging.hpp" +#include "messaging.h" #include #include @@ -47,6 +47,7 @@ public: int connect(Context *context, std::string endpoint, bool check_endpoint=true); int sendMessage(Message *message); int send(char *data, size_t size); + bool all_readers_updated(); ~ZMQPubSocket(); }; diff --git a/cereal/messaging/messaging.cc b/cereal/messaging/messaging.cc index 06bc66ae1..bfa634e61 100644 --- a/cereal/messaging/messaging.cc +++ b/cereal/messaging/messaging.cc @@ -1,6 +1,6 @@ -#include "messaging.hpp" -#include "impl_zmq.hpp" -#include "impl_msgq.hpp" +#include "messaging.h" +#include "impl_zmq.h" +#include "impl_msgq.h" #ifdef __APPLE__ const bool MUST_USE_ZMQ = true; diff --git a/cereal/messaging/messaging.hpp b/cereal/messaging/messaging.h similarity index 93% rename from cereal/messaging/messaging.hpp rename to cereal/messaging/messaging.h index e461f06d2..4acdbb484 100644 --- a/cereal/messaging/messaging.hpp +++ b/cereal/messaging/messaging.h @@ -48,6 +48,7 @@ public: virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0; virtual int sendMessage(Message *message) = 0; virtual int send(char *data, size_t size) = 0; + virtual bool all_readers_updated() = 0; static PubSocket * create(); static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true); static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true); @@ -67,7 +68,8 @@ class SubMaster { public: SubMaster(const std::initializer_list &service_list, const char *address = nullptr, const std::initializer_list &ignore_alive = {}); - int update(int timeout = 1000); + void update(int timeout = 1000); + void update_msgs(uint64_t current_time, std::vector> messages); inline bool allAlive(const std::initializer_list &service_list = {}) { return all_(service_list, false, true); } inline bool allValid(const std::initializer_list &service_list = {}) { return all_(service_list, true, false); } inline bool allAliveAndValid(const std::initializer_list &service_list = {}) { return all_(service_list, true, true); } @@ -76,7 +78,10 @@ public: uint64_t frame = 0; bool updated(const char *name) const; + bool alive(const char *name) const; + bool valid(const char *name) const; uint64_t rcv_frame(const char *name) const; + uint64_t rcv_time(const char *name) const; cereal::Event::Reader &operator[](const char *name); private: diff --git a/cereal/messaging/messaging.pxd b/cereal/messaging/messaging.pxd index 8bbb2c7ab..de232da5f 100644 --- a/cereal/messaging/messaging.pxd +++ b/cereal/messaging/messaging.pxd @@ -6,7 +6,7 @@ from libcpp.vector cimport vector from libcpp cimport bool -cdef extern from "messaging.hpp": +cdef extern from "messaging.h": cdef cppclass Context: @staticmethod Context * create() @@ -31,6 +31,7 @@ cdef extern from "messaging.hpp": int connect(Context *, string) int sendMessage(Message *) int send(char *, size_t) + bool all_readers_updated() cdef cppclass Poller: @staticmethod diff --git a/cereal/messaging/messaging_pyx.pyx b/cereal/messaging/messaging_pyx.pyx index fa220c435..eed548bb8 100644 --- a/cereal/messaging/messaging_pyx.pyx +++ b/cereal/messaging/messaging_pyx.pyx @@ -149,3 +149,6 @@ cdef class PubSocket: raise MultiplePublishersError else: raise MessagingError + + def all_readers_updated(self): + return self.socket.all_readers_updated() diff --git a/cereal/messaging/msgq.cc b/cereal/messaging/msgq.cc index a51aef8e8..0c17b2657 100644 --- a/cereal/messaging/msgq.cc +++ b/cereal/messaging/msgq.cc @@ -21,7 +21,7 @@ #include -#include "msgq.hpp" +#include "msgq.h" void sigusr2_handler(int signal) { assert(signal == SIGUSR2); @@ -452,3 +452,13 @@ int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout){ return num; } + +bool msgq_all_readers_updated(msgq_queue_t *q) { + uint64_t num_readers = *q->num_readers; + for (uint64_t i = 0; i < num_readers; i++) { + if (*q->read_valids[i] && *q->write_pointer != *q->read_pointers[i]) { + return false; + } + } + return num_readers > 0; +} diff --git a/cereal/messaging/msgq.hpp b/cereal/messaging/msgq.h similarity index 97% rename from cereal/messaging/msgq.hpp rename to cereal/messaging/msgq.h index 3bead13d9..301d5d1a3 100644 --- a/cereal/messaging/msgq.hpp +++ b/cereal/messaging/msgq.h @@ -64,3 +64,5 @@ int msgq_msg_send(msgq_msg_t *msg, msgq_queue_t *q); int msgq_msg_recv(msgq_msg_t *msg, msgq_queue_t *q); int msgq_msg_ready(msgq_queue_t * q); int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout); + +bool msgq_all_readers_updated(msgq_queue_t *q); diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc index 1240009fa..b10133e43 100644 --- a/cereal/messaging/socketmaster.cc +++ b/cereal/messaging/socketmaster.cc @@ -1,7 +1,12 @@ -#include #include -#include "messaging.hpp" +#include +#include +#include + #include "services.h" +#include "messaging.h" + +const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1"); static inline uint64_t nanos_since_boot() { struct timespec t; @@ -35,7 +40,7 @@ struct SubMaster::SubMessage { std::string name; SubSocket *socket = nullptr; int freq = 0; - bool updated = false, alive = false, valid = false, ignore_alive; + bool updated = false, alive = false, valid = true, ignore_alive; uint64_t rcv_time = 0, rcv_frame = 0; void *allocated_msg_reader = nullptr; capnp::FlatArrayMessageReader *msg_reader = nullptr; @@ -53,6 +58,7 @@ SubMaster::SubMaster(const std::initializer_list &service_list, co assert(socket != 0); poller_->registerSocket(socket); SubMessage *m = new SubMessage{ + .name = name, .socket = socket, .freq = serv->frequency, .ignore_alive = inList(ignore_alive, name), @@ -62,37 +68,54 @@ SubMaster::SubMaster(const std::initializer_list &service_list, co } } -int SubMaster::update(int timeout) { - if (++frame == UINT64_MAX) frame = 1; +void SubMaster::update(int timeout) { for (auto &kv : messages_) kv.second->updated = false; - int updated = 0; auto sockets = poller_->poll(timeout); uint64_t current_time = nanos_since_boot(); + + std::vector> messages; + for (auto s : sockets) { Message *msg = s->receive(true); if (msg == nullptr) continue; SubMessage *m = messages_.at(s); + if (m->msg_reader) { m->msg_reader->~FlatArrayMessageReader(); } m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.align(msg)); delete msg; - m->event = m->msg_reader->getRoot(); + messages.push_back({m->name, m->msg_reader->getRoot()}); + } + + update_msgs(current_time, messages); +} + +void SubMaster::update_msgs(uint64_t current_time, std::vector> messages){ + if (++frame == UINT64_MAX) frame = 1; + + for(auto &kv : messages) { + auto m_find = services_.find(kv.first); + if (m_find == services_.end()){ + continue; + } + SubMessage *m = m_find->second; + m->event = kv.second; m->updated = true; m->rcv_time = current_time; m->rcv_frame = frame; m->valid = m->event.getValid(); - - ++updated; + if (SIMULATION) m->alive = true; } - for (auto &kv : messages_) { - SubMessage *m = kv.second; - m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq)); + if (!SIMULATION) { + for (auto &kv : messages_) { + SubMessage *m = kv.second; + m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq)); + } } - return updated; } bool SubMaster::all_(const std::initializer_list &service_list, bool valid, bool alive) { @@ -100,7 +123,7 @@ bool SubMaster::all_(const std::initializer_list &service_list, bo for (auto &kv : messages_) { SubMessage *m = kv.second; if (service_list.size() == 0 || inList(service_list, m->name.c_str())) { - found += (!valid || m->valid) && (!alive || (m->alive && !m->ignore_alive)); + found += (!valid || m->valid) && (!alive || (m->alive || m->ignore_alive)); } } return service_list.size() == 0 ? found == messages_.size() : found == service_list.size(); @@ -123,10 +146,22 @@ bool SubMaster::updated(const char *name) const { return services_.at(name)->updated; } +bool SubMaster::alive(const char *name) const { + return services_.at(name)->alive; +} + +bool SubMaster::valid(const char *name) const { + return services_.at(name)->valid; +} + uint64_t SubMaster::rcv_frame(const char *name) const { return services_.at(name)->rcv_frame; } +uint64_t SubMaster::rcv_time(const char *name) const { + return services_.at(name)->rcv_time; +} + cereal::Event::Reader &SubMaster::operator[](const char *name) { return services_.at(name)->event; }; diff --git a/cereal/services.py b/cereal/services.py old mode 100755 new mode 100644 index 98f80acda..a30436bcd --- a/cereal/services.py +++ b/cereal/services.py @@ -3,6 +3,14 @@ import os from typing import Optional EON = os.path.isfile('/EON') +RESERVED_PORT = 8022 # sshd +STARTING_PORT = 8001 + + +def new_port(port: int): + port += STARTING_PORT + return port + 1 if port >= RESERVED_PORT else port + class Service: def __init__(self, port: int, should_log: bool, frequency: float, decimation: Optional[int] = None): @@ -11,55 +19,58 @@ class Service: self.frequency = frequency self.decimation = decimation -service_list = { - "roadCameraState": Service(8002, True, 20., 1), - "sensorEvents": Service(8003, True, 100., 100), - "gpsNMEA": Service(8004, True, 9.), - "deviceState": Service(8005, True, 2., 1), - "can": Service(8006, True, 100.), - "controlsState": Service(8007, True, 100., 100), - "features": Service(8010, True, 0.), - "pandaState": Service(8011, True, 2., 1), - "radarState": Service(8012, True, 20., 5), - "roadEncodeIdx": Service(8015, True, 20., 1), - "liveTracks": Service(8016, True, 20.), - "sendcan": Service(8017, True, 100.), - "logMessage": Service(8018, True, 0.), - "liveCalibration": Service(8019, True, 4., 4), - "androidLog": Service(8020, True, 0., 1), - "carState": Service(8021, True, 100., 10), - "carControl": Service(8023, True, 100., 10), - "longitudinalPlan": Service(8024, True, 20., 2), - "liveLocation": Service(8025, True, 0., 1), - "procLog": Service(8031, True, 0.5), - "gpsLocationExternal": Service(8032, True, 10., 1), - "ubloxGnss": Service(8033, True, 10.), - "clocks": Service(8034, True, 1., 1), - "liveMpc": Service(8035, False, 20.), - "liveLongitudinalMpc": Service(8036, False, 20.), - "ubloxRaw": Service(8042, True, 20.), - "liveLocationKalman": Service(8054, True, 20., 2), - "uiLayoutState": Service(8060, True, 0.), - "liveParameters": Service(8064, True, 20., 2), - "cameraOdometry": Service(8066, True, 20., 5), - "lateralPlan": Service(8067, True, 20., 2), - "thumbnail": Service(8069, True, 0.2, 1), - "carEvents": Service(8070, True, 1., 1), - "carParams": Service(8071, True, 0.02, 1), - "driverCameraState": Service(8072, True, 10. if EON else 20., 1), - "driverEncodeIdx": Service(8061, True, 10. if EON else 20., 1), - "driverState": Service(8063, True, 10. if EON else 20., 1), - "driverMonitoringState": Service(8073, True, 10. if EON else 20., 1), - "offroadLayout": Service(8074, False, 0.), - "wideRoadEncodeIdx": Service(8075, True, 20., 1), - "wideRoadCameraState": Service(8076, True, 20., 1), - "modelV2": Service(8077, True, 20., 20), - "managerState": Service(8078, True, 2., 1), - "testModel": Service(8040, False, 0.), - "testLiveLocation": Service(8045, False, 0.), - "testJoystick": Service(8056, False, 0.), +services = { + "roadCameraState": (True, 20., 1), # should_log, frequency, decimation (optional) + "sensorEvents": (True, 100., 100), + "gpsNMEA": (True, 9.), + "deviceState": (True, 2., 1), + "can": (True, 100.), + "controlsState": (True, 100., 100), + "features": (True, 0.), + "pandaState": (True, 2., 1), + "radarState": (True, 20., 5), + "roadEncodeIdx": (True, 20., 1), + "liveTracks": (True, 20.), + "sendcan": (True, 100.), + "logMessage": (True, 0.), + "liveCalibration": (True, 4., 4), + "androidLog": (True, 0., 1), + "carState": (True, 100., 10), + "carControl": (True, 100., 10), + "longitudinalPlan": (True, 20., 2), + "liveLocation": (True, 0., 1), + "procLog": (True, 0.5), + "gpsLocationExternal": (True, 10., 1), + "ubloxGnss": (True, 10.), + "clocks": (True, 1., 1), + "liveMpc": (False, 20.), + "liveLongitudinalMpc": (False, 20.), + "ubloxRaw": (True, 20.), + "liveLocationKalman": (True, 20., 2), + "uiLayoutState": (True, 0.), + "liveParameters": (True, 20., 2), + "cameraOdometry": (True, 20., 5), + "lateralPlan": (True, 20., 2), + "thumbnail": (True, 0.2, 1), + "carEvents": (True, 1., 1), + "carParams": (True, 0.02, 1), + "driverCameraState": (True, 10. if EON else 20., 1), + "driverEncodeIdx": (True, 10. if EON else 20., 1), + "driverState": (True, 10. if EON else 20., 1), + "driverMonitoringState": (True, 10. if EON else 20., 1), + "offroadLayout": (False, 0.), + "wideRoadEncodeIdx": (True, 20., 1), + "wideRoadCameraState": (True, 20., 1), + "modelV2": (True, 20., 20), + "managerState": (True, 2., 1), + + "testModel": (False, 0.), + "testLiveLocation": (False, 0.), + "testJoystick": (False, 0.), } +service_list = {name: Service(new_port(idx), *vals) for # type: ignore + idx, (name, vals) in enumerate(services.items())} def build_header(): diff --git a/cereal/visionipc/visionipc_client.h b/cereal/visionipc/visionipc_client.h index 469976239..d2c0085ad 100644 --- a/cereal/visionipc/visionipc_client.h +++ b/cereal/visionipc/visionipc_client.h @@ -3,7 +3,7 @@ #include #include -#include "messaging.hpp" +#include "messaging.h" #include "visionipc.h" #include "visionbuf.h" diff --git a/cereal/visionipc/visionipc_server.cc b/cereal/visionipc/visionipc_server.cc index 7c64cd839..f4020133d 100644 --- a/cereal/visionipc/visionipc_server.cc +++ b/cereal/visionipc/visionipc_server.cc @@ -7,7 +7,7 @@ #include #include -#include "messaging.hpp" +#include "messaging.h" #include "ipc.h" #include "visionipc_server.h" diff --git a/cereal/visionipc/visionipc_server.h b/cereal/visionipc/visionipc_server.h index 4472acf61..05c517b16 100644 --- a/cereal/visionipc/visionipc_server.h +++ b/cereal/visionipc/visionipc_server.h @@ -5,7 +5,7 @@ #include #include -#include "messaging.hpp" +#include "messaging.h" #include "visionipc.h" #include "visionbuf.h" diff --git a/common/SConscript b/common/SConscript index 542e10d43..a41fc8b7e 100644 --- a/common/SConscript +++ b/common/SConscript @@ -1,4 +1,4 @@ -Import('envCython') +Import('envCython', 'common') envCython.Program('clock.so', 'clock.pyx') -envCython.Program('params_pyx.so', 'params_pyx.pyx') +envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [common, 'zmq']) diff --git a/common/params.py b/common/params.py index 0acb61fa6..d0a37cce1 100644 --- a/common/params.py +++ b/common/params.py @@ -1,5 +1,6 @@ -from common.params_pyx import Params, UnknownKeyName, put_nonblocking # pylint: disable=no-name-in-module, import-error +from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking # pylint: disable=no-name-in-module, import-error assert Params +assert ParamKeyType assert UnknownKeyName assert put_nonblocking diff --git a/common/params_pxd.pxd b/common/params_pxd.pxd index 9faa117cb..b31a5ab70 100644 --- a/common/params_pxd.pxd +++ b/common/params_pxd.pxd @@ -8,9 +8,20 @@ cdef extern from "selfdrive/common/util.cc": pass cdef extern from "selfdrive/common/params.h": + cpdef enum ParamKeyType: + PERSISTENT + CLEAR_ON_MANAGER_START + CLEAR_ON_PANDA_DISCONNECT + CLEAR_ON_IGNITION + ALL + cdef cppclass Params: Params(bool) Params(string) string get(string, bool) nogil - int delete_db_value(string) - int write_db_value(string, string) + bool getBool(string) + int remove(string) + int put(string, string) + int putBool(string, bool) + bool checkKey(string) + void clearAll(ParamKeyType) diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 3d366b08f..dab8d20fa 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -2,88 +2,19 @@ # cython: language_level = 3 from libcpp cimport bool from libcpp.string cimport string -from common.params_pxd cimport Params as c_Params +from common.params_pxd cimport Params as c_Params, ParamKeyType as c_ParamKeyType 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"ApiCache_DriveStats": [TxType.PERSISTENT], - b"ApiCache_Device": [TxType.PERSISTENT], - b"ApiCache_Owner": [TxType.PERSISTENT], - 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"EndToEndToggle": [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"GitDiff": [TxType.PERSISTENT], - b"GitBranch": [TxType.PERSISTENT], - b"GitCommit": [TxType.PERSISTENT], - b"GitRemote": [TxType.PERSISTENT], - b"GithubSshKeys": [TxType.PERSISTENT], - b"GithubUsername": [TxType.PERSISTENT], - b"HardwareSerial": [TxType.PERSISTENT], - b"HasAcceptedTerms": [TxType.PERSISTENT], - b"HasCompletedSetup": [TxType.PERSISTENT], - b"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START], - b"IMEI": [TxType.PERSISTENT], - 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"LastGPSPosition": [TxType.PERSISTENT], - b"LastUpdateException": [TxType.PERSISTENT], - b"LastUpdateTime": [TxType.PERSISTENT], - b"LiveParameters": [TxType.PERSISTENT], - b"OpenpilotEnabledToggle": [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"RecordFrontLock": [TxType.PERSISTENT], # for the internal fleet - b"ReleaseNotes": [TxType.PERSISTENT], - b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], - b"SubscriberInfo": [TxType.PERSISTENT], - b"SshEnabled": [TxType.PERSISTENT], - b"TermsVersion": [TxType.PERSISTENT], - b"Timezone": [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"VisionRadarToggle": [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], - b"Offroad_HardwareUnsupported": [TxType.CLEAR_ON_MANAGER_START], - b"ForcePowerDown": [TxType.CLEAR_ON_MANAGER_START], -} +cdef class ParamKeyType: + PERSISTENT = c_ParamKeyType.PERSISTENT + CLEAR_ON_MANAGER_START = c_ParamKeyType.CLEAR_ON_MANAGER_START + CLEAR_ON_PANDA_DISCONNECT = c_ParamKeyType.CLEAR_ON_PANDA_DISCONNECT + CLEAR_ON_IGNITION = c_ParamKeyType.CLEAR_ON_IGNITION + ALL = c_ParamKeyType.ALL def ensure_bytes(v): if isinstance(v, str): @@ -108,23 +39,21 @@ cdef class Params: 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) + if tx_type is None: + tx_type = ParamKeyType.ALL - def manager_start(self): - self.clear_all(TxType.CLEAR_ON_MANAGER_START) + self.p.clearAll(tx_type) - def panda_disconnect(self): - self.clear_all(TxType.CLEAR_ON_PANDA_DISCONNECT) - - def get(self, key, block=False, encoding=None): + def check_key(self, key): key = ensure_bytes(key) - if key not in keys: + if not self.p.checkKey(key): raise UnknownKeyName(key) - cdef string k = key + return key + + def get(self, key, block=False, encoding=None): + cdef string k = self.check_key(key) cdef bool b = block cdef string val @@ -144,6 +73,10 @@ cdef class Params: else: return val + def get_bool(self, key): + cdef string k = self.check_key(key) + return self.p.getBool(k) + def put(self, key, dat): """ Warning: This function blocks until the param is written to disk! @@ -151,23 +84,24 @@ cdef class Params: 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) + cdef string k = self.check_key(key) dat = ensure_bytes(dat) + self.p.put(k, dat) - if key not in keys: - raise UnknownKeyName(key) - - self.p.write_db_value(key, dat) + def put_bool(self, key, val): + cdef string k = self.check_key(key) + self.p.putBool(k, val) def delete(self, key): - key = ensure_bytes(key) - self.p.delete_db_value(key) + cdef string k = self.check_key(key) + self.p.remove(k) def put_nonblocking(key, val, d=None): def f(key, val): params = Params(d) - params.put(key, val) + cdef string k = ensure_bytes(key) + params.put(k, val) t = threading.Thread(target=f, args=(key, val)) t.start() diff --git a/common/transformations/coordinates.cc b/common/transformations/coordinates.cc index 8a1aa0ad7..b729ac3d8 100644 --- a/common/transformations/coordinates.cc +++ b/common/transformations/coordinates.cc @@ -6,8 +6,6 @@ #include "coordinates.hpp" -#define DEG2RAD(x) ((x) * M_PI / 180.0) -#define RAD2DEG(x) ((x) * 180.0 / M_PI) double a = 6378137; // lgtm [cpp/short-global-name] diff --git a/common/transformations/coordinates.hpp b/common/transformations/coordinates.hpp index d8beb59ea..f5ba0d3fe 100644 --- a/common/transformations/coordinates.hpp +++ b/common/transformations/coordinates.hpp @@ -1,5 +1,8 @@ #pragma once +#define DEG2RAD(x) ((x) * M_PI / 180.0) +#define RAD2DEG(x) ((x) * 180.0 / M_PI) + struct ECEF { double x, y, z; Eigen::Vector3d to_vector(){ @@ -9,6 +12,9 @@ struct ECEF { struct NED { double n, e, d; + Eigen::Vector3d to_vector(){ + return Eigen::Vector3d(n, e, d); + } }; struct Geodetic { diff --git a/common/transformations/orientation.cc b/common/transformations/orientation.cc index 8d1a304a2..7909c0aff 100644 --- a/common/transformations/orientation.cc +++ b/common/transformations/orientation.cc @@ -30,7 +30,8 @@ Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){ // Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0); // return {euler(2), euler(1), euler(0)}; double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y())); - double theta = asin(2 * (quat.w() * quat.y() - quat.z() * quat.x())); + double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0); + double theta = asin(asin_arg_clipped); double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z())); return {gamma, theta, psi}; } diff --git a/installer/updater/updater b/installer/updater/updater index 729de8434..d833cb782 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 6dd8d0289..b2f9dd45b 100644 --- a/installer/updater/updater.cc +++ b/installer/updater/updater.cc @@ -1,37 +1,33 @@ +#include +#include +#include + +#include #include #include #include -#include - -#include -#include -#include - -#include -#include #include #include -#include #include +#include +#include +#include #include #include #include - -#include #include #include - +#include #include "nanovg.h" #define NANOVG_GLES3_IMPLEMENTATION +#include "json11.hpp" #include "nanovg_gl.h" #include "nanovg_gl_utils.h" -#include "json11.hpp" - -#include "common/framebuffer.h" -#include "common/touch.h" -#include "common/util.h" +#include "selfdrive/common/framebuffer.h" +#include "selfdrive/common/touch.h" +#include "selfdrive/common/util.h" #define USER_AGENT "NEOSUpdater-0.2" @@ -250,7 +246,12 @@ struct Updater { b_y = 720; b_h = 220; - state = CONFIRMATION; + if (download_stage(true)) { + state = RUNNING; + update_thread_handle = std::thread(&Updater::run_stages, this); + } else { + state = CONFIRMATION; + } } int download_file_xferinfo(curl_off_t dltotal, curl_off_t dlno, @@ -351,11 +352,15 @@ struct Updater { state = RUNNING; } - std::string download(std::string url, std::string hash, std::string name) { + std::string download(std::string url, std::string hash, std::string name, bool dry_run) { std::string out_fn = UPDATE_DIR "/" + util::base_name(url); - // start or resume downloading if hash doesn't match std::string fn_hash = sha256_file(out_fn); + if (dry_run) { + return (hash.compare(fn_hash) != 0) ? "" : out_fn; + } + + // start or resume downloading if hash doesn't match if (hash.compare(fn_hash) != 0) { set_progress("Downloading " + name + "..."); bool r = download_file(url, out_fn); @@ -377,14 +382,14 @@ struct Updater { return out_fn; } - bool download_stage() { + bool download_stage(bool dry_run = false) { curl = curl_easy_init(); assert(curl); // ** quick checks before download ** if (!check_space()) { - set_error("2GB of free space required to update"); + if (!dry_run) set_error("2GB of free space required to update"); return false; } @@ -432,7 +437,7 @@ struct Updater { printf("existing recovery hash: %s\n", existing_recovery_hash.c_str()); if (existing_recovery_hash != recovery_hash) { - recovery_fn = download(recovery_url, recovery_hash, "recovery"); + recovery_fn = download(recovery_url, recovery_hash, "recovery", dry_run); if (recovery_fn.empty()) { // error'd return false; @@ -441,7 +446,7 @@ struct Updater { } // ** handle ota download ** - ota_fn = download(ota_url, ota_hash, "update"); + ota_fn = download(ota_url, ota_hash, "update", dry_run); if (ota_fn.empty()) { //error'd return false; diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index c9605cbab..3f9ce9977 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -165,6 +165,9 @@ function launch { # Remove orphaned git lock if it exists on boot [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock + # Pull time from panda + $DIR/selfdrive/boardd/set_time.py + # Check to see if there's a valid overlay-based update available. Conditions # are as follows: # diff --git a/launch_env.sh b/launch_env.sh index da81aa412..a8ab9ad65 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -11,7 +11,7 @@ if [ -z "$REQUIRED_NEOS_VERSION" ]; then fi if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="0.11" + export AGNOS_VERSION="0.14" fi if [ -z "$PASSIVE" ]; then diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc index 9e23311c0..7e64199a1 100644 --- a/opendbc/can/parser.cc +++ b/opendbc/can/parser.cc @@ -272,6 +272,8 @@ void CANParser::UpdateValid(uint64_t sec) { if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { if (state.seen > 0) { DEBUG("0x%X TIMEOUT\n", state.address); + } else { + DEBUG("0x%X MISSING\n", state.address); } can_valid = false; } diff --git a/opendbc/can/parser_pyx.pyx b/opendbc/can/parser_pyx.pyx index 39f00a573..20a96a6ce 100644 --- a/opendbc/can/parser_pyx.pyx +++ b/opendbc/can/parser_pyx.pyx @@ -33,14 +33,14 @@ cdef class CANParser: bool can_valid int can_invalid_cnt - def __init__(self, dbc_name, signals, checks=None, bus=0): + def __init__(self, dbc_name, signals, checks=None, bus=0, enforce_checks=True): if checks is None: checks = [] self.can_valid = True self.dbc_name = dbc_name self.dbc = dbc_lookup(dbc_name) if not self.dbc: - raise RuntimeError("Can't lookup" + dbc_name) + raise RuntimeError(f"Can't find DBC: {dbc_name}") self.vl = {} self.ts = {} @@ -74,6 +74,14 @@ cdef class CANParser: c = (self.msg_name_to_address[name], c[1]) checks[i] = c + if enforce_checks: + checked_addrs = {c[0] for c in checks} + signal_addrs = {s[1] for s in signals} + unchecked = signal_addrs - checked_addrs + if len(unchecked): + err_msg = ', '.join(f"{self.address_to_msg_name[addr].decode()} ({hex(addr)})" for addr in unchecked) + raise RuntimeError(f"Unchecked addrs: {err_msg}") + cdef vector[SignalParseOptions] signal_options_v cdef SignalParseOptions spo for sig_name, sig_address, sig_default in signals: @@ -108,7 +116,6 @@ cdef class CANParser: self.can_invalid_cnt = 0 self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT - for cv in can_values: # Cast char * directly to unicode name = self.address_to_msg_name[cv.address].c_str() @@ -149,8 +156,8 @@ cdef class CANDefine(): self.dbc_name = dbc_name self.dbc = dbc_lookup(dbc_name) if not self.dbc: - raise RuntimeError("Can't lookup" + dbc_name) - + raise RuntimeError(f"Can't find DBC: '{dbc_name}'") + num_vals = self.dbc[0].num_vals address_to_msg_name = {} diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc index 0d6663325..5fe30ac29 100644 --- a/opendbc/honda_civic_touring_2016_can_generated.dbc +++ b/opendbc/honda_civic_touring_2016_can_generated.dbc @@ -346,6 +346,7 @@ BO_ 862 HIGHBEAM_CONTROL: 8 ADAS SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX BO_ 884 STALK_STATUS: 8 XXX + SG_ DASHBOARD_ALERT : 39|8@0+ (1,0) [0|255] "" EON 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 @@ -389,6 +390,7 @@ 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_ 884 DASHBOARD_ALERT 0 "none" 51 "acc_problem" 55 "cmbs_problem" 75 "key_not_detected" 79 "fasten_seatbelt" 111 "lkas_problem" 131 "brake_system_problem" 132 "brake_hold_problem" 139 "tbd" 161 "door_open" VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ; VAL_ 927 ACC_ALERTS 29 "esp_active_acc_canceled" 10 "b_pedal_applied" 9 "speed_too_low" 8 "speed_too_high" 7 "p_brake_applied" 6 "gear_no_d" 5 "seatbelt" 4 "too_steep_downhill" 3 "too_steep_uphill" 2 "too_close" 1 "no_vehicle_ahead" ; diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc index 30646e552..20dbdd7d7 100644 --- a/opendbc/lexus_ct200h_2018_pt_generated.dbc +++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc index 9f02ded0f..8956c429b 100644 --- a/opendbc/lexus_is_2018_pt_generated.dbc +++ b/opendbc/lexus_is_2018_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/lexus_nx300_2018_pt_generated.dbc b/opendbc/lexus_nx300_2018_pt_generated.dbc index 0f4ddab95..43f059e6b 100644 --- a/opendbc/lexus_nx300_2018_pt_generated.dbc +++ b/opendbc/lexus_nx300_2018_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc index 1f232541f..6121973e2 100644 --- a/opendbc/lexus_nx300h_2018_pt_generated.dbc +++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc index d96711e42..e55caa5c5 100644 --- a/opendbc/lexus_rx_350_2016_pt_generated.dbc +++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc index 3d1252dc2..516fd5bef 100644 --- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc index f33f079f8..f1f520eb2 100644 --- a/opendbc/toyota_avalon_2017_pt_generated.dbc +++ b/opendbc/toyota_avalon_2017_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc index 17ef5922d..52b999cfb 100644 --- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc index 2f2dc8bdb..229393698 100644 --- a/opendbc/toyota_corolla_2017_pt_generated.dbc +++ b/opendbc/toyota_corolla_2017_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc index 6cf6d27cb..81a1caf90 100644 --- a/opendbc/toyota_highlander_2017_pt_generated.dbc +++ b/opendbc/toyota_highlander_2017_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc index 2d498cf1b..fbf531dd4 100644 --- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc index 09ed0690f..9afca5350 100644 --- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc +++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc @@ -1,7 +1,20 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _toyota_nodsu_bsm.dbc starts here"; +CM_ "Imported file _toyota_nodsu_common.dbc starts here"; +BO_ 401 STEERING_LTA: 8 XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX + SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX + SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX + SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX + SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX + SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX + SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX + BO_ 1014 BSM: 8 XXX SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX @@ -275,6 +288,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -285,6 +299,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -381,7 +399,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index bf2f02e4f..2bfabcc23 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -1,7 +1,20 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _toyota_nodsu_bsm.dbc starts here"; +CM_ "Imported file _toyota_nodsu_common.dbc starts here"; +BO_ 401 STEERING_LTA: 8 XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX + SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX + SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX + SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX + SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX + SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX + SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX + BO_ 1014 BSM: 8 XXX SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX @@ -275,6 +288,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -285,6 +299,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -381,7 +399,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; @@ -392,19 +410,6 @@ CM_ "toyota_nodsu_pt.dbc starts here"; -BO_ 401 STEERING_LTA: 8 XXX - SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX - SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX - SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX - SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX - SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX - SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX - SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX - SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX - SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX - SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX - SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX - BO_ 550 BRAKE_MODULE: 8 XXX SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc index e44e776f5..40fa754f4 100644 --- a/opendbc/toyota_prius_2017_pt_generated.dbc +++ b/opendbc/toyota_prius_2017_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc index 703726026..724aae6bb 100644 --- a/opendbc/toyota_rav4_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_2017_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc index 99f724ea9..0b36f13dc 100644 --- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc index 06b9388bc..3a7ad53c0 100644 --- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc +++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc @@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX BO_ 1568 SEATS_DOORS: 8 XXX SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX @@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX BO_ 1570 LIGHT_STALK: 8 SCM SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX BO_ 1161 RSA1: 8 FCM SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX @@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; diff --git a/panda/.gitignore b/panda/.gitignore index 2cd2eb8e5..3a8ead1e0 100644 --- a/panda/.gitignore +++ b/panda/.gitignore @@ -3,6 +3,7 @@ .*.swo *.o *.so +*.os *.d *.dump a.out diff --git a/panda/board/SConscript b/panda/board/SConscript index 902498ace..167da9e24 100644 --- a/panda/board/SConscript +++ b/panda/board/SConscript @@ -136,6 +136,11 @@ panda_env = Environment( # Common autogenerated includes version = f'const uint8_t gitversion[] = "{get_version(BUILDER, BUILD_TYPE)}";' gitversion = panda_env.Textfile("obj/gitversion.h", [version, ""]) +Ignore('bootstub.o', gitversion) +Requires('bootstub.o', gitversion) +Ignore('main.o', gitversion) +Requires('main.o', gitversion) + certs = [get_key_header(n) for n in ["debug", "release"]] certheader = panda_env.Textfile("obj/cert.h", certs + [""]) @@ -144,13 +149,11 @@ startup = panda_env.Object(STARTUP_FILE) # Bootstub crypto = ["../crypto/rsa.c", "../crypto/sha.c"] bootstub_elf = panda_env.Program(f"obj/bootstub.{PROJECT}.elf", [startup] + crypto + ["bootstub.c"]) -Requires(bootstub_elf, gitversion) bootstub_bin = panda_env.Objcopy(f"obj/bootstub.{PROJECT}.bin", bootstub_elf) # Build main main_elf = panda_env.Program(f"obj/{PROJECT}.elf", [startup, MAIN], LINKFLAGS=["-Wl,--section-start,.isr_vector=0x8004000"] + flags) -Requires(main_elf, gitversion) main_bin = panda_env.Objcopy(f"obj/{PROJECT}.bin", main_elf) # Sign main diff --git a/panda/board/gpio.h b/panda/board/gpio.h index b50de50c3..01d3b8a3c 100644 --- a/panda/board/gpio.h +++ b/panda/board/gpio.h @@ -12,6 +12,7 @@ void jump_to_bootloader(void) { void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)0x1fff0004)); // jump to bootloader + enable_interrupts(); bootloader(); // reset on exit @@ -21,14 +22,12 @@ void jump_to_bootloader(void) { void early(void) { // Reset global critical depth + disable_interrupts(); global_critical_depth = 0; // Init register and interrupt tables init_registers(); - // neccesary for DFU flashing on a non-power cycled white panda - enable_interrupts(); - // after it's been in the bootloader, things are initted differently, so we reset if ((enter_bootloader_mode != BOOT_NORMAL) && (enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC) && diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index a4a6d4c2a..39966e0dc 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -17,11 +17,9 @@ const CanMsg HYUNDAI_TX_MSGS[] = { // {1186, 0, 8} // 4a2SCC, Bus 0 }; -// TODO: missing checksum for wheel speeds message,worst failure case is -// wheel speeds stuck at 0 and we don't disengage on brake press AddrCheckStruct hyundai_rx_checks[] = { {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}}}, - {.msg = {{902, 0, 8, .check_checksum = false, .max_counter = 15U, .expected_timestep = 10000U}}}, + {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}}}, {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}}}, {.msg = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}}}, }; @@ -63,6 +61,8 @@ static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { uint8_t chksum; if (addr == 608) { chksum = GET_BYTE(to_push, 7) & 0xF; + } else if (addr == 902) { + chksum = ((GET_BYTE(to_push, 7) >> 6) << 2) | (GET_BYTE(to_push, 5) >> 6); } else if (addr == 916) { chksum = GET_BYTE(to_push, 6) & 0xF; } else if (addr == 1057) { @@ -77,15 +77,36 @@ static uint8_t hyundai_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); uint8_t chksum = 0; - // same algorithm, but checksum is in a different place - for (int i = 0; i < 8; i++) { - uint8_t b = GET_BYTE(to_push, i); - if (((addr == 608) && (i == 7)) || ((addr == 916) && (i == 6)) || ((addr == 1057) && (i == 7))) { - b &= (addr == 1057) ? 0x0FU : 0xF0U; // remove checksum + if (addr == 902) { + // count the bits + for (int i = 0; i < 8; i++) { + uint8_t b = GET_BYTE(to_push, i); + for (int j = 0; j < 8; j++) { + uint8_t bit = 0; + // exclude checksum and counter + if (((i != 1) || (j < 6)) && ((i != 3) || (j < 6)) && ((i != 5) || (j < 6)) && ((i != 7) || (j < 6))) { + bit = (b >> (uint8_t)j) & 1U; + } + chksum += bit; + } } - chksum += (b % 16U) + (b / 16U); + chksum = (chksum ^ 9U) & 15U; + } else { + // sum of nibbles + for (int i = 0; i < 8; i++) { + if ((addr == 916) && (i == 7)) { + continue; // exclude + } + uint8_t b = GET_BYTE(to_push, i); + if (((addr == 608) && (i == 7)) || ((addr == 916) && (i == 6)) || ((addr == 1057) && (i == 7))) { + b &= (addr == 1057) ? 0x0FU : 0xF0U; // remove checksum + } + chksum += (b % 16U) + (b / 16U); + } + chksum = (16U - (chksum % 16U)) % 16U; } - return (16U - (chksum % 16U)) % 16U; + + return chksum; } static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 2ffe58b5f..241e5798f 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -30,9 +30,9 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 845; #define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2) // avg between 2 tracks const CanMsg TOYOTA_TX_MSGS[] = {{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, // DSU bus 0 - {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, // DSU bus 1 - {0x2E4, 0, 5}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, // LKAS + ACC - {0x200, 0, 6}}; // interceptor + {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, // DSU bus 1 + {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, // LKAS + ACC + {0x200, 0, 6}}; // interceptor AddrCheckStruct toyota_rx_checks[] = { {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .expected_timestep = 12000U}}}, @@ -180,6 +180,21 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } } + // LTA steering check + // only sent to prevent dash errors, no actuation is accepted + if (addr == 0x191) { + // check the STEER_REQUEST, STEER_REQUEST_2, and STEER_ANGLE_CMD signals + bool lta_request = (GET_BYTE(to_send, 0) & 1) != 0; + bool lta_request2 = ((GET_BYTE(to_send, 3) >> 1) & 1) != 0; + int lta_angle = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2); + lta_angle = to_signed(lta_angle, 16); + + // block LTA msgs with actuation requests + if (lta_request || lta_request2 || (lta_angle != 0)) { + tx = 0; + } + } + // STEER: safety check on bytes 2-3 if (addr == 0x2E4) { int desired_torque = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2); diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 1c02f751c..13d543f8c 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -179,7 +179,7 @@ class Panda(object): self.bootstub = device.getProductID() == 0xddee self.legacy = (device.getbcdDevice() != 0x2300) self._handle = device.open() - if sys.platform not in ["win32", "cygwin", "msys"]: + if sys.platform not in ["win32", "cygwin", "msys", "darwin"]: self._handle.setAutoDetachKernelDriver(True) if claim: self._handle.claimInterface(0) diff --git a/panda/python/uds.py b/panda/python/uds.py index 243dd5acf..5106e6de8 100644 --- a/panda/python/uds.py +++ b/panda/python/uds.py @@ -240,7 +240,7 @@ _negative_response_codes = { 0x31: 'request out of range', 0x33: 'security access denied', 0x35: 'invalid key', - 0x36: 'exceed numebr of attempts', + 0x36: 'exceed number of attempts', 0x37: 'required time delay not expired', 0x70: 'upload download not accepted', 0x71: 'transfer data suspended', diff --git a/phonelibs/SConscript b/phonelibs/SConscript index a23b02a8e..5a7a62ae5 100644 --- a/phonelibs/SConscript +++ b/phonelibs/SConscript @@ -2,3 +2,5 @@ Import('env') env.Library('json11', ['json11/json11.cpp']) env.Append(CPPPATH=[Dir('json11')]) + +env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) diff --git a/phonelibs/kaitai/custom_decoder.h b/phonelibs/kaitai/custom_decoder.h new file mode 100644 index 000000000..6da7f5fd2 --- /dev/null +++ b/phonelibs/kaitai/custom_decoder.h @@ -0,0 +1,16 @@ +#ifndef KAITAI_CUSTOM_DECODER_H +#define KAITAI_CUSTOM_DECODER_H + +#include + +namespace kaitai { + +class custom_decoder { +public: + virtual ~custom_decoder() {}; + virtual std::string decode(std::string src) = 0; +}; + +} + +#endif diff --git a/phonelibs/kaitai/exceptions.h b/phonelibs/kaitai/exceptions.h new file mode 100644 index 000000000..5c09c4672 --- /dev/null +++ b/phonelibs/kaitai/exceptions.h @@ -0,0 +1,189 @@ +#ifndef KAITAI_EXCEPTIONS_H +#define KAITAI_EXCEPTIONS_H + +#include + +#include +#include + +// We need to use "noexcept" in virtual destructor of our exceptions +// subclasses. Different compilers have different ideas on how to +// achieve that: C++98 compilers prefer `throw()`, C++11 and later +// use `noexcept`. We define KS_NOEXCEPT macro for that. + +#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) +#define KS_NOEXCEPT noexcept +#else +#define KS_NOEXCEPT throw() +#endif + +namespace kaitai { + +/** + * Common ancestor for all error originating from Kaitai Struct usage. + * Stores KSY source path, pointing to an element supposedly guilty of + * an error. + */ +class kstruct_error: public std::runtime_error { +public: + kstruct_error(const std::string what, const std::string src_path): + std::runtime_error(src_path + ": " + what), + m_src_path(src_path) + { + } + + virtual ~kstruct_error() KS_NOEXCEPT {}; + +protected: + const std::string m_src_path; +}; + +/** + * Error that occurs when default endianness should be decided with + * a switch, but nothing matches (although using endianness expression + * implies that there should be some positive result). + */ +class undecided_endianness_error: public kstruct_error { +public: + undecided_endianness_error(const std::string src_path): + kstruct_error("unable to decide on endianness for a type", src_path) + { + } + + virtual ~undecided_endianness_error() KS_NOEXCEPT {}; +}; + +/** + * Common ancestor for all validation failures. Stores pointer to + * KaitaiStream IO object which was involved in an error. + */ +class validation_failed_error: public kstruct_error { +public: + validation_failed_error(const std::string what, kstream* io, const std::string src_path): + kstruct_error("at pos " + kstream::to_string(static_cast(io->pos())) + ": validation failed: " + what, src_path), + m_io(io) + { + } + +// "at pos #{io.pos}: validation failed: #{msg}" + + virtual ~validation_failed_error() KS_NOEXCEPT {}; + +protected: + kstream* m_io; +}; + +/** + * Signals validation failure: we required "actual" value to be equal to + * "expected", but it turned out that it's not. + */ +template +class validation_not_equal_error: public validation_failed_error { +public: + validation_not_equal_error(const T& expected, const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not equal", io, src_path), + m_expected(expected), + m_actual(actual) + { + } + + // "not equal, expected #{expected.inspect}, but got #{actual.inspect}" + + virtual ~validation_not_equal_error() KS_NOEXCEPT {}; + +protected: + const T& m_expected; + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to be greater + * than or equal to "min", but it turned out that it's not. + */ +template +class validation_less_than_error: public validation_failed_error { +public: + validation_less_than_error(const T& min, const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not in range", io, src_path), + m_min(min), + m_actual(actual) + { + } + + // "not in range, min #{min.inspect}, but got #{actual.inspect}" + + virtual ~validation_less_than_error() KS_NOEXCEPT {}; + +protected: + const T& m_min; + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to be less + * than or equal to "max", but it turned out that it's not. + */ +template +class validation_greater_than_error: public validation_failed_error { +public: + validation_greater_than_error(const T& max, const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not in range", io, src_path), + m_max(max), + m_actual(actual) + { + } + + // "not in range, max #{max.inspect}, but got #{actual.inspect}" + + virtual ~validation_greater_than_error() KS_NOEXCEPT {}; + +protected: + const T& m_max; + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to be from + * the list, but it turned out that it's not. + */ +template +class validation_not_any_of_error: public validation_failed_error { +public: + validation_not_any_of_error(const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not any of the list", io, src_path), + m_actual(actual) + { + } + + // "not any of the list, got #{actual.inspect}" + + virtual ~validation_not_any_of_error() KS_NOEXCEPT {}; + +protected: + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to match + * the expression, but it turned out that it doesn't. + */ +template +class validation_expr_error: public validation_failed_error { +public: + validation_expr_error(const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not matching the expression", io, src_path), + m_actual(actual) + { + } + + // "not matching the expression, got #{actual.inspect}" + + virtual ~validation_expr_error() KS_NOEXCEPT {}; + +protected: + const T& m_actual; +}; + +} + +#endif diff --git a/phonelibs/kaitai/kaitaistream.cpp b/phonelibs/kaitai/kaitaistream.cpp new file mode 100644 index 000000000..d82ddb7e8 --- /dev/null +++ b/phonelibs/kaitai/kaitaistream.cpp @@ -0,0 +1,689 @@ +#include + +#if defined(__APPLE__) +#include +#include +#define bswap_16(x) OSSwapInt16(x) +#define bswap_32(x) OSSwapInt32(x) +#define bswap_64(x) OSSwapInt64(x) +#define __BYTE_ORDER BYTE_ORDER +#define __BIG_ENDIAN BIG_ENDIAN +#define __LITTLE_ENDIAN LITTLE_ENDIAN +#elif defined(_MSC_VER) // !__APPLE__ +#include +#define __LITTLE_ENDIAN 1234 +#define __BIG_ENDIAN 4321 +#define __BYTE_ORDER __LITTLE_ENDIAN +#define bswap_16(x) _byteswap_ushort(x) +#define bswap_32(x) _byteswap_ulong(x) +#define bswap_64(x) _byteswap_uint64(x) +#else // !__APPLE__ or !_MSC_VER +#include +#include +#endif + +#include +#include +#include + +kaitai::kstream::kstream(std::istream* io) { + m_io = io; + init(); +} + +kaitai::kstream::kstream(std::string& data): m_io_str(data) { + m_io = &m_io_str; + init(); +} + +void kaitai::kstream::init() { + exceptions_enable(); + align_to_byte(); +} + +void kaitai::kstream::close() { + // m_io->close(); +} + +void kaitai::kstream::exceptions_enable() const { + m_io->exceptions( + std::istream::eofbit | + std::istream::failbit | + std::istream::badbit + ); +} + +// ======================================================================== +// Stream positioning +// ======================================================================== + +bool kaitai::kstream::is_eof() const { + if (m_bits_left > 0) { + return false; + } + char t; + m_io->exceptions( + std::istream::badbit + ); + m_io->get(t); + if (m_io->eof()) { + m_io->clear(); + exceptions_enable(); + return true; + } else { + m_io->unget(); + exceptions_enable(); + return false; + } +} + +void kaitai::kstream::seek(uint64_t pos) { + m_io->seekg(pos); +} + +uint64_t kaitai::kstream::pos() { + return m_io->tellg(); +} + +uint64_t kaitai::kstream::size() { + std::iostream::pos_type cur_pos = m_io->tellg(); + m_io->seekg(0, std::ios::end); + std::iostream::pos_type len = m_io->tellg(); + m_io->seekg(cur_pos); + return len; +} + +// ======================================================================== +// Integer numbers +// ======================================================================== + +// ------------------------------------------------------------------------ +// Signed +// ------------------------------------------------------------------------ + +int8_t kaitai::kstream::read_s1() { + char t; + m_io->get(t); + return t; +} + +// ........................................................................ +// Big-endian +// ........................................................................ + +int16_t kaitai::kstream::read_s2be() { + int16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +int32_t kaitai::kstream::read_s4be() { + int32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +int64_t kaitai::kstream::read_s8be() { + int64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ........................................................................ +// Little-endian +// ........................................................................ + +int16_t kaitai::kstream::read_s2le() { + int16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +int32_t kaitai::kstream::read_s4le() { + int32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +int64_t kaitai::kstream::read_s8le() { + int64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ------------------------------------------------------------------------ +// Unsigned +// ------------------------------------------------------------------------ + +uint8_t kaitai::kstream::read_u1() { + char t; + m_io->get(t); + return t; +} + +// ........................................................................ +// Big-endian +// ........................................................................ + +uint16_t kaitai::kstream::read_u2be() { + uint16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +uint32_t kaitai::kstream::read_u4be() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +uint64_t kaitai::kstream::read_u8be() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ........................................................................ +// Little-endian +// ........................................................................ + +uint16_t kaitai::kstream::read_u2le() { + uint16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +uint32_t kaitai::kstream::read_u4le() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +uint64_t kaitai::kstream::read_u8le() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ======================================================================== +// Floating point numbers +// ======================================================================== + +// ........................................................................ +// Big-endian +// ........................................................................ + +float kaitai::kstream::read_f4be() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_32(t); +#endif + return reinterpret_cast(t); +} + +double kaitai::kstream::read_f8be() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_64(t); +#endif + return reinterpret_cast(t); +} + +// ........................................................................ +// Little-endian +// ........................................................................ + +float kaitai::kstream::read_f4le() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_32(t); +#endif + return reinterpret_cast(t); +} + +double kaitai::kstream::read_f8le() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_64(t); +#endif + return reinterpret_cast(t); +} + +// ======================================================================== +// Unaligned bit values +// ======================================================================== + +void kaitai::kstream::align_to_byte() { + m_bits_left = 0; + m_bits = 0; +} + +uint64_t kaitai::kstream::read_bits_int_be(int n) { + int bits_needed = n - m_bits_left; + if (bits_needed > 0) { + // 1 bit => 1 byte + // 8 bits => 1 byte + // 9 bits => 2 bytes + int bytes_needed = ((bits_needed - 1) / 8) + 1; + if (bytes_needed > 8) + throw std::runtime_error("read_bits_int: more than 8 bytes requested"); + char buf[8]; + m_io->read(buf, bytes_needed); + for (int i = 0; i < bytes_needed; i++) { + uint8_t b = buf[i]; + m_bits <<= 8; + m_bits |= b; + m_bits_left += 8; + } + } + + // raw mask with required number of 1s, starting from lowest bit + uint64_t mask = get_mask_ones(n); + // shift mask to align with highest bits available in @bits + int shift_bits = m_bits_left - n; + mask <<= shift_bits; + // derive reading result + uint64_t res = (m_bits & mask) >> shift_bits; + // clear top bits that we've just read => AND with 1s + m_bits_left -= n; + mask = get_mask_ones(m_bits_left); + m_bits &= mask; + + return res; +} + +// Deprecated, use read_bits_int_be() instead. +uint64_t kaitai::kstream::read_bits_int(int n) { + return read_bits_int_be(n); +} + +uint64_t kaitai::kstream::read_bits_int_le(int n) { + int bits_needed = n - m_bits_left; + if (bits_needed > 0) { + // 1 bit => 1 byte + // 8 bits => 1 byte + // 9 bits => 2 bytes + int bytes_needed = ((bits_needed - 1) / 8) + 1; + if (bytes_needed > 8) + throw std::runtime_error("read_bits_int_le: more than 8 bytes requested"); + char buf[8]; + m_io->read(buf, bytes_needed); + for (int i = 0; i < bytes_needed; i++) { + uint8_t b = buf[i]; + m_bits |= (static_cast(b) << m_bits_left); + m_bits_left += 8; + } + } + + // raw mask with required number of 1s, starting from lowest bit + uint64_t mask = get_mask_ones(n); + // derive reading result + uint64_t res = m_bits & mask; + // remove bottom bits that we've just read by shifting + m_bits >>= n; + m_bits_left -= n; + + return res; +} + +uint64_t kaitai::kstream::get_mask_ones(int n) { + if (n == 64) { + return 0xFFFFFFFFFFFFFFFF; + } else { + return ((uint64_t) 1 << n) - 1; + } +} + +// ======================================================================== +// Byte arrays +// ======================================================================== + +std::string kaitai::kstream::read_bytes(std::streamsize len) { + std::vector result(len); + + // NOTE: streamsize type is signed, negative values are only *supposed* to not be used. + // http://en.cppreference.com/w/cpp/io/streamsize + if (len < 0) { + throw std::runtime_error("read_bytes: requested a negative amount"); + } + + if (len > 0) { + m_io->read(&result[0], len); + } + + return std::string(result.begin(), result.end()); +} + +std::string kaitai::kstream::read_bytes_full() { + std::iostream::pos_type p1 = m_io->tellg(); + m_io->seekg(0, std::ios::end); + std::iostream::pos_type p2 = m_io->tellg(); + size_t len = p2 - p1; + + // Note: this requires a std::string to be backed with a + // contiguous buffer. Officially, it's a only requirement since + // C++11 (C++98 and C++03 didn't have this requirement), but all + // major implementations had contiguous buffers anyway. + std::string result(len, ' '); + m_io->seekg(p1); + m_io->read(&result[0], len); + + return result; +} + +std::string kaitai::kstream::read_bytes_term(char term, bool include, bool consume, bool eos_error) { + std::string result; + std::getline(*m_io, result, term); + if (m_io->eof()) { + // encountered EOF + if (eos_error) { + throw std::runtime_error("read_bytes_term: encountered EOF"); + } + } else { + // encountered terminator + if (include) + result.push_back(term); + if (!consume) + m_io->unget(); + } + return result; +} + +std::string kaitai::kstream::ensure_fixed_contents(std::string expected) { + std::string actual = read_bytes(expected.length()); + + if (actual != expected) { + // NOTE: I think printing it outright is not best idea, it could contain non-ascii charactes like backspace and beeps and whatnot. It would be better to print hexlified version, and also to redirect it to stderr. + throw std::runtime_error("ensure_fixed_contents: actual data does not match expected data"); + } + + return actual; +} + +std::string kaitai::kstream::bytes_strip_right(std::string src, char pad_byte) { + std::size_t new_len = src.length(); + + while (new_len > 0 && src[new_len - 1] == pad_byte) + new_len--; + + return src.substr(0, new_len); +} + +std::string kaitai::kstream::bytes_terminate(std::string src, char term, bool include) { + std::size_t new_len = 0; + std::size_t max_len = src.length(); + + while (new_len < max_len && src[new_len] != term) + new_len++; + + if (include && new_len < max_len) + new_len++; + + return src.substr(0, new_len); +} + +// ======================================================================== +// Byte array processing +// ======================================================================== + +std::string kaitai::kstream::process_xor_one(std::string data, uint8_t key) { + size_t len = data.length(); + std::string result(len, ' '); + + for (size_t i = 0; i < len; i++) + result[i] = data[i] ^ key; + + return result; +} + +std::string kaitai::kstream::process_xor_many(std::string data, std::string key) { + size_t len = data.length(); + size_t kl = key.length(); + std::string result(len, ' '); + + size_t ki = 0; + for (size_t i = 0; i < len; i++) { + result[i] = data[i] ^ key[ki]; + ki++; + if (ki >= kl) + ki = 0; + } + + return result; +} + +std::string kaitai::kstream::process_rotate_left(std::string data, int amount) { + size_t len = data.length(); + std::string result(len, ' '); + + for (size_t i = 0; i < len; i++) { + uint8_t bits = data[i]; + result[i] = (bits << amount) | (bits >> (8 - amount)); + } + + return result; +} + +#ifdef KS_ZLIB +#include + +std::string kaitai::kstream::process_zlib(std::string data) { + int ret; + + unsigned char *src_ptr = reinterpret_cast(&data[0]); + std::stringstream dst_strm; + + z_stream strm; + strm.zalloc = Z_NULL; + strm.zfree = Z_NULL; + strm.opaque = Z_NULL; + + ret = inflateInit(&strm); + if (ret != Z_OK) + throw std::runtime_error("process_zlib: inflateInit error"); + + strm.next_in = src_ptr; + strm.avail_in = data.length(); + + unsigned char outbuffer[ZLIB_BUF_SIZE]; + std::string outstring; + + // get the decompressed bytes blockwise using repeated calls to inflate + do { + strm.next_out = reinterpret_cast(outbuffer); + strm.avail_out = sizeof(outbuffer); + + ret = inflate(&strm, 0); + + if (outstring.size() < strm.total_out) + outstring.append(reinterpret_cast(outbuffer), strm.total_out - outstring.size()); + } while (ret == Z_OK); + + if (ret != Z_STREAM_END) { // an error occurred that was not EOF + std::ostringstream exc_msg; + exc_msg << "process_zlib: error #" << ret << "): " << strm.msg; + throw std::runtime_error(exc_msg.str()); + } + + if (inflateEnd(&strm) != Z_OK) + throw std::runtime_error("process_zlib: inflateEnd error"); + + return outstring; +} +#endif + +// ======================================================================== +// Misc utility methods +// ======================================================================== + +int kaitai::kstream::mod(int a, int b) { + if (b <= 0) + throw std::invalid_argument("mod: divisor b <= 0"); + int r = a % b; + if (r < 0) + r += b; + return r; +} + +#include +std::string kaitai::kstream::to_string(int val) { + // if int is 32 bits, "-2147483648" is the longest string representation + // => 11 chars + zero => 12 chars + // if int is 64 bits, "-9223372036854775808" is the longest + // => 20 chars + zero => 21 chars + char buf[25]; + int got_len = snprintf(buf, sizeof(buf), "%d", val); + + // should never happen, but check nonetheless + if (got_len > sizeof(buf)) + throw std::invalid_argument("to_string: integer is longer than string buffer"); + + return std::string(buf); +} + +#include +std::string kaitai::kstream::reverse(std::string val) { + std::reverse(val.begin(), val.end()); + + return val; +} + +uint8_t kaitai::kstream::byte_array_min(const std::string val) { + uint8_t min = 0xff; // UINT8_MAX + std::string::const_iterator end = val.end(); + for (std::string::const_iterator it = val.begin(); it != end; ++it) { + uint8_t cur = static_cast(*it); + if (cur < min) { + min = cur; + } + } + return min; +} + +uint8_t kaitai::kstream::byte_array_max(const std::string val) { + uint8_t max = 0; // UINT8_MIN + std::string::const_iterator end = val.end(); + for (std::string::const_iterator it = val.begin(); it != end; ++it) { + uint8_t cur = static_cast(*it); + if (cur > max) { + max = cur; + } + } + return max; +} + +// ======================================================================== +// Other internal methods +// ======================================================================== + +#ifndef KS_STR_DEFAULT_ENCODING +#define KS_STR_DEFAULT_ENCODING "UTF-8" +#endif + +#ifdef KS_STR_ENCODING_ICONV + +#include +#include +#include + +std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) { + iconv_t cd = iconv_open(KS_STR_DEFAULT_ENCODING, src_enc.c_str()); + + if (cd == (iconv_t) -1) { + if (errno == EINVAL) { + throw std::runtime_error("bytes_to_str: invalid encoding pair conversion requested"); + } else { + throw std::runtime_error("bytes_to_str: error opening iconv"); + } + } + + size_t src_len = src.length(); + size_t src_left = src_len; + + // Start with a buffer length of double the source length. + size_t dst_len = src_len * 2; + std::string dst(dst_len, ' '); + size_t dst_left = dst_len; + + char *src_ptr = &src[0]; + char *dst_ptr = &dst[0]; + + while (true) { + size_t res = iconv(cd, &src_ptr, &src_left, &dst_ptr, &dst_left); + + if (res == (size_t) -1) { + if (errno == E2BIG) { + // dst buffer is not enough to accomodate whole string + // enlarge the buffer and try again + size_t dst_used = dst_len - dst_left; + dst_left += dst_len; + dst_len += dst_len; + dst.resize(dst_len); + + // dst.resize might have allocated destination buffer in another area + // of memory, thus our previous pointer "dst" will be invalid; re-point + // it using "dst_used". + dst_ptr = &dst[dst_used]; + } else { + throw std::runtime_error("bytes_to_str: iconv error"); + } + } else { + // conversion successful + dst.resize(dst_len - dst_left); + break; + } + } + + if (iconv_close(cd) != 0) { + throw std::runtime_error("bytes_to_str: iconv close error"); + } + + return dst; +} +#elif defined(KS_STR_ENCODING_NONE) +std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) { + return src; +} +#else +#error Need to decide how to handle strings: please define one of: KS_STR_ENCODING_ICONV, KS_STR_ENCODING_NONE +#endif diff --git a/phonelibs/kaitai/kaitaistream.h b/phonelibs/kaitai/kaitaistream.h new file mode 100644 index 000000000..e7f4c6ce3 --- /dev/null +++ b/phonelibs/kaitai/kaitaistream.h @@ -0,0 +1,268 @@ +#ifndef KAITAI_STREAM_H +#define KAITAI_STREAM_H + +// Kaitai Struct runtime API version: x.y.z = 'xxxyyyzzz' decimal +#define KAITAI_STRUCT_VERSION 9000L + +#include +#include +#include +#include + +namespace kaitai { + +/** + * Kaitai Stream class (kaitai::kstream) is an implementation of + * Kaitai Struct stream API + * for C++/STL. It's implemented as a wrapper over generic STL std::istream. + * + * It provides a wide variety of simple methods to read (parse) binary + * representations of primitive types, such as integer and floating + * point numbers, byte arrays and strings, and also provides stream + * positioning / navigation methods with unified cross-language and + * cross-toolkit semantics. + * + * Typically, end users won't access Kaitai Stream class manually, but would + * describe a binary structure format using .ksy language and then would use + * Kaitai Struct compiler to generate source code in desired target language. + * That code, in turn, would use this class and API to do the actual parsing + * job. + */ +class kstream { +public: + /** + * Constructs new Kaitai Stream object, wrapping a given std::istream. + * \param io istream object to use for this Kaitai Stream + */ + kstream(std::istream* io); + + /** + * Constructs new Kaitai Stream object, wrapping a given in-memory data + * buffer. + * \param data data buffer to use for this Kaitai Stream + */ + kstream(std::string& data); + + void close(); + + /** @name Stream positioning */ + //@{ + /** + * Check if stream pointer is at the end of stream. Note that the semantics + * are different from traditional STL semantics: one does *not* need to do a + * read (which will fail) after the actual end of the stream to trigger EOF + * flag, which can be accessed after that read. It is sufficient to just be + * at the end of the stream for this method to return true. + * \return "true" if we are located at the end of the stream. + */ + bool is_eof() const; + + /** + * Set stream pointer to designated position. + * \param pos new position (offset in bytes from the beginning of the stream) + */ + void seek(uint64_t pos); + + /** + * Get current position of a stream pointer. + * \return pointer position, number of bytes from the beginning of the stream + */ + uint64_t pos(); + + /** + * Get total size of the stream in bytes. + * \return size of the stream in bytes + */ + uint64_t size(); + //@} + + /** @name Integer numbers */ + //@{ + + // ------------------------------------------------------------------------ + // Signed + // ------------------------------------------------------------------------ + + int8_t read_s1(); + + // ........................................................................ + // Big-endian + // ........................................................................ + + int16_t read_s2be(); + int32_t read_s4be(); + int64_t read_s8be(); + + // ........................................................................ + // Little-endian + // ........................................................................ + + int16_t read_s2le(); + int32_t read_s4le(); + int64_t read_s8le(); + + // ------------------------------------------------------------------------ + // Unsigned + // ------------------------------------------------------------------------ + + uint8_t read_u1(); + + // ........................................................................ + // Big-endian + // ........................................................................ + + uint16_t read_u2be(); + uint32_t read_u4be(); + uint64_t read_u8be(); + + // ........................................................................ + // Little-endian + // ........................................................................ + + uint16_t read_u2le(); + uint32_t read_u4le(); + uint64_t read_u8le(); + + //@} + + /** @name Floating point numbers */ + //@{ + + // ........................................................................ + // Big-endian + // ........................................................................ + + float read_f4be(); + double read_f8be(); + + // ........................................................................ + // Little-endian + // ........................................................................ + + float read_f4le(); + double read_f8le(); + + //@} + + /** @name Unaligned bit values */ + //@{ + + void align_to_byte(); + uint64_t read_bits_int_be(int n); + uint64_t read_bits_int(int n); + uint64_t read_bits_int_le(int n); + + //@} + + /** @name Byte arrays */ + //@{ + + std::string read_bytes(std::streamsize len); + std::string read_bytes_full(); + std::string read_bytes_term(char term, bool include, bool consume, bool eos_error); + std::string ensure_fixed_contents(std::string expected); + + static std::string bytes_strip_right(std::string src, char pad_byte); + static std::string bytes_terminate(std::string src, char term, bool include); + static std::string bytes_to_str(std::string src, std::string src_enc); + + //@} + + /** @name Byte array processing */ + //@{ + + /** + * Performs a XOR processing with given data, XORing every byte of input with a single + * given value. + * @param data data to process + * @param key value to XOR with + * @return processed data + */ + static std::string process_xor_one(std::string data, uint8_t key); + + /** + * Performs a XOR processing with given data, XORing every byte of input with a key + * array, repeating key array many times, if necessary (i.e. if data array is longer + * than key array). + * @param data data to process + * @param key array of bytes to XOR with + * @return processed data + */ + static std::string process_xor_many(std::string data, std::string key); + + /** + * Performs a circular left rotation shift for a given buffer by a given amount of bits, + * using groups of 1 bytes each time. Right circular rotation should be performed + * using this procedure with corrected amount. + * @param data source data to process + * @param amount number of bits to shift by + * @return copy of source array with requested shift applied + */ + static std::string process_rotate_left(std::string data, int amount); + +#ifdef KS_ZLIB + /** + * Performs an unpacking ("inflation") of zlib-compressed data with usual zlib headers. + * @param data data to unpack + * @return unpacked data + * @throws IOException + */ + static std::string process_zlib(std::string data); +#endif + + //@} + + /** + * Performs modulo operation between two integers: dividend `a` + * and divisor `b`. Divisor `b` is expected to be positive. The + * result is always 0 <= x <= b - 1. + */ + static int mod(int a, int b); + + /** + * Converts given integer `val` to a decimal string representation. + * Should be used in place of std::to_string() (which is available only + * since C++11) in older C++ implementations. + */ + static std::string to_string(int val); + + /** + * Reverses given string `val`, so that the first character becomes the + * last and the last one becomes the first. This should be used to avoid + * the need of local variables at the caller. + */ + static std::string reverse(std::string val); + + /** + * Finds the minimal byte in a byte array, treating bytes as + * unsigned values. + * @param val byte array to scan + * @return minimal byte in byte array as integer + */ + static uint8_t byte_array_min(const std::string val); + + /** + * Finds the maximal byte in a byte array, treating bytes as + * unsigned values. + * @param val byte array to scan + * @return maximal byte in byte array as integer + */ + static uint8_t byte_array_max(const std::string val); + +private: + std::istream* m_io; + std::istringstream m_io_str; + int m_bits_left; + uint64_t m_bits; + + void init(); + void exceptions_enable() const; + + static uint64_t get_mask_ones(int n); + + static const int ZLIB_BUF_SIZE = 128 * 1024; +}; + +} + +#endif diff --git a/phonelibs/kaitai/kaitaistruct.h b/phonelibs/kaitai/kaitaistruct.h new file mode 100644 index 000000000..8172ede6c --- /dev/null +++ b/phonelibs/kaitai/kaitaistruct.h @@ -0,0 +1,20 @@ +#ifndef KAITAI_STRUCT_H +#define KAITAI_STRUCT_H + +#include + +namespace kaitai { + +class kstruct { +public: + kstruct(kstream *_io) { m__io = _io; } + virtual ~kstruct() {} +protected: + kstream *m__io; +public: + kstream *_io() { return m__io; } +}; + +} + +#endif diff --git a/rednose/SConscript b/rednose/SConscript new file mode 100644 index 000000000..36daf2cd7 --- /dev/null +++ b/rednose/SConscript @@ -0,0 +1,40 @@ +Import('env', 'envCython', 'arch', 'rednose_config') + +generated_folder = rednose_config['generated_folder'] + +templates = Glob('#rednose/templates/*') + +sympy_helpers = "#rednose/helpers/sympy_helpers.py" +ekf_sym = "#rednose/helpers/ekf_sym.py" +ekf_sym_pyx = "#rednose/helpers/ekf_sym_pyx.pyx" +ekf_sym_cc = env.Object("#rednose/helpers/ekf_sym.cc") +common_ekf = "#rednose/helpers/common_ekf.cc" + +found = {} +for target, (command, combined_lib, extra_generated) in rednose_config['to_build'].items(): + if File(command).exists(): + found[target] = (command, combined_lib, extra_generated) + +lib_target = [common_ekf] +for target, (command, combined_lib, extra_generated) in found.items(): + target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h']) + extra_generated = [File(f'{generated_folder}/{x}') for x in extra_generated] + command_file = File(command) + + env.Command(target_files + extra_generated, + [templates, command_file, sympy_helpers, ekf_sym], + command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath()) + + if combined_lib: + lib_target.append(target_files[0]) + else: + env.SharedLibrary(f'{generated_folder}/' + target, [target_files[0], common_ekf]) + +libkf = env.SharedLibrary(f'{generated_folder}/libkf', lib_target) + +lenv = envCython.Clone() +lenv["LINKFLAGS"] += [libkf[0].get_labspath()] +ekf_sym_so = lenv.Program('#rednose/helpers/ekf_sym_pyx.so', [ekf_sym_pyx, ekf_sym_cc, common_ekf]) +lenv.Depends(ekf_sym_so, libkf) + +Export('libkf') diff --git a/rednose/helpers/__init__.py b/rednose/helpers/__init__.py index aba6b41fe..bbd239b77 100644 --- a/rednose/helpers/__init__.py +++ b/rednose/helpers/__init__.py @@ -13,14 +13,19 @@ def write_code(folder, name, code, header): open(os.path.join(folder, f"{name}.h"), 'w').write(header) -def load_code(folder, name): +def load_code(folder, name, lib_name=None): + if lib_name is None: + lib_name = name shared_ext = "dylib" if platform.system() == "Darwin" else "so" - shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}") + shared_fn = os.path.join(folder, f"lib{lib_name}.{shared_ext}") header_fn = os.path.join(folder, f"{name}.h") with open(header_fn) as f: header = f.read() + # is the only thing that can be parsed by cffi + header = "\n".join([line for line in header.split("\n") if line.startswith("void ")]) + ffi = FFI() ffi.cdef(header) return (ffi, ffi.dlopen(shared_fn)) diff --git a/rednose/helpers/common_ekf.cc b/rednose/helpers/common_ekf.cc new file mode 100644 index 000000000..1e6390250 --- /dev/null +++ b/rednose/helpers/common_ekf.cc @@ -0,0 +1,19 @@ +#include "common_ekf.h" + +std::vector& get_ekfs() { + static std::vector vec; + return vec; +} + +void ekf_register(const EKF* ekf) { + get_ekfs().push_back(ekf); +} + +const EKF* ekf_lookup(const std::string& ekf_name) { + for (const auto& ekfi : get_ekfs()) { + if (ekf_name == ekfi->name) { + return ekfi; + } + } + return NULL; +} diff --git a/rednose/helpers/common_ekf.h b/rednose/helpers/common_ekf.h new file mode 100644 index 000000000..5dfdd448b --- /dev/null +++ b/rednose/helpers/common_ekf.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +typedef void (*extra_routine_t)(double *, double *); + +struct EKF { + std::string name; + std::vector kinds; + std::vector feature_kinds; + + void (*f_fun)(double *, double, double *); + void (*F_fun)(double *, double, double *); + void (*err_fun)(double *, double *, double *); + void (*inv_err_fun)(double *, double *, double *); + void (*H_mod_fun)(double *, double *); + void (*predict)(double *, double *, double *, double); + std::unordered_map hs = {}; + std::unordered_map Hs = {}; + std::unordered_map updates = {}; + std::unordered_map Hes = {}; + std::unordered_map sets = {}; + std::unordered_map extra_routines = {}; +}; + +std::vector& get_ekfs(); +const EKF* ekf_lookup(const std::string& ekf_name); + +void ekf_register(const EKF* ekf); + +#define ekf_init(ekf) \ +static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \ + ekf_register(&ekf); \ +} diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc new file mode 100644 index 000000000..365a6f989 --- /dev/null +++ b/rednose/helpers/ekf_sym.cc @@ -0,0 +1,223 @@ +#include "ekf_sym.h" + +using namespace EKFS; +using namespace Eigen; + +EKFSym::EKFSym(std::string name, Map Q, Map x_initial, Map P_initial, int dim_main, + int dim_main_err, int N, int dim_augment, int dim_augment_err, std::vector maha_test_kinds, + std::vector quaternion_idxs, std::vector global_vars, double max_rewind_age) +{ + // TODO: add logger + + this->ekf = ekf_lookup(name); + assert(this->ekf); + + this->msckf = N > 0; + this->N = N; + this->dim_augment = dim_augment; + this->dim_augment_err = dim_augment_err; + this->dim_main = dim_main; + this->dim_main_err = dim_main_err; + + this->dim_x = x_initial.rows(); + this->dim_err = P_initial.rows(); + + assert(dim_main + dim_augment * N == dim_x); + assert(dim_main_err + dim_augment_err * N == this->dim_err); + assert(Q.rows() == P_initial.rows() && Q.cols() == P_initial.cols()); + + // kinds that should get mahalanobis distance + // tested for outlier rejection + this->maha_test_kinds = maha_test_kinds; + + // quaternions need normalization + this->quaternion_idxs = quaternion_idxs; + + this->global_vars = global_vars; + + // Process noise + this->Q = Q; + + this->max_rewind_age = max_rewind_age; + this->init_state(x_initial, P_initial, NAN); +} + +void EKFSym::init_state(Map state, Map covs, double filter_time) { + this->x = state; + this->P = covs; + this->filter_time = filter_time; + this->augment_times = VectorXd::Zero(this->N); + this->reset_rewind(); +} + +VectorXd EKFSym::state() { + return this->x; +} + +MatrixXdr EKFSym::covs() { + return this->P; +} + +void EKFSym::set_filter_time(double t) { + this->filter_time = t; +} + +double EKFSym::get_filter_time() { + return this->filter_time; +} + +void EKFSym::normalize_quaternions() { + for(std::size_t i = 0; i < this->quaternion_idxs.size(); ++i) { + this->normalize_slice(this->quaternion_idxs[i], this->quaternion_idxs[i] + 4); + } +} + +void EKFSym::normalize_slice(int slice_start, int slice_end_ex) { + this->x.block(slice_start, 0, slice_end_ex - slice_start, this->x.cols()).normalize(); +} + +void EKFSym::set_global(std::string global_var, double val) { + this->ekf->sets.at(global_var)(val); +} + +std::optional EKFSym::predict_and_update_batch(double t, int kind, std::vector> z_map, + std::vector> R_map, std::vector> extra_args, bool augment) +{ + // TODO handle rewinding at this level + + std::deque rewound; + if (!std::isnan(this->filter_time) && t < this->filter_time) { + if (this->rewind_t.empty() || t < this->rewind_t.front() || t < this->rewind_t.back() - this->max_rewind_age) { + std::cout << "observation too old at " << t << " with filter at " << this->filter_time << ", ignoring" << std::endl; + return std::nullopt; + } + rewound = this->rewind(t); + } + + Observation obs; + obs.t = t; + obs.kind = kind; + obs.extra_args = extra_args; + for (Map zi : z_map) { + obs.z.push_back(zi); + } + for (Map Ri : R_map) { + obs.R.push_back(Ri); + } + + std::optional res = std::make_optional(this->predict_and_update_batch(obs, augment)); + + // optional fast forward + while (!rewound.empty()) { + this->predict_and_update_batch(rewound.front(), false); + rewound.pop_front(); + } + + return res; +} + +void EKFSym::reset_rewind() { + this->rewind_obscache.clear(); + this->rewind_t.clear(); + this->rewind_states.clear(); +} + +std::deque EKFSym::rewind(double t) { + std::deque rewound; + + // rewind observations until t is after previous observation + while (this->rewind_t.back() > t) { + rewound.push_front(this->rewind_obscache.back()); + this->rewind_t.pop_back(); + this->rewind_states.pop_back(); + this->rewind_obscache.pop_back(); + } + + // set the state to the time right before that + this->filter_time = this->rewind_t.back(); + this->x = this->rewind_states.back().first; + this->P = this->rewind_states.back().second; + + return rewound; +} + +void EKFSym::checkpoint(Observation& obs) { + // push to rewinder + this->rewind_t.push_back(this->filter_time); + this->rewind_states.push_back(std::make_pair(this->x, this->P)); + this->rewind_obscache.push_back(obs); + + // only keep a certain number around + if (this->rewind_t.size() > REWIND_TO_KEEP) { + this->rewind_t.pop_front(); + this->rewind_states.pop_front(); + this->rewind_obscache.pop_front(); + } +} + +Estimate EKFSym::predict_and_update_batch(Observation& obs, bool augment) { + assert(obs.z.size() == obs.R.size()); + assert(obs.z.size() == obs.extra_args.size()); + + this->predict(obs.t); + + Estimate res; + res.t = obs.t; + res.kind = obs.kind; + res.z = obs.z; + res.extra_args = obs.extra_args; + res.xk1 = this->x; + res.Pk1 = this->P; + + // update batch + std::vector y; + for (int i = 0; i < obs.z.size(); i++) { + assert(obs.z[i].rows() == obs.R[i].rows()); + assert(obs.z[i].rows() == obs.R[i].cols()); + + // update state + y.push_back(this->update(obs.kind, obs.z[i], obs.R[i], obs.extra_args[i])); + } + + res.xk = this->x; + res.Pk = this->P; + res.y = y; + + assert(!augment); // TODO + // if (augment) { + // this->augment(); + // } + + this->checkpoint(obs); + + return res; +} + +void EKFSym::predict(double t) { + // initialize time + if (std::isnan(this->filter_time)) { + this->filter_time = t; + } + + // predict + double dt = t - this->filter_time; + assert(dt >= 0.0); + + this->ekf->predict(this->x.data(), this->P.data(), this->Q.data(), dt); + this->normalize_quaternions(); + this->filter_time = t; +} + +VectorXd EKFSym::update(int kind, VectorXd z, MatrixXdr R, std::vector extra_args) { + this->ekf->updates.at(kind)(this->x.data(), this->P.data(), z.data(), R.data(), extra_args.data()); + this->normalize_quaternions(); + + if (this->msckf && std::find(this->feature_track_kinds.begin(), this->feature_track_kinds.end(), kind) != this->feature_track_kinds.end()) { + return z.head(z.rows() - extra_args.size()); + } + return z; +} + +extra_routine_t EKFSym::get_extra_routine(const std::string& routine) { + return this->ekf->extra_routines.at(routine); +} diff --git a/rednose/helpers/ekf_sym.h b/rednose/helpers/ekf_sym.h new file mode 100644 index 000000000..86d6ca193 --- /dev/null +++ b/rednose/helpers/ekf_sym.h @@ -0,0 +1,112 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "common_ekf.h" + +#define REWIND_TO_KEEP 512 + +namespace EKFS { + +typedef Eigen::Matrix MatrixXdr; + +typedef struct Observation { + double t; + int kind; + std::vector z; + std::vector R; + std::vector> extra_args; +} Observation; + +typedef struct Estimate { + Eigen::VectorXd xk1; + Eigen::VectorXd xk; + MatrixXdr Pk1; + MatrixXdr Pk; + double t; + int kind; + std::vector y; + std::vector z; + std::vector> extra_args; +} Estimate; + +class EKFSym { +public: + EKFSym(std::string name, Eigen::Map Q, Eigen::Map x_initial, + Eigen::Map P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0, + int dim_augment_err = 0, std::vector maha_test_kinds = std::vector(), + std::vector quaternion_idxs = std::vector(), + std::vector global_vars = std::vector(), double max_rewind_age = 1.0); + void init_state(Eigen::Map state, Eigen::Map covs, double filter_time); + + Eigen::VectorXd state(); + MatrixXdr covs(); + void set_filter_time(double t); + double get_filter_time(); + void normalize_quaternions(); + void normalize_slice(int slice_start, int slice_end_ex); + void set_global(std::string global_var, double val); + void reset_rewind(); + + void predict(double t); + std::optional predict_and_update_batch(double t, int kind, std::vector> z, + std::vector> R, std::vector> extra_args = {{}}, bool augment = false); + + extra_routine_t get_extra_routine(const std::string& routine); + +private: + std::deque rewind(double t); + void checkpoint(Observation& obs); + + Estimate predict_and_update_batch(Observation& obs, bool augment); + Eigen::VectorXd update(int kind, Eigen::VectorXd z, MatrixXdr R, std::vector extra_args); + + // stuct with linked sympy generated functions + const EKF *ekf = NULL; + + Eigen::VectorXd x; // state + MatrixXdr P; // covs + + bool msckf; + int N; + int dim_augment; + int dim_augment_err; + int dim_main; + int dim_main_err; + + // state + int dim_x; + int dim_err; + + double filter_time; + + std::vector maha_test_kinds; + std::vector quaternion_idxs; + + std::vector global_vars; + + // process noise + MatrixXdr Q; + + // rewind stuff + double max_rewind_age; + std::deque rewind_t; + std::deque> rewind_states; + std::deque rewind_obscache; + + Eigen::VectorXd augment_times; + + std::vector feature_track_kinds; +}; + +} diff --git a/rednose/helpers/ekf_sym.py b/rednose/helpers/ekf_sym.py index 9f635c48d..c9cf0bc22 100644 --- a/rednose/helpers/ekf_sym.py +++ b/rednose/helpers/ekf_sym.py @@ -7,7 +7,7 @@ import sympy as sp from numpy import dot from rednose.helpers.sympy_helpers import sympy_into_c -from rednose.helpers import (TEMPLATE_DIR, load_code, write_code) +from rednose.helpers import TEMPLATE_DIR, load_code from rednose.helpers.chi2_lookup import chi2_ppf @@ -27,7 +27,7 @@ def null(H, eps=1e-12): def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, # pylint: disable=dangerous-default-value - maha_test_kinds=[], global_vars=None): + maha_test_kinds=[], quaternion_idxs=[], global_vars=None, extra_routines=[]): # optional state transition matrix, H modifier # and err_function if an error-state kalman filter (ESKF) # is desired. Best described in "Quaternion kinematics @@ -91,6 +91,9 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p # collect sympy functions sympy_functions = [] + # extra routines + sympy_functions += extra_routines + # error functions sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]])) sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]])) @@ -110,15 +113,26 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym])) # Generate and wrap all th c code - header, code = sympy_into_c(sympy_functions, global_vars) - extra_header = "#define DIM %d\n" % dim_x - extra_header += "#define EDIM %d\n" % dim_err - extra_header += "#define MEDIM %d\n" % dim_main_err - extra_header += "typedef void (*Hfun)(double *, double *, double *);\n" + sympy_header, code = sympy_into_c(sympy_functions, global_vars) - extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);" + header = "#pragma once\n" + header += "#include \"rednose/helpers/common_ekf.h\"\n" + header += "extern \"C\" {\n" - extra_post = "" + pre_code = f"#include \"{name}.h\"\n" + pre_code += "\nnamespace {\n" + pre_code += "#define DIM %d\n" % dim_x + pre_code += "#define EDIM %d\n" % dim_err + pre_code += "#define MEDIM %d\n" % dim_main_err + pre_code += "typedef void (*Hfun)(double *, double *, double *);\n" + + if global_vars is not None: + for var in global_vars: + pre_code += f"\ndouble {var.name};\n" + pre_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n" + + post_code = "\n}\n" # namespace + post_code += "extern \"C\" {\n\n" for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: if msckf and kind in feature_track_kinds: @@ -129,36 +143,80 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p # ea_dim = 1 # not really dim of ea but makes c function work maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection maha_test = kind in maha_test_kinds - extra_post += """ - void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d); - } - """ % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind) - extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh) - extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind - code += '\nextern "C"{\n' + extra_header + "\n}\n" - code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read() - code += '\nextern "C"{\n' + extra_post + "\n}\n" + pre_code += f"const static double MAHA_THRESH_{kind} = {maha_thresh};\n" + header += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);\n" + post_code += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {{\n" + post_code += f" update<{h_sym.shape[0]}, 3, {int(maha_test)}>(in_x, in_P, h_{kind}, H_{kind}, {He_str}, in_z, in_R, in_ea, MAHA_THRESH_{kind});\n" + post_code += "}\n" + + # For ffi loading of specific functions + for line in sympy_header.split("\n"): + if line.startswith("void "): # sympy functions + func_call = line[5: line.index(')') + 1] + header += f"void {name}_{func_call};\n" + post_code += f"void {name}_{func_call} {{\n" + post_code += f" {func_call.replace('double *', '').replace('double', '')};\n" + post_code += "}\n" + header += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt);\n" + post_code += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt) {{\n" + post_code += " predict(in_x, in_P, in_Q, dt);\n" + post_code += "}\n" if global_vars is not None: - global_code = '\nextern "C"{\n' for var in global_vars: - global_code += f"\ndouble {var.name};\n" - global_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n" - extra_header += f"\nvoid set_{var.name}(double x);\n" + header += f"void {name}_set_{var.name}(double x);\n" + post_code += f"void {name}_set_{var.name}(double x) {{\n" + post_code += f" set_{var.name}(x);\n" + post_code += "}\n" - global_code += '\n}\n' - code = global_code + code + post_code += "}\n\n" # extern c - header += "\n" + extra_header + funcs = ['f_fun', 'F_fun', 'err_fun', 'inv_err_fun', 'H_mod_fun', 'predict'] + func_lists = { + 'h': [kind for _, kind, _, _, _ in obs_eqs], + 'H': [kind for _, kind, _, _, _ in obs_eqs], + 'update': [kind for _, kind, _, _, _ in obs_eqs], + 'He': [kind for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds], + 'set': [var.name for var in global_vars] if global_vars is not None else [], + } + func_extra = [x[0] for x in extra_routines] - write_code(folder, name, code, header) + # For dynamic loading of specific functions + post_code += f"const EKF {name} = {{\n" + post_code += f" .name = \"{name}\",\n" + post_code += f" .kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs])} }},\n" + post_code += f" .feature_kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds])} }},\n" + for func in funcs: + post_code += f" .{func} = {name}_{func},\n" + for group, kinds in func_lists.items(): + post_code += f" .{group}s = {{\n" + for kind in kinds: + str_kind = f"\"{kind}\"" if type(kind) == str else kind + post_code += f" {{ {str_kind}, {name}_{group}_{kind} }},\n" + post_code += " },\n" + post_code += " .extra_routines = {\n" + for f in func_extra: + post_code += f" {{ \"{f}\", {name}_{f} }},\n" + post_code += " },\n" + post_code += "};\n\n" + post_code += f"ekf_init({name});\n" + + # merge code blocks + header += "}" + code = "\n".join([pre_code, code, open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read(), post_code]) + + # write to file + if not os.path.exists(folder): + os.mkdir(folder) + + open(os.path.join(folder, f"{name}.h"), 'w').write(header) # header is used for ffi import + open(os.path.join(folder, f"{name}.cpp"), 'w').write(code) class EKF_sym(): def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err, # pylint: disable=dangerous-default-value - N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None, max_rewind_age=1.0, logger=logging): + N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], quaternion_idxs=[], global_vars=None, max_rewind_age=1.0, logger=logging): """Generates process function and all observation functions for the kalman filter.""" self.msckf = N > 0 self.N = N @@ -181,7 +239,8 @@ class EKF_sym(): # tested for outlier rejection self.maha_test_kinds = maha_test_kinds - self.global_vars = global_vars + # quaternions need normalization + self.quaternion_idxs = quaternion_idxs # process noise self.Q = Q @@ -193,25 +252,25 @@ class EKF_sym(): self.rewind_obscache = [] self.init_state(x_initial, P_initial, None) - ffi, lib = load_code(folder, name) + ffi, lib = load_code(folder, name, "kf") kinds, self.feature_track_kinds = [], [] for func in dir(lib): - if func[:2] == 'h_': - kinds.append(int(func[2:])) - if func[:3] == 'He_': - self.feature_track_kinds.append(int(func[3:])) + if func[:len(name) + 3] == f'{name}_h_': + kinds.append(int(func[len(name) + 3:])) + if func[:len(name) + 4] == f'{name}_He_': + self.feature_track_kinds.append(int(func[len(name) + 4:])) # wrap all the sympy functions - def wrap_1lists(name): - func = eval("lib.%s" % name, {"lib": lib}) # pylint: disable=eval-used + def wrap_1lists(func_name): + func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used def ret(lst1, out): func(ffi.cast("double *", lst1.ctypes.data), ffi.cast("double *", out.ctypes.data)) return ret - def wrap_2lists(name): - func = eval("lib.%s" % name, {"lib": lib}) # pylint: disable=eval-used + def wrap_2lists(func_name): + func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used def ret(lst1, lst2, out): func(ffi.cast("double *", lst1.ctypes.data), @@ -219,8 +278,8 @@ class EKF_sym(): ffi.cast("double *", out.ctypes.data)) return ret - def wrap_1list_1float(name): - func = eval("lib.%s" % name, {"lib": lib}) # pylint: disable=eval-used + def wrap_1list_1float(func_name): + func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used def ret(lst1, fl, out): func(ffi.cast("double *", lst1.ctypes.data), @@ -237,27 +296,28 @@ class EKF_sym(): self.hs, self.Hs, self.Hes = {}, {}, {} for kind in kinds: - self.hs[kind] = wrap_2lists("h_%d" % kind) - self.Hs[kind] = wrap_2lists("H_%d" % kind) + self.hs[kind] = wrap_2lists(f"h_{kind}") + self.Hs[kind] = wrap_2lists(f"H_{kind}") if self.msckf and kind in self.feature_track_kinds: - self.Hes[kind] = wrap_2lists("He_%d" % kind) + self.Hes[kind] = wrap_2lists(f"He_{kind}") - if self.global_vars is not None: - for var in self.global_vars: - fun_name = f"set_{var.name}" - setattr(self, fun_name, getattr(lib, fun_name)) + self.set_globals = {} + if global_vars is not None: + for global_var in global_vars: + self.set_globals[global_var] = getattr(lib, f"{name}_set_{global_var}") # wrap the C++ predict function def _predict_blas(x, P, dt): - lib.predict(ffi.cast("double *", x.ctypes.data), - ffi.cast("double *", P.ctypes.data), - ffi.cast("double *", self.Q.ctypes.data), - ffi.cast("double", dt)) + func = eval(f"lib.{name}_predict", {"lib": lib}) # pylint: disable=eval-used + func(ffi.cast("double *", x.ctypes.data), + ffi.cast("double *", P.ctypes.data), + ffi.cast("double *", self.Q.ctypes.data), + ffi.cast("double", dt)) return x, P # wrap the C++ update function def fun_wrapper(f, kind): - f = eval("lib.%s" % f, {"lib": lib}) # pylint: disable=eval-used + f = eval(f"lib.{name}_{f}", {"lib": lib}) # pylint: disable=eval-used def _update_inner_blas(x, P, z, R, extra_args): f(ffi.cast("double *", x.ctypes.data), @@ -333,6 +393,25 @@ class EKF_sym(): def covs(self): return self.P + def set_filter_time(self, t): + self.filter_time = t + + def get_filter_time(self): + return self.filter_time + + def normalize_quaternions(self): + for idx in self.quaternion_idxs: + self.normalize_slice(idx, idx+4) + + def normalize_slice(self, slice_start, slice_end_ex): + self.x[slice_start:slice_end_ex] /= np.linalg.norm(self.x[slice_start:slice_end_ex]) + + def get_augment_times(self): + return self.augment_times + + def set_global(self, global_var, val): + self.set_globals[global_var](val) + def rewind(self, t): # find where we are rewinding to idx = bisect_right(self.rewind_t, t) @@ -376,6 +455,7 @@ class EKF_sym(): dt = t - self.filter_time assert dt >= 0 self.x, self.P = self._predict(self.x, self.P, dt) + self.normalize_quaternions() self.filter_time = t def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # pylint: disable=dangerous-default-value @@ -401,11 +481,9 @@ class EKF_sym(): def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False): """The main kalman filter function Predicts the state and then updates a batch of observations - dim_x: dimensionality of the state space dim_z: dimensionality of the observation and depends on kind n: number of observations - Args: t (float): Time of observation kind (int): Type of observation @@ -437,6 +515,7 @@ class EKF_sym(): extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F') # update self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i) + self.normalize_quaternions() y.append(y_i) xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P) @@ -570,7 +649,6 @@ class EKF_sym(): ''' Returns rts smoothed results of kalman filter estimates - If the kalman state is augmented with old states only the main state is smoothed ''' diff --git a/rednose/helpers/ekf_sym_pyx.pyx b/rednose/helpers/ekf_sym_pyx.pyx new file mode 100644 index 000000000..a5c0d5681 --- /dev/null +++ b/rednose/helpers/ekf_sym_pyx.pyx @@ -0,0 +1,190 @@ +# cython: language_level=3 +# cython: profile=True +# distutils: language = c++ + +cimport cython + +from libcpp.string cimport string +from libcpp.vector cimport vector +from libcpp cimport bool +cimport numpy as np + +import numpy as np + +cdef extern from "" namespace "std" nogil: + cdef cppclass optional[T]: + ctypedef T value_type + bool has_value() + T& value() + +cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS": + cdef cppclass MapVectorXd "Eigen::Map": + MapVectorXd(double*, int) + + cdef cppclass MapMatrixXdr "Eigen::Map >": + MapMatrixXdr(double*, int, int) + + cdef cppclass VectorXd "Eigen::VectorXd": + VectorXd() + double* data() + int rows() + + cdef cppclass MatrixXdr "Eigen::Matrix": + MatrixXdr() + double* data() + int rows() + int cols() + + ctypedef struct Estimate: + VectorXd xk1 + VectorXd xk + MatrixXdr Pk1 + MatrixXdr Pk + double t + int kind + vector[VectorXd] y + vector[VectorXd] z + vector[vector[double]] extra_args + + cdef cppclass EKFSym: + EKFSym(string name, MapMatrixXdr Q, MapVectorXd x_initial, MapMatrixXdr P_initial, int dim_main, + int dim_main_err, int N, int dim_augment, int dim_augment_err, vector[int] maha_test_kinds, + vector[int] quaternion_idxs, vector[string] global_vars, double max_rewind_age) + void init_state(MapVectorXd state, MapMatrixXdr covs, double filter_time) + + VectorXd state() + MatrixXdr covs() + void set_filter_time(double t) + double get_filter_time() + void set_global(string name, double val) + void reset_rewind() + + void predict(double t) + optional[Estimate] predict_and_update_batch(double t, int kind, vector[MapVectorXd] z, vector[MapMatrixXdr] z, + vector[vector[double]] extra_args, bool augment) + +# Functions like `numpy_to_matrix` are not possible, cython requires default +# constructor for return variable types which aren't available with Eigen::Map + +@cython.wraparound(False) +@cython.boundscheck(False) +cdef np.ndarray[np.float64_t, ndim=2, mode="c"] matrix_to_numpy(MatrixXdr arr): + cdef double[:,:] mem_view = arr.data() + return np.copy(np.asarray(mem_view, dtype=np.double, order="C")) + +@cython.wraparound(False) +@cython.boundscheck(False) +cdef np.ndarray[np.float64_t, ndim=1, mode="c"] vector_to_numpy(VectorXd arr): + cdef double[:] mem_view = arr.data() + return np.copy(np.asarray(mem_view, dtype=np.double, order="C")) + +cdef class EKF_sym: + cdef EKFSym* ekf + def __cinit__(self, str gen_dir, str name, np.ndarray[np.float64_t, ndim=2] Q, + np.ndarray[np.float64_t, ndim=1] x_initial, np.ndarray[np.float64_t, ndim=2] P_initial, int dim_main, + int dim_main_err, int N=0, int dim_augment=0, int dim_augment_err=0, list maha_test_kinds=[], + list quaternion_idxs=[], list global_vars=[], double max_rewind_age=1.0, logger=None): + # TODO logger + + cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Q_b = np.ascontiguousarray(Q, dtype=np.double) + cdef np.ndarray[np.float64_t, ndim=1, mode='c'] x_initial_b = np.ascontiguousarray(x_initial, dtype=np.double) + cdef np.ndarray[np.float64_t, ndim=2, mode='c'] P_initial_b = np.ascontiguousarray(P_initial, dtype=np.double) + self.ekf = new EKFSym( + name.encode('utf8'), + MapMatrixXdr( Q_b.data, Q.shape[0], Q.shape[1]), + MapVectorXd( x_initial_b.data, x_initial.shape[0]), + MapMatrixXdr( P_initial_b.data, P_initial.shape[0], P_initial.shape[1]), + dim_main, + dim_main_err, + N, + dim_augment, + dim_augment_err, + maha_test_kinds, + quaternion_idxs, + [x.encode('utf8') for x in global_vars], + max_rewind_age + ) + + def init_state(self, np.ndarray[np.float64_t, ndim=1] state, np.ndarray[np.float64_t, ndim=2] covs, filter_time): + cdef np.ndarray[np.float64_t, ndim=1, mode='c'] state_b = np.ascontiguousarray(state, dtype=np.double) + cdef np.ndarray[np.float64_t, ndim=2, mode='c'] covs_b = np.ascontiguousarray(covs, dtype=np.double) + self.ekf.init_state( + MapVectorXd( state_b.data, state.shape[0]), + MapMatrixXdr( covs_b.data, covs.shape[0], covs.shape[1]), + np.nan if filter_time is None else filter_time + ) + + def state(self): + cdef np.ndarray res = vector_to_numpy(self.ekf.state()) + return res + + def covs(self): + return matrix_to_numpy(self.ekf.covs()) + + def set_filter_time(self, double t): + self.ekf.set_filter_time(t) + + def get_filter_time(self): + return self.ekf.get_filter_time() + + def set_global(self, str global_var, double val): + self.ekf.set_global(global_var.encode('utf8'), val) + + def reset_rewind(self): + self.ekf.reset_rewind() + + def predict(self, double t): + self.ekf.predict(t) + + def predict_and_update_batch(self, double t, int kind, z, R, extra_args=[[]], bool augment=False): + cdef vector[MapVectorXd] z_map + cdef np.ndarray[np.float64_t, ndim=1, mode='c'] zi_b + for zi in z: + zi_b = np.ascontiguousarray(zi, dtype=np.double) + z_map.push_back(MapVectorXd( zi_b.data, zi.shape[0])) + + cdef vector[MapMatrixXdr] R_map + cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Ri_b + for Ri in R: + Ri_b = np.ascontiguousarray(Ri, dtype=np.double) + R_map.push_back(MapMatrixXdr( Ri_b.data, Ri.shape[0], Ri.shape[1])) + + cdef vector[vector[double]] extra_args_map + cdef vector[double] args_map + for args in extra_args: + args_map.clear() + for a in args: + args_map.push_back(a) + extra_args_map.push_back(args_map) + + cdef optional[Estimate] res = self.ekf.predict_and_update_batch(t, kind, z_map, R_map, extra_args_map, augment) + if not res.has_value(): + return None + + cdef VectorXd tmpvec + return ( + vector_to_numpy(res.value().xk1), + vector_to_numpy(res.value().xk), + matrix_to_numpy(res.value().Pk1), + matrix_to_numpy(res.value().Pk), + res.value().t, + res.value().kind, + [vector_to_numpy(tmpvec) for tmpvec in res.value().y], + z, # TODO: take return values? + extra_args, + ) + + def augment(self): + raise NotImplementedError() # TODO + + def get_augment_times(self): + raise NotImplementedError() # TODO + + def rts_smooth(self, estimates, norm_quats=False): + raise NotImplementedError() # TODO + + def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95): + raise NotImplementedError() # TODO + + def __dealloc__(self): + del self.ekf diff --git a/rednose/helpers/feature_handler.py b/rednose/helpers/feature_handler.py index f8ac65145..6a20b85e1 100755 --- a/rednose/helpers/feature_handler.py +++ b/rednose/helpers/feature_handler.py @@ -35,7 +35,9 @@ class FeatureHandler(): c_code = "#include \n" c_code += "#include \n" c_code += "#define K %d\n" % K + c_code += "extern \"C\" {\n" c_code += "\n" + open(os.path.join(TEMPLATE_DIR, "feature_handler.c")).read() + c_code += "\n}\n" filename = f"{FeatureHandler.name}_{K}" write_code(generated_dir, filename, c_code, c_header) diff --git a/rednose/__init__.py b/rednose/helpers/kalmanfilter.py similarity index 74% rename from rednose/__init__.py rename to rednose/helpers/kalmanfilter.py index 25f82adc7..0c30e4916 100644 --- a/rednose/__init__.py +++ b/rednose/helpers/kalmanfilter.py @@ -2,8 +2,6 @@ from typing import Any, Dict import numpy as np -from rednose.helpers.ekf_sym import EKF_sym - class KalmanFilter: name = "" @@ -12,12 +10,7 @@ class KalmanFilter: Q = np.zeros((0, 0)) obs_noise: Dict[int, Any] = {} - def __init__(self, generated_dir): - dim_state = self.initial_x.shape[0] - dim_state_err = self.initial_P_diag.shape[0] - - # init filter - self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err) + filter = None # Should be initialized when initializating a KalmanFilter implementation @property def x(self): @@ -25,7 +18,7 @@ class KalmanFilter: @property def t(self): - return self.filter.filter_time + return self.filter.get_filter_time() @property def P(self): diff --git a/rednose/helpers/lst_sq_computer.py b/rednose/helpers/lst_sq_computer.py index 18fd6d7f5..56c3bc8c4 100755 --- a/rednose/helpers/lst_sq_computer.py +++ b/rednose/helpers/lst_sq_computer.py @@ -48,14 +48,16 @@ class LstSqComputer(): @staticmethod def generate_code(generated_dir, K=4): sympy_functions = generate_residual(K) - header, code = sympy_into_c(sympy_functions) + header, sympy_code = sympy_into_c(sympy_functions) + code = "\n#include \"rednose/helpers/common_ekf.h\"\n" code += "\n#define KDIM %d\n" % K - code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read() + code += "extern \"C\" {\n" + code += sympy_code + code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read() + "\n" + code += "}\n" - header += """ - void compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos); - """ + header += "\nvoid compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);\n" filename = f"{LstSqComputer.name}_{K}" write_code(generated_dir, filename, code, header) diff --git a/rednose/helpers/sympy_helpers.py b/rednose/helpers/sympy_helpers.py index 12b7188e7..716e94e01 100644 --- a/rednose/helpers/sympy_helpers.py +++ b/rednose/helpers/sympy_helpers.py @@ -151,6 +151,5 @@ def sympy_into_c(sympy_functions, global_vars=None): c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#') c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#') - c_code = 'extern "C" {\n#include \n' + c_code + "\n}\n" return c_header, c_code diff --git a/rednose/templates/compute_pos.c b/rednose/templates/compute_pos.c index 3c7b16efa..742c7d618 100644 --- a/rednose/templates/compute_pos.c +++ b/rednose/templates/compute_pos.c @@ -7,7 +7,6 @@ typedef Eigen::Matrix R1M; typedef Eigen::Matrix O1M; typedef Eigen::Matrix M3D; -extern "C" { void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) { double res[KDIM*2] = {0}; @@ -51,4 +50,3 @@ void compute_pos(double *to_c, double *poses, double *img_positions, double *par ecef_output = rot*ecef_output + ecef_offset; memcpy(pos, ecef_output.data(), 3 * sizeof(double)); } -} diff --git a/rednose/templates/feature_handler.c b/rednose/templates/feature_handler.c index 330972339..9c580c2ea 100644 --- a/rednose/templates/feature_handler.c +++ b/rednose/templates/feature_handler.c @@ -1,4 +1,3 @@ -extern "C"{ bool sane(double track [K + 1][5]) { double diffs_x [K-1]; double diffs_y [K-1]; @@ -55,4 +54,3 @@ void merge_features(double *tracks, double *features, long long *empty_idxs) { } memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double)); } -} diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 5191e3224..e71b5b80e 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -17,7 +17,7 @@ from typing import Any import requests from jsonrpc import JSONRPCResponseManager, dispatcher -from websocket import ABNF, WebSocketTimeoutException, create_connection +from websocket import ABNF, WebSocketTimeoutException, WebSocketException, create_connection import cereal.messaging as messaging from cereal.services import service_list @@ -29,6 +29,8 @@ from selfdrive.hardware import HARDWARE, PC from selfdrive.loggerd.config import ROOT from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.swaglog import cloudlog, SWAGLOG_DIR +import selfdrive.crash as crash +from selfdrive.version import dirty, origin, branch, commit ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) @@ -409,7 +411,12 @@ def backoff(retries): def main(): params = Params() - dongle_id = params.get("DongleId").decode('utf-8') + dongle_id = params.get("DongleId", encoding='utf-8') + crash.init() + crash.bind_user(id=dongle_id) + crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit, + device=HARDWARE.get_device_type()) + ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id api = Api(dongle_id) @@ -426,8 +433,13 @@ def main(): handle_long_poll(ws) except (KeyboardInterrupt, SystemExit): break + except (ConnectionError, TimeoutError, WebSocketException): + conn_retries += 1 + params.delete("LastAthenaPingTime") except Exception: + crash.capture_exception() cloudlog.exception("athenad.main.exception") + conn_retries += 1 params.delete("LastAthenaPingTime") diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index c16bef07d..7bb717e07 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -3,7 +3,6 @@ import time from multiprocessing import Process -import selfdrive.crash as crash from common.params import Params from selfdrive.manager.process import launcher from selfdrive.swaglog import cloudlog @@ -16,9 +15,6 @@ def main(): params = Params() dongle_id = params.get("DongleId").decode('utf-8') cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty) - crash.bind_user(id=dongle_id) - crash.bind_extra(version=version, dirty=dirty) - crash.install() try: while 1: diff --git a/selfdrive/registration.py b/selfdrive/athena/registration.py similarity index 67% rename from selfdrive/registration.py rename to selfdrive/athena/registration.py index b38853a9f..6bcb29e4b 100644 --- a/selfdrive/registration.py +++ b/selfdrive/athena/registration.py @@ -10,27 +10,22 @@ from common.params import Params from common.spinner import Spinner from common.file_helpers import mkdirs_exists_ok from common.basedir import PERSIST +from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.hardware import HARDWARE from selfdrive.swaglog import cloudlog -from selfdrive.version import version, terms_version, training_version, get_git_commit, \ - get_git_branch, get_git_remote -def register(show_spinner=False): +UNREGISTERED_DONGLE_ID = "UnregisteredDevice" + + +def register(show_spinner=False) -> str: params = Params() - params.put("Version", version) - params.put("TermsVersion", terms_version) - params.put("TrainingVersion", training_version) - - params.put("GitCommit", get_git_commit(default="")) - params.put("GitBranch", get_git_branch(default="")) - params.put("GitRemote", get_git_remote(default="")) params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) IMEI = params.get("IMEI", encoding='utf8') HardwareSerial = params.get("HardwareSerial", encoding='utf8') - - needs_registration = (None in [IMEI, HardwareSerial]) + dongle_id = params.get("DongleId", encoding='utf8') + needs_registration = None in (IMEI, HardwareSerial, dongle_id) # create a key for auth # your private key is kept on your device persist partition and never sent to our servers @@ -44,22 +39,15 @@ def register(show_spinner=False): os.rename(PERSIST+"/comma/id_rsa.tmp", PERSIST+"/comma/id_rsa") os.rename(PERSIST+"/comma/id_rsa.tmp.pub", PERSIST+"/comma/id_rsa.pub") - # make key readable by app users (ai.comma.plus.offroad) - os.chmod(PERSIST+'/comma/', 0o755) - os.chmod(PERSIST+'/comma/id_rsa', 0o744) - - dongle_id = params.get("DongleId", encoding='utf8') - needs_registration = needs_registration or dongle_id is None - if needs_registration: if show_spinner: spinner = Spinner() spinner.update("registering device") # Create registration token, in the future, this key will make JWTs directly - private_key = open(PERSIST+"/comma/id_rsa").read() - public_key = open(PERSIST+"/comma/id_rsa.pub").read() - register_token = jwt.encode({'register': True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256') + with open(PERSIST+"/comma/id_rsa.pub") as f1, open(PERSIST+"/comma/id_rsa") as f2: + public_key = f1.read() + private_key = f2.read() # Block until we get the imei imei1, imei2 = None, None @@ -74,22 +62,32 @@ def register(show_spinner=False): params.put("IMEI", imei1) params.put("HardwareSerial", serial) + backoff = 0 while True: try: + register_token = jwt.encode({'register': True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256') cloudlog.info("getting pilotauth") resp = api_get("v2/pilotauth/", method='POST', timeout=15, imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token) - dongleauth = json.loads(resp.text) - dongle_id = dongleauth["dongle_id"] - params.put("DongleId", dongle_id) + + if resp.status_code in (402, 403): + cloudlog.info(f"Unable to register device, got {resp.status_code}") + dongle_id = UNREGISTERED_DONGLE_ID + else: + dongleauth = json.loads(resp.text) + dongle_id = dongleauth["dongle_id"] break except Exception: cloudlog.exception("failed to authenticate") - time.sleep(1) + backoff = min(backoff + 1, 15) + time.sleep(backoff) if show_spinner: spinner.close() + if dongle_id: + params.put("DongleId", dongle_id) + set_offroad_alert("Offroad_UnofficialHardware", dongle_id == UNREGISTERED_DONGLE_ID) return dongle_id diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index a93e05d24..f1f885b0c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -1,36 +1,35 @@ -#include -#include -#include -#include -#include #include +#include +#include +#include +#include #include -#include #include +#include +#include -#include -#include -#include #include -#include -#include #include +#include +#include +#include +#include +#include #include #include #include "cereal/gen/cpp/car.capnp.h" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/locationd/ublox_msg.h" -#include "common/util.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/timing.h" -#include "messaging.hpp" -#include "locationd/ublox_msg.h" - -#include "panda.h" -#include "pigeon.h" - +#include "selfdrive/boardd/panda.h" +#include "selfdrive/boardd/pigeon.h" #define MAX_IR_POWER 0.5f #define MIN_IR_POWER 0.0f @@ -64,6 +63,8 @@ void safety_setter_thread() { // diagnostic only is the default, needed for VIN query panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327); + Params p = Params(); + // switch to SILENT when CarVin param is read while (true) { if (do_exit || !panda->connected){ @@ -71,12 +72,11 @@ void safety_setter_thread() { return; }; - std::vector value_vin = Params().read_db_bytes("CarVin"); + std::string value_vin = p.get("CarVin"); if (value_vin.size() > 0) { // sanity check VIN format assert(value_vin.size() == 17); - std::string str_vin(value_vin.begin(), value_vin.end()); - LOGW("got CarVin %s", str_vin.c_str()); + LOGW("got CarVin %s", value_vin.c_str()); break; } util::sleep_for(100); @@ -85,7 +85,7 @@ void safety_setter_thread() { // VIN query done, stop listening to OBDII panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - std::vector params; + std::string params; LOGW("waiting for params to set safety model"); while (true) { if (do_exit || !panda->connected){ @@ -93,8 +93,10 @@ void safety_setter_thread() { return; }; - params = Params().read_db_bytes("CarParams"); - if (params.size() > 0) break; + if (p.getBool("ControlsReady")) { + params = p.get("CarParams"); + if (params.size() > 0) break; + } util::sleep_for(100); } LOGW("got %d bytes CarParams", params.size()); @@ -133,7 +135,7 @@ bool usb_connect() { } if (auto fw_sig = tmp_panda->get_firmware_version(); fw_sig) { - params.write_db_value("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size()); + params.put("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size()); // Convert to hex for offroad char fw_sig_hex_buf[16] = {0}; @@ -143,13 +145,13 @@ bool usb_connect() { fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF); } - params.write_db_value("PandaFirmwareHex", fw_sig_hex_buf, 16); + params.put("PandaFirmwareHex", fw_sig_hex_buf, 16); LOGW("fw signature: %.*s", 16, fw_sig_hex_buf); } else { return false; } // get panda serial if (auto serial = tmp_panda->get_serial(); serial) { - params.write_db_value("PandaDongleId", serial->c_str(), serial->length()); + params.put("PandaDongleId", serial->c_str(), serial->length()); LOGW("panda serial: %s", serial->c_str()); } else { return false; } @@ -161,13 +163,18 @@ bool usb_connect() { #endif if (tmp_panda->has_rtc){ + setenv("TZ","UTC",1); struct tm sys_time = get_time(); struct tm rtc_time = tmp_panda->get_rtc(); if (!time_valid(sys_time) && time_valid(rtc_time)) { - LOGE("System time wrong, setting from RTC"); + LOGE("System time wrong, setting from RTC. " + "System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d", + sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday, + sys_time.tm_hour, sys_time.tm_min, sys_time.tm_sec, + rtc_time.tm_year + 1900, rtc_time.tm_mon + 1, rtc_time.tm_mday, + rtc_time.tm_hour, rtc_time.tm_min, rtc_time.tm_sec); - setenv("TZ","UTC",1); const struct timeval tv = {mktime(&rtc_time), 0}; settimeofday(&tv, 0); } @@ -313,10 +320,7 @@ void panda_state_thread(bool spoofing_started) { // clear VIN, CarParams, and set new safety on car start if (ignition && !ignition_last) { - int result = params.delete_db_value("CarVin"); - assert((result == 0) || (result == ERR_NO_VALUE)); - result = params.delete_db_value("CarParams"); - assert((result == 0) || (result == ERR_NO_VALUE)); + params.clearAll(CLEAR_ON_IGNITION); if (!safety_setter_thread_running) { safety_setter_thread_running = true; @@ -329,9 +333,23 @@ void panda_state_thread(bool spoofing_started) { // Write to rtc once per minute when no ignition present if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)){ // Write time to RTC if it looks reasonable + setenv("TZ","UTC",1); struct tm sys_time = get_time(); + if (time_valid(sys_time)){ - panda->set_rtc(sys_time); + struct tm rtc_time = panda->get_rtc(); + double seconds = difftime(mktime(&rtc_time), mktime(&sys_time)); + + if (std::abs(seconds) > 1.1) { + panda->set_rtc(sys_time); + LOGW("Updating panda RTC. dt = %.2f " + "System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d", + seconds, + sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday, + sys_time.tm_hour, sys_time.tm_min, sys_time.tm_sec, + rtc_time.tm_year + 1900, rtc_time.tm_mon + 1, rtc_time.tm_mday, + rtc_time.tm_hour, rtc_time.tm_min, rtc_time.tm_sec); + } } } @@ -343,13 +361,18 @@ void panda_state_thread(bool spoofing_started) { auto ps = msg.initEvent().initPandaState(); ps.setUptime(pandaState.uptime); -#ifdef QCOM2 - ps.setVoltage(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input"))); - ps.setCurrent(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input"))); -#else - ps.setVoltage(pandaState.voltage); - ps.setCurrent(pandaState.current); -#endif + if (Hardware::TICI()) { + double read_time = millis_since_boot(); + ps.setVoltage(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input"))); + ps.setCurrent(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input"))); + read_time = millis_since_boot() - read_time; + if (read_time > 50) { + LOGW("reading hwmon took %lfms", read_time); + } + } else { + ps.setVoltage(pandaState.voltage); + ps.setCurrent(pandaState.current); + } ps.setIgnitionLine(pandaState.ignition_line); ps.setIgnitionCan(pandaState.ignition_can); @@ -394,17 +417,14 @@ void hardware_control_thread() { uint16_t prev_fan_speed = 999; uint16_t ir_pwr = 0; uint16_t prev_ir_pwr = 999; -#if defined(QCOM) || defined(QCOM2) bool prev_charging_disabled = false; -#endif unsigned int cnt = 0; while (!do_exit && panda->connected) { cnt++; sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? -#if defined(QCOM) || defined(QCOM2) - if (sm.updated("deviceState")){ + if (!Hardware::PC() && sm.updated("deviceState")){ // Charging mode bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled(); if (charging_disabled != prev_charging_disabled){ @@ -418,7 +438,6 @@ void hardware_control_thread() { prev_charging_disabled = charging_disabled; } } -#endif // Other pandas don't have fan/IR to control if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue; @@ -468,16 +487,12 @@ void pigeon_thread() { PubMaster pm({"ubloxRaw"}); bool ignition_last = false; -#ifdef QCOM2 - Pigeon *pigeon = Pigeon::connect("/dev/ttyHS0"); -#else - Pigeon *pigeon = Pigeon::connect(panda); -#endif + Pigeon *pigeon = Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda); std::unordered_map last_recv_time; std::unordered_map cls_max_dt = { - {(char)ublox::CLASS_NAV, int64_t(250000000ULL)}, // 0.25s - {(char)ublox::CLASS_RXM, int64_t(250000000ULL)}, // 0.25s + {(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s + {(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s }; while (!do_exit && panda->connected) { @@ -548,10 +563,9 @@ int main() { // set process priority and affinity err = set_realtime_priority(54); LOG("set priority returns %d", err); - err = set_core_affinity(3); - LOG("set affinity returns %d", err); - panda_set_power(true); + err = set_core_affinity(Hardware::TICI() ? 4 : 3); + LOG("set affinity returns %d", err); while (!do_exit){ std::vector threads; diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/boardd/boardd_api_impl.pyx index 3f50fbaab..0d428a925 100644 --- a/selfdrive/boardd/boardd_api_impl.pyx +++ b/selfdrive/boardd/boardd_api_impl.pyx @@ -14,6 +14,8 @@ cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, st def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True): cdef vector[can_frame] can_list + can_list.reserve(len(can_msgs)) + cdef can_frame f for can_msg in can_msgs: f.address = can_msg[0] diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc index bbbceee6e..a4c9f3f6c 100644 --- a/selfdrive/boardd/can_list_to_can_capnp.cc +++ b/selfdrive/boardd/can_list_to_can_capnp.cc @@ -1,4 +1,4 @@ -#include "messaging.hpp" +#include "cereal/messaging/messaging.h" typedef struct { long address; diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index e8cb23e10..71f5770ed 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -1,31 +1,16 @@ -#include -#include -#include -#include +#include "selfdrive/boardd/panda.h" + #include -#include "common/swaglog.h" -#include "common/gpio.h" -#include "common/util.h" -#include "messaging.hpp" -#include "panda.h" +#include +#include +#include +#include -void panda_set_power(bool power){ -#ifdef QCOM2 - int err = 0; - - err += gpio_init(GPIO_STM_RST_N, true); - err += gpio_init(GPIO_STM_BOOT0, true); - - err += gpio_set(GPIO_STM_RST_N, true); - err += gpio_set(GPIO_STM_BOOT0, false); - - util::sleep_for(100); // 100 ms - - err += gpio_set(GPIO_STM_RST_N, !power); - assert(err == 0); -#endif -} +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/gpio.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" Panda::Panda(){ // init libusb diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index d34a01c9b..e725da4fa 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -1,11 +1,13 @@ #pragma once -#include -#include #include + +#include +#include +#include #include -#include #include +#include #include @@ -39,8 +41,6 @@ struct __attribute__((packed)) health_t { }; -void panda_set_power(bool power); - class Panda { private: libusb_context *ctx = NULL; diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index 3f2588f25..937925e48 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -1,14 +1,15 @@ -#include -#include -#include +#include "selfdrive/boardd/pigeon.h" + #include +#include #include +#include -#include "common/swaglog.h" -#include "common/gpio.h" -#include "common/util.h" +#include -#include "pigeon.h" +#include "selfdrive/common/gpio.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" // Termios on macos doesn't define all baud rate constants #ifndef B460800 @@ -17,6 +18,11 @@ using namespace std::string_literals; +extern ExitHandler do_exit; + +const std::string ack = "\xb5\x62\x05\x01\x02\x00"; +const std::string nack = "\xb5\x62\x05\x00\x02\x00"; + Pigeon * Pigeon::connect(Panda * p){ PandaPigeon * pigeon = new PandaPigeon(); @@ -32,53 +38,83 @@ Pigeon * Pigeon::connect(const char * tty){ return pigeon; } +bool Pigeon::wait_for_ack(){ + std::string s; + while (!do_exit){ + s += receive(); + + if (s.find(ack) != std::string::npos){ + LOGD("Received ACK from ublox"); + return true; + } else if (s.find(nack) != std::string::npos) { + LOGE("Received NACK from ublox"); + return false; + } else if (s.size() > 0x1000) { + LOGE("No response from ublox"); + return false; + } + + util::sleep_for(1); // Allow other threads to be scheduled + } + return false; +} + +bool Pigeon::send_with_ack(std::string cmd){ + send(cmd); + return wait_for_ack(); +} + void Pigeon::init() { - util::sleep_for(1000); - LOGW("panda GPS start"); + for (int i = 0; i < 10; i++){ + if (do_exit) return; + LOGW("panda GPS start"); - // power off pigeon - set_power(false); - util::sleep_for(100); + // power off pigeon + set_power(false); + util::sleep_for(100); - // 9600 baud at init - set_baud(9600); + // 9600 baud at init + set_baud(9600); - // power on pigeon - set_power(true); - util::sleep_for(500); + // power on pigeon + set_power(true); + util::sleep_for(500); - // baud rate upping - send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s); - util::sleep_for(100); + // baud rate upping + send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s); + util::sleep_for(100); - // set baud rate to 460800 - set_baud(460800); - util::sleep_for(100); + // set baud rate to 460800 + set_baud(460800); - // init from ubloxd - // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py - send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s); - send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"s); - send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s); - send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s); - send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s); - send("\xB5\x62\x06\x00\x00\x00\x06\x18"s); - send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s); - send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23"s); - send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s); - send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s); - send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s); - send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s); - send("\xB5\x62\x06\x24\x00\x00\x2A\x84"s); - send("\xB5\x62\x06\x23\x00\x00\x29\x81"s); - send("\xB5\x62\x06\x1E\x00\x00\x24\x72"s); - send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s); - send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s); - send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s); - send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s); - send("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s); + // init from ubloxd + // To generate this data, run selfdrive/locationd/test/ubloxd.py + if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x00\x00\x00\x06\x18"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x24\x00\x00\x2A\x84"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x23\x00\x00\x29\x81"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x1E\x00\x00\x24\x72"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x39\x00\x00\x3F\xC3"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue; + if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue; - LOGW("panda GPS on"); + + LOGW("panda GPS on"); + return; + } + LOGE("failed to initialize panda GPS"); } void PandaPigeon::connect(Panda * p) { diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h index 927d74058..0543c4828 100644 --- a/selfdrive/boardd/pigeon.h +++ b/selfdrive/boardd/pigeon.h @@ -1,9 +1,11 @@ #pragma once -#include + #include +#include +#include -#include "panda.h" +#include "selfdrive/boardd/panda.h" class Pigeon { public: @@ -12,6 +14,8 @@ class Pigeon { virtual ~Pigeon(){}; void init(); + bool wait_for_ack(); + bool send_with_ack(std::string cmd); virtual void set_baud(int baud) = 0; virtual void send(const std::string &s) = 0; virtual std::string receive() = 0; diff --git a/selfdrive/boardd/set_time.py b/selfdrive/boardd/set_time.py new file mode 100755 index 000000000..7d17be4de --- /dev/null +++ b/selfdrive/boardd/set_time.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +import datetime +import os +import struct +import usb1 + +REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE +MIN_DATE = datetime.datetime(year=2021, month=4, day=1) + +def set_time(logger): + sys_time = datetime.datetime.today() + if sys_time > MIN_DATE: + logger.info("System time valid") + return + + try: + ctx = usb1.USBContext() + dev = ctx.openByVendorIDAndProductID(0xbbaa, 0xddcc) + if dev is None: + logger.info("No panda found") + return + + # Set system time from panda RTC time + dat = dev.controlRead(REQUEST_IN, 0xa0, 0, 0, 8) + a = struct.unpack("HBBBBBB", dat) + panda_time = datetime.datetime(a[0], a[1], a[2], a[4], a[5], a[6]) + if panda_time > MIN_DATE: + logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'") + os.system(f"TZ=UTC date -s '{panda_time}'") + except Exception: + logger.warn("Failed to fetch time from panda") + +if __name__ == "__main__": + import logging + logging.basicConfig(level=logging.INFO) + + set_time(logging) diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index b431eecac..6294c1580 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -1,29 +1,31 @@ -#include -#include -#include +#include "selfdrive/camerad/cameras/camera_common.h" + #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 "libyuv.h" #include -#include "clutil.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "modeldata.h" -#include "imgproc/utils.h" +#include "selfdrive/camerad/imgproc/utils.h" +#include "selfdrive/common/clutil.h" +#include "selfdrive/common/modeldata.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" + +#if defined(QCOM) && !defined(QCOM_REPLAY) +#include "selfdrive/camerad/cameras/camera_qcom.h" +#elif QCOM2 +#include "selfdrive/camerad/cameras/camera_qcom2.h" +#elif WEBCAM +#include "selfdrive/camerad/cameras/camera_webcam.h" +#else +#include "selfdrive/camerad/cameras/camera_frame_stream.h" +#endif static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b, const CameraState *s) { char args[4096]; @@ -35,11 +37,8 @@ static cl_program build_debayer_program(cl_device_id device_id, cl_context conte ci->frame_width, ci->frame_height, ci->frame_stride, b->rgb_width, b->rgb_height, b->rgb_stride, ci->bayer_flip, ci->hdr, s->camera_num); -#ifdef QCOM2 - return cl_program_from_file(context, device_id, "cameras/real_debayer.cl", args); -#else - return cl_program_from_file(context, device_id, "cameras/debayer.cl", args); -#endif + const char *cl_file = Hardware::TICI() ? "cameras/real_debayer.cl" : "cameras/debayer.cl"; + return cl_program_from_file(context, device_id, cl_file, args); } void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType rgb_type, VisionStreamType yuv_type, release_cb release_callback) { @@ -64,13 +63,13 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, rgb_width = ci->frame_width; rgb_height = ci->frame_height; -#ifndef QCOM2 - // debayering does a 2x downscale - if (ci->bayer) { + + if (!Hardware::TICI() && ci->bayer) { + // debayering does a 2x downscale rgb_width = ci->frame_width / 2; rgb_height = ci->frame_height / 2; } -#endif + yuv_transform = get_model_yuv_transform(ci->bayer); vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height); @@ -84,7 +83,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, CL_CHECK(clReleaseProgram(prg_debayer)); } - rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, rgb_width, rgb_height, rgb_stride); + rgb2yuv = std::make_unique(context, device_id, rgb_width, rgb_height, rgb_stride); #ifdef __APPLE__ q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); @@ -99,7 +98,6 @@ CameraBuf::~CameraBuf() { camera_bufs[i].free(); } - if (rgb_to_yuv_state.rgb_to_yuv_krnl) rgb_to_yuv_destroy(&rgb_to_yuv_state); if (krnl_debayer) CL_CHECK(clReleaseKernel(krnl_debayer)); if (q) CL_CHECK(clReleaseCommandQueue(q)); } @@ -114,7 +112,6 @@ bool CameraBuf::acquire() { } cur_frame_data = camera_bufs_metadata[cur_buf_idx]; - cur_rgb_buf = vipc_server->get_buffer(rgb_type); cl_event debayer_event; @@ -151,7 +148,7 @@ bool CameraBuf::acquire() { CL_CHECK(clReleaseEvent(debayer_event)); cur_yuv_buf = vipc_server->get_buffer(yuv_type); - rgb_to_yuv_queue(&rgb_to_yuv_state, q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl); + rgb2yuv->queue(q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl); VisionIpcBufExtra extra = { cur_frame_data.frame_id, @@ -348,73 +345,60 @@ std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, pro return std::thread(processing_thread, cameras, cs, callback); } -void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) { +static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { + static const bool is_rhd = Params().getBool("IsRHD"); + struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; const CameraBuf *b = &c->buf; - static int x_min = 0, x_max = 0, y_min = 0, y_max = 0; - static const bool is_rhd = Params().read_db_bool("IsRHD"); - - // auto exposure - if (cnt % 3 == 0) { - if (sm->update(0) > 0 && sm->updated("driverState")) { - auto state = (*sm)["driverState"].getDriverState(); - // set driver camera metering target - if (state.getFaceProb() > 0.4) { - auto face_position = state.getFacePosition(); + bool hist_ceil = false, hl_weighted = false; + int x_offset = 0, y_offset = 0; + int frame_width = b->rgb_width, frame_height = b->rgb_height; #ifndef QCOM2 - int frame_width = b->rgb_width; - int frame_height = b->rgb_height; + int analog_gain = -1; #else - int frame_width = 668; - int frame_height = frame_width / 1.33; + int analog_gain = c->analog_gain; #endif - int x_offset = is_rhd ? 0 : frame_width - (0.5 * frame_height); - x_offset += (face_position[0] * (is_rhd ? -1.0 : 1.0) + 0.5) * (0.5 * frame_height); - int y_offset = (face_position[1] + 0.5) * frame_height; -#ifdef QCOM2 - x_offset += 630; - y_offset += 156; -#endif - x_min = std::max(0, x_offset - 72); - x_max = std::min(b->rgb_width - 1, x_offset + 72); - y_min = std::max(0, y_offset - 72); - y_max = std::min(b->rgb_height - 1, y_offset + 72); - } else { // use default setting if no face - x_min = x_max = y_min = y_max = 0; - } - } - int skip = 1; - // use driver face crop for AE - if (x_max == 0) { - // default setting -#ifndef QCOM2 - x_min = is_rhd ? 0 : b->rgb_width * 3 / 5; - x_max = is_rhd ? b->rgb_width * 2 / 5 : b->rgb_width; - y_min = b->rgb_height / 3; - y_max = b->rgb_height; -#else - x_min = 96; - x_max = 1832; - y_min = 242; - y_max = 1148; - skip = 4; -#endif - } - -#ifdef QCOM2 - camera_autoexposure(c, set_exposure_target(b, x_min, x_max, 2, y_min, y_max, skip, (int)c->analog_gain, true, true)); -#else - camera_autoexposure(c, set_exposure_target(b, x_min, x_max, 2, y_min, y_max, skip, -1, false, false)); -#endif + ExpRect def_rect; + if (Hardware::TICI()) { + hist_ceil = hl_weighted = true; + x_offset = 630, y_offset = 156; + frame_width = 668, frame_height = frame_width / 1.33; + def_rect = {96, 1832, 2, 242, 1148, 4}; + } else { + def_rect = {is_rhd ? 0 : b->rgb_width * 3 / 5, is_rhd ? b->rgb_width * 2 / 5 : b->rgb_width, 2, + b->rgb_height / 3, b->rgb_height, 1}; } + static ExpRect rect = def_rect; + // use driver face crop for AE + if (sm.updated("driverState")) { + if (auto state = sm["driverState"].getDriverState(); state.getFaceProb() > 0.4) { + auto face_position = state.getFacePosition(); + int x = is_rhd ? 0 : frame_width - (0.5 * frame_height); + x += (face_position[0] * (is_rhd ? -1.0 : 1.0) + 0.5) * (0.5 * frame_height) + x_offset; + int y = (face_position[1] + 0.5) * frame_height + y_offset; + rect = {std::max(0, x - 72), std::min(b->rgb_width - 1, x + 72), 2, + std::max(0, y - 72), std::min(b->rgb_height - 1, y + 72), 1}; + } else { + rect = def_rect; + } + } + + camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip, analog_gain, hist_ceil, hl_weighted)); +} + +void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) { + if (cnt % 3 == 0) { + sm->update(0); + driver_cam_auto_exposure(c, *sm); + } MessageBuilder msg; auto framed = msg.initEvent().initDriverCameraState(); framed.setFrameType(cereal::FrameData::FrameType::FRONT); - fill_frame_data(framed, b->cur_frame_data); + fill_frame_data(framed, c->buf.cur_frame_data); if (env_send_driver) { - framed.setImage(get_frame_image(b)); + framed.setImage(get_frame_image(&c->buf)); } pm->send("driverCameraState", msg); } diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 55c25214e..337d33bdd 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -1,20 +1,21 @@ #pragma once -#include #include #include +#include + #include #include -#include "common/mat.h" -#include "common/swaglog.h" -#include "common/queue.h" -#include "visionbuf.h" -#include "common/visionimg.h" -#include "messaging.hpp" -#include "transforms/rgb_to_yuv.h" -#include "visionipc.h" -#include "visionipc_server.h" +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionbuf.h" +#include "cereal/visionipc/visionipc.h" +#include "cereal/visionipc/visionipc_server.h" +#include "selfdrive/camerad/transforms/rgb_to_yuv.h" +#include "selfdrive/common/mat.h" +#include "selfdrive/common/queue.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/visionimg.h" #define CAMERA_ID_IMX298 0 #define CAMERA_ID_IMX179 1 @@ -94,7 +95,7 @@ private: CameraState *camera_state; cl_kernel krnl_debayer; - RGBToYUVState rgb_to_yuv_state; + std::unique_ptr rgb2yuv; VisionStreamType rgb_type, yuv_type; diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index d6d44748f..a03feb132 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -1,12 +1,12 @@ -#include "camera_frame_stream.h" +#include "selfdrive/camerad/cameras/camera_frame_stream.h" #include #include #include -#include "messaging.hpp" -#include "common/util.h" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/util.h" #define FRAME_WIDTH 1164 #define FRAME_HEIGHT 874 @@ -35,7 +35,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { }; void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { - assert(camera_id < ARRAYSIZE(cameras_supported)); + assert(camera_id < std::size(cameras_supported)); s->ci = cameras_supported[camera_id]; assert(s->ci.frame_width != 0); @@ -49,25 +49,26 @@ void run_frame_stream(CameraState &camera, const char* frame_pkt) { size_t buf_idx = 0; while (!do_exit) { - if (sm.update(1000) == 0) continue; + sm.update(1000); + if(sm.updated(frame_pkt)){ + auto msg = static_cast(sm[frame_pkt]); + auto frame = msg.get(frame_pkt).as(); + camera.buf.camera_bufs_metadata[buf_idx] = { + .frame_id = frame.get("frameId").as(), + .timestamp_eof = frame.get("timestampEof").as(), + .frame_length = frame.get("frameLength").as(), + .integ_lines = frame.get("integLines").as(), + .global_gain = frame.get("globalGain").as(), + }; - auto msg = static_cast(sm[frame_pkt]); - auto frame = msg.get(frame_pkt).as(); - camera.buf.camera_bufs_metadata[buf_idx] = { - .frame_id = frame.get("frameId").as(), - .timestamp_eof = frame.get("timestampEof").as(), - .frame_length = frame.get("frameLength").as(), - .integ_lines = frame.get("integLines").as(), - .global_gain = frame.get("globalGain").as(), - }; + cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q; + cl_mem yuv_cl = camera.buf.camera_bufs[buf_idx].buf_cl; - cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q; - cl_mem yuv_cl = camera.buf.camera_bufs[buf_idx].buf_cl; - - auto image = frame.get("image").as(); - clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, image.size(), image.begin(), 0, NULL, NULL); - camera.buf.queue(buf_idx); - buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT; + auto image = frame.get("image").as(); + clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, image.size(), image.begin(), 0, NULL, NULL); + camera.buf.queue(buf_idx); + buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT; + } } } diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 40628e52f..7d5b1611c 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -1,32 +1,31 @@ -#include -#include +#include "selfdrive/camerad/cameras/camera_qcom.h" + #include -#include #include #include #include +#include +#include +#include #include -#include -#include +#include -#include +#include +#include #include +#include -#include -#include "msmb_isp.h" -#include "msmb_ispif.h" -#include "msmb_camera.h" -#include "msm_cam_sensor.h" - -#include "common/util.h" -#include "common/timing.h" -#include "common/swaglog.h" -#include "common/params.h" -#include "clutil.h" - -#include "sensor_i2c.h" -#include "camera_qcom.h" +#include "selfdrive/camerad/cameras/sensor_i2c.h" +#include "selfdrive/camerad/include/msm_cam_sensor.h" +#include "selfdrive/camerad/include/msmb_camera.h" +#include "selfdrive/camerad/include/msmb_isp.h" +#include "selfdrive/camerad/include/msmb_ispif.h" +#include "selfdrive/common/clutil.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" // leeco actuator (DW9800W H-Bridge Driver IC) // from sniff @@ -34,6 +33,13 @@ const uint16_t INFINITY_DAC = 364; extern ExitHandler do_exit; +static int cam_ioctl(int fd, unsigned long int request, void *arg, const char *log_msg = nullptr) { + int err = ioctl(fd, request, arg); + if (err != 0 && log_msg) { + LOG(util::string_format("%s: %d", log_msg, err).c_str()); + } + return err; +} // global var for AE/AF ops std::atomic road_cam_exp{{0}}; std::atomic driver_cam_exp{{0}}; @@ -93,10 +99,10 @@ int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size return ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &cfg_data); } -static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { +static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, uint32_t frame_length) { int analog_gain = std::min(gain, 448); s->digital_gain = gain > 448 ? (512.0/(512-(gain))) / 8.0 : 1.0; - //printf("%5d/%5d %5d %f\n", s->cur_integ_lines, s->cur_frame_length, analog_gain, s->digital_gain); + //printf("%5d/%5d %5d %f\n", s->cur_integ_lines, s->frame_length, analog_gain, s->digital_gain); struct msm_camera_i2c_reg_array reg_array[] = { // REG_HOLD @@ -120,17 +126,17 @@ static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int // REG_HOLD {0x104,0x0,0}, }; - return sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + return sensor_write_regs(s, reg_array, std::size(reg_array), MSM_CAMERA_I2C_BYTE_DATA); } -static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { +static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, uint32_t frame_length) { //printf("driver camera: %d %d %d\n", gain, integ_lines, frame_length); int coarse_gain_bitmap, fine_gain_bitmap; // get bitmaps from iso static const int gains[] = {0, 100, 200, 400, 800}; int i; - for (i = 1; i < ARRAYSIZE(gains); i++) { + for (i = 1; i < std::size(gains); i++) { if (gain >= gains[i - 1] && gain < gains[i]) break; } @@ -155,7 +161,7 @@ static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int //{0x104,0x0,0}, }; - return sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + return sensor_write_regs(s, reg_array, std::size(reg_array), MSM_CAMERA_I2C_BYTE_DATA); } static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int camera_num, @@ -165,14 +171,14 @@ static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int c s->camera_num = camera_num; s->camera_id = camera_id; - assert(camera_id < ARRAYSIZE(cameras_supported)); + assert(camera_id < std::size(cameras_supported)); s->ci = cameras_supported[camera_id]; assert(s->ci.frame_width != 0); s->pixel_clock = pixel_clock; - s->line_length_pclk = line_length_pclk; s->max_gain = max_gain; s->fps = fps; + s->frame_length = s->pixel_clock / line_length_pclk / s->fps; s->self_recover = 0; s->apply_exposure = (camera_id == CAMERA_ID_IMX298) ? imx298_apply_exposure : ov8865_apply_exposure; @@ -228,18 +234,15 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { int err = 0; - - uint32_t frame_length = s->pixel_clock / s->line_length_pclk / s->fps; - uint32_t gain = s->cur_gain; uint32_t integ_lines = s->cur_integ_lines; if (exposure_frac >= 0) { - exposure_frac = std::clamp(exposure_frac, 2.0f / frame_length, 1.0f); - integ_lines = frame_length * exposure_frac; + exposure_frac = std::clamp(exposure_frac, 2.0f / s->frame_length, 1.0f); + integ_lines = s->frame_length * exposure_frac; // See page 79 of the datasheet, this is the max allowed (-1 for phase adjust) - integ_lines = std::min(integ_lines, frame_length-11); + integ_lines = std::min(integ_lines, s->frame_length - 11); } if (gain_frac >= 0) { @@ -256,19 +259,15 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { gain = (s->max_gain/510) * (512 - 512/(256*gain_frac)); } - if (gain != s->cur_gain - || integ_lines != s->cur_integ_lines - || frame_length != s->cur_frame_length) { - + if (gain != s->cur_gain || integ_lines != s->cur_integ_lines) { if (s->apply_exposure == ov8865_apply_exposure) { gain = 800 * gain_frac; // ISO } - err = s->apply_exposure(s, gain, integ_lines, frame_length); + err = s->apply_exposure(s, gain, integ_lines, s->frame_length); if (err == 0) { std::lock_guard lk(s->frame_info_lock); s->cur_gain = gain; s->cur_integ_lines = integ_lines; - s->cur_frame_length = frame_length; } else { LOGE("camera %d apply_exposure err: %d", s->camera_num, err); } @@ -290,9 +289,8 @@ static void do_autoexposure(CameraState *s, float grey_frac) { const float gain_frac_min = 0.015625; const float gain_frac_max = 1.0; // exposure time limits - uint32_t frame_length = s->pixel_clock / s->line_length_pclk / s->fps; const uint32_t exposure_time_min = 16; - const uint32_t exposure_time_max = frame_length - 11; // copied from set_exposure() + const uint32_t exposure_time_max = s->frame_length - 11; // copied from set_exposure() float cur_gain_frac = s->cur_gain_frac; float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.05); @@ -325,14 +323,8 @@ static void do_autoexposure(CameraState *s, float grey_frac) { } static void sensors_init(MultiCameraState *s) { - unique_fd sensorinit_fd; - sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK); - assert(sensorinit_fd >= 0); - - // init road camera sensor - - struct msm_camera_sensor_slave_info slave_info = {0}; - slave_info = (struct msm_camera_sensor_slave_info){ + msm_camera_sensor_slave_info slave_infos[2] = { + (msm_camera_sensor_slave_info){ // road camera .sensor_name = "imx298", .eeprom_name = "sony_imx298", .actuator_name = "dw9800w", @@ -367,16 +359,8 @@ static void sensors_init(MultiCameraState *s) { .is_init_params_valid = 0, .sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 90}, .output_format = MSM_SENSOR_BAYER, - }; - slave_info.power_setting_array.power_setting = &slave_info.power_setting_array.power_setting_a[0]; - slave_info.power_setting_array.power_down_setting = &slave_info.power_setting_array.power_down_setting_a[0]; - sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &slave_info}; - int err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); - LOG("sensor init cfg (road camera): %d", err); - assert(err >= 0); - - struct msm_camera_sensor_slave_info slave_info2 = {0}; - slave_info2 = (struct msm_camera_sensor_slave_info){ + }, + (msm_camera_sensor_slave_info){ // driver camera .sensor_name = "ov8865_sunny", .eeprom_name = "ov8865_plus", .actuator_name = "", @@ -409,14 +393,17 @@ static void sensors_init(MultiCameraState *s) { .is_init_params_valid = 0, .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270}, .output_format = MSM_SENSOR_BAYER, - }; - slave_info2.power_setting_array.power_setting = &slave_info2.power_setting_array.power_setting_a[0]; - slave_info2.power_setting_array.power_down_setting = &slave_info2.power_setting_array.power_down_setting_a[0]; - sensor_init_cfg.cfgtype = CFG_SINIT_PROBE; - sensor_init_cfg.cfg.setting = &slave_info2; - err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); - LOG("sensor init cfg (driver camera): %d", err); - assert(err >= 0); + }}; + + unique_fd sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK); + assert(sensorinit_fd >= 0); + for (auto &info : slave_infos) { + info.power_setting_array.power_setting = &info.power_setting_array.power_setting_a[0]; + info.power_setting_array.power_down_setting = &info.power_setting_array.power_down_setting_a[0]; + sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &info}; + int err = cam_ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg, "sensor init cfg"); + assert(err >= 0); + } } static void camera_open(CameraState *s, bool is_road_cam) { @@ -463,23 +450,19 @@ static void camera_open(CameraState *s, bool is_road_cam) { struct msm_camera_csi_lane_params csi_lane_params = {0}; csi_lane_params.csi_lane_mask = 0x1f; csiphy_cfg_data csiphy_cfg_data = { .cfg.csi_lane_params = &csi_lane_params, .cfgtype = CSIPHY_RELEASE}; - int err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); - LOG("release csiphy: %d", err); + int err = cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "release csiphy"); // CSID: release csid csid_cfg_data.cfgtype = CSID_RELEASE; - err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); - LOG("release csid: %d", err); + cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "release csid"); // SENSOR: send power down struct sensorb_cfg_data sensorb_cfg_data = {.cfgtype = CFG_POWER_DOWN}; - err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); - LOG("sensor power down: %d", err); + cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power down"); // actuator powerdown actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERDOWN; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator powerdown: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerdown"); // reset isp // struct msm_vfe_axi_halt_cmd halt_cmd = { @@ -508,39 +491,35 @@ static void camera_open(CameraState *s, bool is_road_cam) { // CSID: init csid csid_cfg_data.cfgtype = CSID_INIT; - err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); - LOG("init csid: %d", err); + cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "init csid"); // CSIPHY: init csiphy csiphy_cfg_data = {.cfgtype = CSIPHY_INIT}; - err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); - LOG("init csiphy: %d", err); + cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "init csiphy"); // SENSOR: stop stream struct msm_camera_i2c_reg_setting stop_settings = { .reg_setting = stop_reg_array, - .size = ARRAYSIZE(stop_reg_array), + .size = std::size(stop_reg_array), .addr_type = MSM_CAMERA_I2C_WORD_ADDR, .data_type = MSM_CAMERA_I2C_BYTE_DATA, .delay = 0 }; sensorb_cfg_data.cfgtype = CFG_SET_STOP_STREAM_SETTING; sensorb_cfg_data.cfg.setting = &stop_settings; - err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); - LOG("stop stream: %d", err); + cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "stop stream"); // SENSOR: send power up sensorb_cfg_data = {.cfgtype = CFG_POWER_UP}; - err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); - LOG("sensor power up: %d", err); + cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power up"); // **** configure the sensor **** // SENSOR: send i2c configuration if (s->camera_id == CAMERA_ID_IMX298) { - err = sensor_write_regs(s, init_array_imx298, ARRAYSIZE(init_array_imx298), MSM_CAMERA_I2C_BYTE_DATA); + err = sensor_write_regs(s, init_array_imx298, std::size(init_array_imx298), MSM_CAMERA_I2C_BYTE_DATA); } else if (s->camera_id == CAMERA_ID_OV8865) { - err = sensor_write_regs(s, init_array_ov8865, ARRAYSIZE(init_array_ov8865), MSM_CAMERA_I2C_BYTE_DATA); + err = sensor_write_regs(s, init_array_ov8865, std::size(init_array_ov8865), MSM_CAMERA_I2C_BYTE_DATA); } else { assert(false); } @@ -549,12 +528,10 @@ static void camera_open(CameraState *s, bool is_road_cam) { if (is_road_cam) { // init the actuator actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERUP; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator powerup: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerup"); actuator_cfg_data.cfgtype = CFG_ACTUATOR_INIT; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator init: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator init"); struct msm_actuator_reg_params_t actuator_reg_params[] = { { @@ -605,12 +582,11 @@ static void camera_open(CameraState *s, bool is_road_cam) { }, }; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator set info: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set info"); } if (s->camera_id == CAMERA_ID_IMX298) { - err = sensor_write_regs(s, mode_setting_array_imx298, ARRAYSIZE(mode_setting_array_imx298), MSM_CAMERA_I2C_BYTE_DATA); + err = sensor_write_regs(s, mode_setting_array_imx298, std::size(mode_setting_array_imx298), MSM_CAMERA_I2C_BYTE_DATA); LOG("sensor setup: %d", err); } @@ -624,8 +600,7 @@ static void camera_open(CameraState *s, bool is_road_cam) { } csiphy_cfg_data.cfgtype = CSIPHY_CFG; csiphy_cfg_data.cfg.csiphy_params = &csiphy_params; - err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); - LOG("csiphy configure: %d", err); + cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "csiphy configure"); // CSID: configure csid #define CSI_STATS 0x35 @@ -648,13 +623,11 @@ static void camera_open(CameraState *s, bool is_road_cam) { csid_cfg_data.cfgtype = CSID_CFG; csid_cfg_data.cfg.csid_params = &csid_params; - err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); - LOG("csid configure: %d", err); + cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "csid configure"); // ISP: SMMU_ATTACH msm_vfe_smmu_attach_cmd smmu_attach_cmd = {.security_mode = 0, .iommu_attach_mode = IOMMU_ATTACH}; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd); - LOG("isp smmu attach: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd, "isp smmu attach"); // ******************* STREAM RAW ***************************** @@ -714,8 +687,7 @@ static void camera_open(CameraState *s, bool is_road_cam) { ss->buf_request.num_buf = FRAME_BUF_COUNT; ss->buf_request.buf_type = ISP_PRIVATE_BUF; ss->buf_request.handle = 0; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request); - LOG("isp request buf: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request, "isp request buf"); LOG("got buf handle: 0x%x", ss->buf_request.handle); // ENQUEUE all buffers @@ -734,16 +706,14 @@ static void camera_open(CameraState *s, bool is_road_cam) { update_cmd.update_info[0].user_stream_id = ss->stream_req.stream_id; update_cmd.update_info[0].stream_handle = ss->stream_req.axi_stream_handle; update_cmd.update_type = UPDATE_STREAM_ADD_BUFQ; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd); - LOG("isp update stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd, "isp update stream"); } LOG("******** START STREAMS ********"); sub.id = 0; sub.type = 0x1ff; - err = ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub); - LOG("isp subscribe: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub, "isp subscribe"); // ISP: START_STREAM s->stream_cfg.cmd = START_STREAM; @@ -751,14 +721,13 @@ static void camera_open(CameraState *s, bool is_road_cam) { for (int i = 0; i < s->stream_cfg.num_streams; i++) { s->stream_cfg.stream_handle[i] = s->ss[i].stream_req.axi_stream_handle; } - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); - LOG("isp start stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp start stream"); } static void road_camera_start(CameraState *s) { set_exposure(s, 1.0, 1.0); - int err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); + int err = sensor_write_regs(s, start_reg_array, std::size(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); LOG("sensor start regs: %d", err); int inf_step = 512 - INFINITY_DAC; @@ -775,8 +744,7 @@ static void road_camera_start(CameraState *s) { .pos = {INFINITY_DAC, 0}, .delay = {0,} }; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator set pos: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set pos"); // TODO: confirm this isn't needed /*memset(&actuator_cfg_data, 0, sizeof(actuator_cfg_data)); @@ -881,7 +849,8 @@ static void parse_autofocus(CameraState *s, uint8_t *d) { } static std::optional get_accel_z(SubMaster *sm) { - if (sm->update(0) > 0) { + sm->update(0); + if(sm->updated("sensorEvents")){ for (auto event : (*sm)["sensorEvents"].getSensorEvents()) { if (event.which() == cereal::SensorEventData::ACCELERATION) { if (auto v = event.getAcceleration().getV(); v.size() >= 3) @@ -936,7 +905,7 @@ void camera_autoexposure(CameraState *s, float grey_frac) { static void driver_camera_start(CameraState *s) { set_exposure(s, 1.0, 1.0); - int err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); + int err = sensor_write_regs(s, start_reg_array, std::size(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); LOG("sensor start regs: %d", err); } @@ -994,16 +963,13 @@ void cameras_open(MultiCameraState *s) { // ISPIF: setup ispif_cfg_data = {.cfg_type = ISPIF_INIT, .csid_version = 0x30050000 /* CSID_VERSION_V35*/}; - err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); - LOG("ispif setup: %d", err); + cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif setup"); ispif_cfg_data = {.cfg_type = ISPIF_CFG, .params = ispif_params}; - err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); - LOG("ispif cfg: %d", err); + cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif cfg"); ispif_cfg_data.cfg_type = ISPIF_START_FRAME_BOUNDARY; - err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); - LOG("ispif start_frame_boundary: %d", err); + cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif start_frame_boundary"); driver_camera_start(&s->driver_cam); road_camera_start(&s->road_cam); @@ -1013,20 +979,17 @@ void cameras_open(MultiCameraState *s) { static void camera_close(CameraState *s) { // ISP: STOP_STREAM s->stream_cfg.cmd = STOP_STREAM; - int err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); - LOG("isp stop stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp stop stream"); for (int i = 0; i < 3; i++) { StreamState *ss = &s->ss[i]; if (ss->stream_req.axi_stream_handle != 0) { - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request); - LOG("isp release buf: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request, "isp release buf"); struct msm_vfe_axi_stream_release_cmd stream_release = { .stream_handle = ss->stream_req.axi_stream_handle, }; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release); - LOG("isp release stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release, "isp release stream"); } } } @@ -1161,7 +1124,7 @@ void cameras_run(MultiCameraState *s) { while (!do_exit) { struct pollfd fds[2] = {{.fd = cameras[0]->isp_fd, .events = POLLPRI}, {.fd = cameras[1]->isp_fd, .events = POLLPRI}}; - int ret = poll(fds, ARRAYSIZE(fds), 1000); + int ret = poll(fds, std::size(fds), 1000); if (ret < 0) { if (errno == EINTR || errno == EAGAIN) continue; LOGE("poll failed (%d - %d)", ret, errno); @@ -1199,7 +1162,7 @@ void cameras_run(MultiCameraState *s) { c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){ .frame_id = isp_event_data->frame_id, .timestamp_eof = timestamp, - .frame_length = (uint32_t)c->cur_frame_length, + .frame_length = (uint32_t)c->frame_length, .integ_lines = (uint32_t)c->cur_integ_lines, .global_gain = (uint32_t)c->cur_gain, .lens_pos = c->cur_lens_pos, diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 1ab817f41..55792c4ff 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -1,24 +1,22 @@ #pragma once -#include -#include #include -#include +#include +#include + #include -#include "messaging.hpp" +#include -#include "msmb_isp.h" -#include "msmb_ispif.h" -#include "msmb_camera.h" -#include "msm_cam_sensor.h" - -#include "visionbuf.h" - -#include "common/mat.h" -#include "common/util.h" -#include "imgproc/utils.h" - -#include "camera_common.h" +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionbuf.h" +#include "selfdrive/camerad/cameras/camera_common.h" +#include "selfdrive/camerad/imgproc/utils.h" +#include "selfdrive/camerad/include/msm_cam_sensor.h" +#include "selfdrive/camerad/include/msmb_camera.h" +#include "selfdrive/camerad/include/msmb_isp.h" +#include "selfdrive/camerad/include/msmb_ispif.h" +#include "selfdrive/common/mat.h" +#include "selfdrive/common/util.h" #define FRAME_BUF_COUNT 4 #define METADATA_BUF_COUNT 4 @@ -35,7 +33,7 @@ typedef struct CameraState CameraState; -typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, int frame_length); +typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, uint32_t frame_length); typedef struct StreamState { struct msm_isp_buf_request buf_request; @@ -67,10 +65,10 @@ typedef struct CameraState { // exposure uint32_t pixel_clock, line_length_pclk; - uint32_t max_gain; + uint32_t frame_length; + unsigned int max_gain; float cur_exposure_frac, cur_gain_frac; int cur_gain, cur_integ_lines; - int cur_frame_length; std::atomic digital_gain; camera_apply_exposure_func apply_exposure; diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index ae81db6bb..0f0fb10b5 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -1,4 +1,3 @@ - #include #include #include @@ -11,19 +10,17 @@ #include #include -#include "common/util.h" -#include "common/swaglog.h" -#include "camera_qcom2.h" - #include "media/cam_defs.h" #include "media/cam_isp.h" #include "media/cam_isp_ife.h" #include "media/cam_sensor_cmn_header.h" #include "media/cam_sensor.h" #include "media/cam_sync.h" - #include "sensor2_i2c.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/camerad/cameras/camera_qcom2.h" + #define FRAME_WIDTH 1928 #define FRAME_HEIGHT 1208 //#define FRAME_STRIDE 1936 // for 8 bit output @@ -140,7 +137,7 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) { void sensors_poke(struct CameraState *s, int request_id) { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet); - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 0; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -157,14 +154,14 @@ void sensors_poke(struct CameraState *s, int request_id) { assert(ret == 0); munmap(pkt, size); - release_fd(s->video0_fd, cam_packet_handle); + release_fd(s->multi_cam_state->video0_fd, cam_packet_handle); } void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) { // LOGD("sensors_i2c: %d", len); uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -174,7 +171,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); buf_desc[0].type = CAM_CMD_BUF_I2C; - struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); i2c_random_wr->header.count = len; i2c_random_wr->header.op_code = 1; i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; @@ -192,10 +189,17 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l assert(ret == 0); munmap(i2c_random_wr, buf_desc[0].size); - release_fd(s->video0_fd, buf_desc[0].mem_handle); + release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle); munmap(pkt, size); - release_fd(s->video0_fd, cam_packet_handle); + release_fd(s->multi_cam_state->video0_fd, cam_packet_handle); } +static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { + cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings))); + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = delay_ms; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + return (struct cam_cmd_power *)(unconditional_wait + 1); +}; void sensors_init(int video0_fd, int sensor_fd, int camera_num) { uint32_t cam_packet_handle = 0; @@ -246,11 +250,8 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) { //buf_desc[1].size = buf_desc[1].length = 148; buf_desc[1].size = buf_desc[1].length = 196; buf_desc[1].type = CAM_CMD_BUF_I2C; - struct cam_cmd_power *power = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); - memset(power, 0, buf_desc[1].size); - struct cam_cmd_unconditional_wait *unconditional_wait; - - //void *ptr = power; + struct cam_cmd_power *power_settings = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + memset(power_settings, 0, buf_desc[1].size); // 7750 /*power->count = 2; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; @@ -259,45 +260,28 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) { power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/ // 885a + struct cam_cmd_power *power = power_settings; power->count = 4; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; power->power_settings[0].power_seq_type = 3; // clock?? power->power_settings[1].power_seq_type = 1; // analog power->power_settings[2].power_seq_type = 2; // digital power->power_settings[3].power_seq_type = 8; // reset low - power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); - - unconditional_wait = (struct cam_cmd_unconditional_wait *)power; - unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; - unconditional_wait->delay = 5; - unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; - power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + power = power_set_wait(power, 5); // set clock power->count = 1; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; power->power_settings[0].power_seq_type = 0; power->power_settings[0].config_val_low = 24000000; //Hz - power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); - - unconditional_wait = (struct cam_cmd_unconditional_wait *)power; - unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; - unconditional_wait->delay = 10; // ms - unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; - power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + power = power_set_wait(power, 10); // 8,1 is this reset? power->count = 1; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; power->power_settings[0].power_seq_type = 8; power->power_settings[0].config_val_low = 1; - power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); - - unconditional_wait = (struct cam_cmd_unconditional_wait *)power; - unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; - unconditional_wait->delay = 100; // ms - unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; - power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + power = power_set_wait(power, 100); // probe happens here @@ -306,39 +290,21 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) { power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; power->power_settings[0].power_seq_type = 0; power->power_settings[0].config_val_low = 0; - power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); - - unconditional_wait = (struct cam_cmd_unconditional_wait *)power; - unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; - unconditional_wait->delay = 1; - unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; - power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + power = power_set_wait(power, 1); // reset high power->count = 1; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; power->power_settings[0].power_seq_type = 8; power->power_settings[0].config_val_low = 1; - power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); - - unconditional_wait = (struct cam_cmd_unconditional_wait *)power; - unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; - unconditional_wait->delay = 1; - unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; - power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + power = power_set_wait(power, 1); // reset low power->count = 1; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; power->power_settings[0].power_seq_type = 8; power->power_settings[0].config_val_low = 0; - power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); - - unconditional_wait = (struct cam_cmd_unconditional_wait *)power; - unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; - unconditional_wait->delay = 1; - unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; - power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + power = power_set_wait(power, 1); // 7750 /*power->count = 1; @@ -352,7 +318,6 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) { power->power_settings[0].power_seq_type = 2; power->power_settings[1].power_seq_type = 1; power->power_settings[2].power_seq_type = 3; - power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); LOGD("probing the sensor"); int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); @@ -360,7 +325,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) { munmap(i2c_info, buf_desc[0].size); release_fd(video0_fd, buf_desc[0].mem_handle); - munmap(power, buf_desc[1].size); + munmap(power_settings, buf_desc[1].size); release_fd(video0_fd, buf_desc[1].mem_handle); munmap(pkt, size); release_fd(video0_fd, cam_packet_handle); @@ -372,7 +337,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request if (io_mem_handle != 0) { size += sizeof(struct cam_buf_io_cfg); } - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = 0; @@ -408,7 +373,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request } buf_desc[1].type = CAM_CMD_BUF_GENERIC; buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; - uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20); + uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20); // cam_isp_packet_generic_blob_handler uint32_t tmp[] = { @@ -466,16 +431,16 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request config_dev_cmd.offset = 0; config_dev_cmd.packet_handle = cam_packet_handle; - int ret = cam_control(s->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + int ret = cam_control(s->multi_cam_state->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); if (ret != 0) { printf("ISP CONFIG FAILED\n"); } munmap(buf2, buf_desc[1].size); - release_fd(s->video0_fd, buf_desc[1].mem_handle); - // release_fd(s->video0_fd, buf_desc[0].mem_handle); + release_fd(s->multi_cam_state->video0_fd, buf_desc[1].mem_handle); + // release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle); munmap(pkt, size); - release_fd(s->video0_fd, cam_packet_handle); + release_fd(s->multi_cam_state->video0_fd, cam_packet_handle); } void enqueue_buffer(struct CameraState *s, int i, bool dp) { @@ -483,12 +448,12 @@ void enqueue_buffer(struct CameraState *s, int i, bool dp) { int request_id = s->request_ids[i]; if (s->buf_handle[i]) { - release(s->video0_fd, s->buf_handle[i]); + release(s->multi_cam_state->video0_fd, s->buf_handle[i]); // wait struct cam_sync_wait sync_wait = {0}; sync_wait.sync_obj = s->sync_objs[i]; sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23 - ret = cam_control(s->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); + ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj); s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof @@ -498,7 +463,7 @@ void enqueue_buffer(struct CameraState *s, int i, bool dp) { struct cam_sync_info sync_destroy = {0}; strcpy(sync_destroy.name, "NodeOutputPortFence"); sync_destroy.sync_obj = s->sync_objs[i]; - ret = cam_control(s->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj); } @@ -507,23 +472,23 @@ void enqueue_buffer(struct CameraState *s, int i, bool dp) { req_mgr_sched_request.session_hdl = s->session_handle; req_mgr_sched_request.link_hdl = s->link_handle; req_mgr_sched_request.req_id = request_id; - ret = cam_control(s->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); // LOGD("sched req: %d %d", ret, request_id); // create output fence struct cam_sync_info sync_create = {0}; strcpy(sync_create.name, "NodeOutputPortFence"); - ret = cam_control(s->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); // LOGD("fence req: %d %d", ret, sync_create.sync_obj); s->sync_objs[i] = sync_create.sync_obj; // configure ISP to put the image in place struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; - mem_mgr_map_cmd.mmu_hdls[0] = s->device_iommu; + mem_mgr_map_cmd.mmu_hdls[0] = s->multi_cam_state->device_iommu; mem_mgr_map_cmd.num_hdl = 1; mem_mgr_map_cmd.flags = 1; mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd; - ret = cam_control(s->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); // LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; @@ -544,10 +509,10 @@ void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) { // ******************* camera ******************* -static void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { +static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { LOGD("camera init %d", camera_num); - - assert(camera_id < ARRAYSIZE(cameras_supported)); + s->multi_cam_state = multi_cam_state; + assert(camera_id < std::size(cameras_supported)); s->ci = cameras_supported[camera_id]; assert(s->ci.frame_width != 0); @@ -606,10 +571,10 @@ static void camera_open(CameraState *s) { // probe the sensor LOGD("-- Probing sensor %d", s->camera_num); - sensors_init(s->video0_fd, s->sensor_fd, s->camera_num); + sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num); memset(&s->req_mgr_session_info, 0, sizeof(s->req_mgr_session_info)); - ret = cam_control(s->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); LOGD("get session: %d 0x%X", ret, s->req_mgr_session_info.session_hdl); s->session_handle = s->req_mgr_session_info.session_hdl; // access the sensor @@ -689,7 +654,7 @@ static void camera_open(CameraState *s) { .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }; - ret = cam_control(s->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); + ret = cam_control(s->multi_cam_state->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); LOGD("acquire isp dev: %d", ret); free(in_port_info); s->isp_dev_handle = acquire_dev_cmd.dev_handle; @@ -710,7 +675,7 @@ static void camera_open(CameraState *s) { // acquires done // config ISP - alloc_w_mmu_hdl(s->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->device_iommu, s->cdm_iommu); + alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu); config_isp(s, 0, 0, 1, s->buf0_handle, 0); LOG("-- Configuring sensor"); @@ -726,7 +691,7 @@ static void camera_open(CameraState *s) { { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -735,7 +700,7 @@ static void camera_open(CameraState *s) { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); buf_desc[0].type = CAM_CMD_BUF_GENERIC; - struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); csiphy_info->lane_mask = 0x1f; csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane @@ -754,8 +719,8 @@ static void camera_open(CameraState *s) { int ret = cam_control(s->csiphy_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); assert(ret == 0); - release(s->video0_fd, buf_desc[0].mem_handle); - release(s->video0_fd, cam_packet_handle); + release(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle); + release(s->multi_cam_state->video0_fd, cam_packet_handle); } // link devices @@ -765,7 +730,7 @@ static void camera_open(CameraState *s) { req_mgr_link_info.num_devices = 2; req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle; req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle; - ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); LOGD("link: %d", ret); s->link_handle = req_mgr_link_info.link_hdl; @@ -774,13 +739,13 @@ static void camera_open(CameraState *s) { req_mgr_link_control.session_hdl = s->session_handle; req_mgr_link_control.num_links = 1; req_mgr_link_control.link_hdls[0] = s->link_handle; - ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); LOGD("link control: %d", ret); LOGD("start csiphy: %d", ret); ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle); LOGD("start isp: %d", ret); - ret = device_control(s->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle); + ret = device_control(s->multi_cam_state->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle); LOGD("start sensor: %d", ret); ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle); @@ -788,13 +753,13 @@ static void camera_open(CameraState *s) { } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - camera_init(v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx, + camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); // swap left/right printf("road camera initted \n"); - camera_init(v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx, + camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE, VISION_STREAM_YUV_WIDE); printf("wide road camera initted \n"); - camera_init(v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx, + camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT); printf("driver camera initted \n"); @@ -810,19 +775,16 @@ void cameras_open(MultiCameraState *s) { s->video0_fd = open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK); assert(s->video0_fd >= 0); LOGD("opened video0"); - s->road_cam.video0_fd = s->driver_cam.video0_fd = s->wide_road_cam.video0_fd = s->video0_fd; // video1 is cam_sync, the target of some ioctls s->video1_fd = open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK); assert(s->video1_fd >= 0); LOGD("opened video1"); - s->road_cam.video1_fd = s->driver_cam.video1_fd = s->wide_road_cam.video1_fd = s->video1_fd; // looks like there's only one of these s->isp_fd = open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK); assert(s->isp_fd >= 0); LOGD("opened isp"); - s->road_cam.isp_fd = s->driver_cam.isp_fd = s->wide_road_cam.isp_fd = s->isp_fd; // query icp for MMU handles LOG("-- Query ICP for MMU handles"); @@ -835,10 +797,8 @@ void cameras_open(MultiCameraState *s) { assert(ret == 0); LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure); LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure); - int device_iommu = isp_query_cap_cmd.device_iommu.non_secure; - int cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure; - s->road_cam.device_iommu = s->driver_cam.device_iommu = s->wide_road_cam.device_iommu = device_iommu; - s->road_cam.cdm_iommu = s->driver_cam.cdm_iommu = s->wide_road_cam.cdm_iommu = cdm_iommu; + s->device_iommu = isp_query_cap_cmd.device_iommu.non_secure; + s->cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure; // subscribe LOG("-- Subscribing"); @@ -863,7 +823,7 @@ static void camera_close(CameraState *s) { LOG("-- Stop devices"); // ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle); // LOGD("stop sensor: %d", ret); - ret = device_control(s->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle); + ret = device_control(s->multi_cam_state->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle); LOGD("stop isp: %d", ret); ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle); LOGD("stop csiphy: %d", ret); @@ -874,7 +834,7 @@ static void camera_close(CameraState *s) { req_mgr_link_control.session_hdl = s->session_handle; req_mgr_link_control.num_links = 1; req_mgr_link_control.link_hdls[0] = s->link_handle; - ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); LOGD("link control stop: %d", ret); // unlink @@ -882,19 +842,19 @@ static void camera_close(CameraState *s) { static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; req_mgr_unlink_info.session_hdl = s->session_handle; req_mgr_unlink_info.link_hdl = s->link_handle; - ret = cam_control(s->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); LOGD("unlink: %d", ret); // release devices LOGD("-- Release devices"); ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle); LOGD("release sensor: %d", ret); - ret = device_control(s->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle); + ret = device_control(s->multi_cam_state->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle); LOGD("release isp: %d", ret); ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle); LOGD("release csiphy: %d", ret); - ret = cam_control(s->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); + ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); LOGD("destroyed session: %d", ret); } @@ -923,7 +883,7 @@ void handle_camera_event(CameraState *s, void *evdat) { // check for skipped frames if (main_id > s->frame_id_last + 1 && !s->skipped) { // realign - clear_req_queue(s->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl); + clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl); enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0); s->skipped = true; } else if (main_id == s->frame_id_last + 1) { @@ -953,7 +913,7 @@ void handle_camera_event(CameraState *s, void *evdat) { } else { // not ready // reset after half second of no response if (main_id > s->frame_id_last + 10) { - clear_req_queue(s->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl); + clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl); enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0); s->frame_id_last = main_id; s->skipped = true; @@ -1141,7 +1101,7 @@ void cameras_run(MultiCameraState *s) { fds[0].fd = s->video0_fd; fds[0].events = POLLPRI; - int ret = poll(fds, ARRAYSIZE(fds), 1000); + int ret = poll(fds, std::size(fds), 1000); if (ret < 0) { if (errno == EINTR || errno == EAGAIN) continue; LOGE("poll failed (%d - %d)", ret, errno); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index ada2744e1..ac37f557a 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -4,8 +4,10 @@ #include #include -#include "camera_common.h" -#include "media/cam_req_mgr.h" +#include + +#include "selfdrive/camerad/cameras/camera_common.h" +#include "selfdrive/common/util.h" #define FRAME_BUF_COUNT 4 @@ -18,8 +20,9 @@ #define DEBAYER_LOCAL_WORKSIZE 16 typedef struct CameraState { + MultiCameraState *multi_cam_state; CameraInfo ci; - + std::mutex exp_lock; float analog_gain_frac; uint16_t analog_gain; @@ -29,19 +32,11 @@ typedef struct CameraState { int exposure_time_max; float ef_filtered; - int device_iommu; - int cdm_iommu; - - int video0_fd; - int video1_fd; - int isp_fd; - - int sensor_fd; - int csiphy_fd; + unique_fd sensor_fd; + unique_fd csiphy_fd; int camera_num; - uint32_t session_handle; uint32_t sensor_dev_handle; @@ -67,9 +62,12 @@ typedef struct CameraState { typedef struct MultiCameraState { int device; - int video0_fd; - int video1_fd; - int isp_fd; + unique_fd video0_fd; + unique_fd video1_fd; + unique_fd isp_fd; + int device_iommu; + int cdm_iommu; + CameraState road_cam; CameraState wide_road_cam; diff --git a/selfdrive/camerad/imgproc/utils.cc b/selfdrive/camerad/imgproc/utils.cc index 7dd0ab97a..5c2110131 100644 --- a/selfdrive/camerad/imgproc/utils.cc +++ b/selfdrive/camerad/imgproc/utils.cc @@ -1,7 +1,9 @@ -#include "utils.h" +#include "selfdrive/camerad/imgproc/utils.h" + #include #include #include + #include #include diff --git a/selfdrive/camerad/imgproc/utils.h b/selfdrive/camerad/imgproc/utils.h index e33cfe9c8..d7d518c0f 100644 --- a/selfdrive/camerad/imgproc/utils.h +++ b/selfdrive/camerad/imgproc/utils.h @@ -1,9 +1,11 @@ #pragma once -#include #include +#include + #include -#include "clutil.h" + +#include "selfdrive/common/clutil.h" #define NUM_SEGMENTS_X 8 #define NUM_SEGMENTS_Y 6 diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 0c6d54d92..6cfa85bdb 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -1,28 +1,30 @@ -#include -#include -#include #include -#include +#include +#include #include +#include + +#include + +#include "libyuv.h" + +#include "cereal/visionipc/visionipc_server.h" +#include "selfdrive/common/clutil.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" #if defined(QCOM) && !defined(QCOM_REPLAY) -#include "cameras/camera_qcom.h" +#include "selfdrive/camerad/cameras/camera_qcom.h" #elif QCOM2 -#include "cameras/camera_qcom2.h" +#include "selfdrive/camerad/cameras/camera_qcom2.h" #elif WEBCAM -#include "cameras/camera_webcam.h" +#include "selfdrive/camerad/cameras/camera_webcam.h" #else -#include "cameras/camera_frame_stream.h" +#include "selfdrive/camerad/cameras/camera_frame_stream.h" #endif -#include - -#include "clutil.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "visionipc_server.h" - ExitHandler do_exit; void party(cl_device_id device_id, cl_context context) { @@ -43,11 +45,11 @@ void party(cl_device_id device_id, cl_context context) { int main(int argc, char *argv[]) { set_realtime_priority(53); -#if defined(QCOM) - set_core_affinity(2); -#elif defined(QCOM2) - set_core_affinity(6); -#endif + if (Hardware::EON()) { + set_core_affinity(2); + } else if (Hardware::TICI()) { + set_core_affinity(6); + } cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 60883608f..eb7bb93af 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -33,24 +33,30 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): frame_sizes = [eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size] frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes} - sm = messaging.SubMaster([frame, front_frame]) + sockets = [] + if frame is not None: + sockets.append(frame) + if front_frame is not None: + sockets.append(front_frame) + + sm = messaging.SubMaster(sockets) while min(sm.logMonoTime.values()) == 0: sm.update() - rear = extract_image(sm[frame].image, frame_sizes) - front = extract_image(sm[front_frame].image, frame_sizes) + rear = extract_image(sm[frame].image, frame_sizes) if frame is not None else None + front = extract_image(sm[front_frame].image, frame_sizes) if front_frame is not None else None return rear, front def snapshot(): params = Params() - front_camera_allowed = int(params.get("RecordFront")) + front_camera_allowed = params.get_bool("RecordFront") - if params.get("IsOffroad") != b"1" or params.get("IsTakingSnapshot") == b"1": + if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"): print("Already taking snapshot") return None, None - params.put("IsTakingSnapshot", "1") + params.put_bool("IsTakingSnapshot", True) set_offroad_alert("Offroad_IsTakingSnapshot", True) time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start @@ -59,7 +65,7 @@ def snapshot(): subprocess.check_call(["pgrep", "camerad"]) print("Camerad already running") - params.put("IsTakingSnapshot", "0") + params.put_bool("IsTakingSnapshot", False) params.delete("Offroad_IsTakingSnapshot") return None, None except subprocess.CalledProcessError: @@ -68,18 +74,22 @@ def snapshot(): env = os.environ.copy() env["SEND_ROAD"] = "1" env["SEND_WIDE_ROAD"] = "1" - env["SEND_DRIVER"] = "1" + + if front_camera_allowed: + env["SEND_DRIVER"] = "1" + proc = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad"), env=env) time.sleep(3.0) frame = "wideRoadCameraState" if TICI else "roadCameraState" - rear, front = get_snapshots(frame) + front_frame = "driverCameraState" if front_camera_allowed else None + rear, front = get_snapshots(frame, front_frame) proc.send_signal(signal.SIGINT) proc.communicate() - params.put("IsTakingSnapshot", "0") + params.put_bool("IsTakingSnapshot", False) set_offroad_alert("Offroad_IsTakingSnapshot", False) if not front_camera_allowed: diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.cc b/selfdrive/camerad/transforms/rgb_to_yuv.cc index c0ac19207..1fdeb593d 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv.cc +++ b/selfdrive/camerad/transforms/rgb_to_yuv.cc @@ -1,17 +1,9 @@ -#include -#include - -#include "clutil.h" - #include "rgb_to_yuv.h" -void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { - memset(s, 0, sizeof(*s)); - printf("width %d, height %d, rgb_stride %d\n", width, height, rgb_stride); - assert(width % 2 == 0); - assert(height % 2 == 0); - s->width = width; - s->height = height; +#include + +Rgb2Yuv::Rgb2Yuv(cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { + assert(width % 2 == 0 && height % 2 == 0); char args[1024]; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " @@ -19,27 +11,25 @@ void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, i "-DCL_DEBUG " #endif "-DWIDTH=%d -DHEIGHT=%d -DUV_WIDTH=%d -DUV_HEIGHT=%d -DRGB_STRIDE=%d -DRGB_SIZE=%d", - width, height, width/ 2, height / 2, rgb_stride, width * height); + width, height, width / 2, height / 2, rgb_stride, width * height); + cl_program prg = cl_program_from_file(ctx, device_id, "transforms/rgb_to_yuv.cl", args); - - s->rgb_to_yuv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb_to_yuv", &err)); - // done with this + krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb_to_yuv", &err)); CL_CHECK(clReleaseProgram(prg)); + + work_size[0] = (width + (width % 4 == 0 ? 0 : (4 - width % 4))) / 4; + work_size[1] = (height + (height % 4 == 0 ? 0 : (4 - height % 4))) / 4; } -void rgb_to_yuv_destroy(RGBToYUVState* s) { - CL_CHECK(clReleaseKernel(s->rgb_to_yuv_krnl)); +Rgb2Yuv::~Rgb2Yuv() { + CL_CHECK(clReleaseKernel(krnl)); } -void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) { - CL_CHECK(clSetKernelArg(s->rgb_to_yuv_krnl, 0, sizeof(cl_mem), &rgb_cl)); - CL_CHECK(clSetKernelArg(s->rgb_to_yuv_krnl, 1, sizeof(cl_mem), &yuv_cl)); - const size_t work_size[2] = { - (size_t)(s->width + (s->width % 4 == 0 ? 0 : (4 - s->width % 4))) / 4, - (size_t)(s->height + (s->height % 4 == 0 ? 0 : (4 - s->height % 4))) / 4 - }; +void Rgb2Yuv::queue(cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) { + CL_CHECK(clSetKernelArg(krnl, 0, sizeof(cl_mem), &rgb_cl)); + CL_CHECK(clSetKernelArg(krnl, 1, sizeof(cl_mem), &yuv_cl)); cl_event event; - CL_CHECK(clEnqueueNDRangeKernel(q, s->rgb_to_yuv_krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event)); + CL_CHECK(clEnqueueNDRangeKernel(q, krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event)); CL_CHECK(clWaitForEvents(1, &event)); CL_CHECK(clReleaseEvent(event)); } diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.h b/selfdrive/camerad/transforms/rgb_to_yuv.h index 05d5c5ece..3bb50669e 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv.h +++ b/selfdrive/camerad/transforms/rgb_to_yuv.h @@ -1,21 +1,14 @@ #pragma once -#include -#include +#include "selfdrive/common/clutil.h" -#ifdef __APPLE__ -#include -#else -#include -#endif +class Rgb2Yuv { +public: + Rgb2Yuv(cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride); + ~Rgb2Yuv(); + void queue(cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl); +private: + size_t work_size[2]; + cl_kernel krnl; +}; -typedef struct { - int width, height; - cl_kernel rgb_to_yuv_krnl; -} RGBToYUVState; - -void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride); - -void rgb_to_yuv_destroy(RGBToYUVState* s); - -void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl); diff --git a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc index a2fd4035b..04b6afd09 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc +++ b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc @@ -1,18 +1,19 @@ -#include -#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include +#include #include #include + #include #include +#include +#include +#include +#include +#include +#include +#include #ifdef ANDROID @@ -26,13 +27,11 @@ #endif -#include - #include -#include "clutil.h" -#include "rgb_to_yuv.h" - +#include "libyuv.h" +#include "selfdrive/camerad/transforms/rgb_to_yuv.h" +#include "selfdrive/common/clutil.h" static inline double millis_since_boot() { struct timespec t; diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 7701f90a6..9cd13a58d 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -119,17 +119,5 @@ def create_gas_command(packer, gas_amount, idx): return packer.make_can_msg("GAS_COMMAND", 0, values) -def is_ecu_disconnected(fingerprint, fingerprint_list, ecu_fingerprint, car, ecu): - # check if a stock ecu is disconnected by looking for specific CAN msgs in the fingerprint - # return True if the reference car fingerprint contains the ecu fingerprint msg and - # fingerprint does not contains messages normally sent by a given ecu - ecu_in_car = False - for car_finger in fingerprint_list[car]: - if any(msg in car_finger for msg in ecu_fingerprint[ecu]): - ecu_in_car = True - - return ecu_in_car and not any(msg in fingerprint for msg in ecu_fingerprint[ecu]) - - def make_can_msg(addr, dat, bus): return [addr, 0, dat, bus] diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 2af6df269..9f3ebfa08 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -13,7 +13,7 @@ from cereal import car EventName = car.CarEvent.EventName -def get_startup_event(car_recognized, controller_available): +def get_startup_event(car_recognized, controller_available, fuzzy_fingerprint): if comma_remote and tested_branch: event = EventName.startup else: @@ -23,6 +23,8 @@ def get_startup_event(car_recognized, controller_available): event = EventName.startupNoCar elif car_recognized and not controller_available: event = EventName.startupNoControl + elif car_recognized and fuzzy_fingerprint: + event = EventName.startupFuzzyFingerprint return event @@ -104,10 +106,10 @@ def fingerprint(logcan, sendcan): _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan, bus) - fw_candidates = match_fw_to_car(car_fw) + exact_fw_match, fw_candidates = match_fw_to_car(car_fw) else: vin = VIN_UNKNOWN - fw_candidates, car_fw = set(), [] + exact_fw_match, fw_candidates, car_fw = True, set(), [] cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) @@ -146,29 +148,31 @@ def fingerprint(logcan, sendcan): car_fingerprint = candidate_cars[b][0] # bail if no cars left or we've been waiting for more than 2s - failed = all(len(cc) == 0 for cc in candidate_cars.values()) or frame > 200 + failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200 succeeded = car_fingerprint is not None done = failed or succeeded frame += 1 + exact_match = True source = car.CarParams.FingerprintSource.can # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] source = car.CarParams.FingerprintSource.fw + exact_match = exact_fw_match if fixed_fingerprint: car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed cloudlog.warning("fingerprinted %s", car_fingerprint) - return car_fingerprint, finger, vin, car_fw, source + return car_fingerprint, finger, vin, car_fw, source, exact_match def get_car(logcan, sendcan): - candidate, fingerprints, vin, car_fw, source = fingerprint(logcan, sendcan) + candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) @@ -179,5 +183,6 @@ def get_car(logcan, sendcan): car_params.carVin = vin car_params.carFw = car_fw car_params.fingerprintSource = source + car_params.fuzzyFingerprint = not exact_match return CarInterface(car_params, CarController, CarState), car_params diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 2d3c1a2ec..e7f8ee65b 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -59,6 +59,10 @@ class CarState(CarStateBase): ret.steerError = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed) ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH']) + + if self.CP.enableBsm: + ret.leftBlindspot = cp.vl["BLIND_SPOT_WARNINGS"]['BLIND_SPOT_LEFT'] == 1 + ret.rightBlindspot = cp.vl["BLIND_SPOT_WARNINGS"]['BLIND_SPOT_RIGHT'] == 1 self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]['COUNTER'] self.lkas_car_model = cp_cam.vl["LKAS_HUD"]['CAR_MODEL'] @@ -115,6 +119,13 @@ class CarState(CarStateBase): ("TRACTION_BUTTON", 1), ] + if CP.enableBsm: + signals += [ + ("BLIND_SPOT_RIGHT", "BLIND_SPOT_WARNINGS", 0), + ("BLIND_SPOT_LEFT", "BLIND_SPOT_WARNINGS", 0), + ] + checks += [("BLIND_SPOT_WARNINGS", 2)] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @staticmethod diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index ccf3bc9fe..f37ae953a 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -22,7 +22,7 @@ class CarInterface(CarInterfaceBase): # Speed conversion: 20, 45 mph ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 - ret.mass = 2858. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017 + ret.mass = 2242. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017 ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 @@ -50,6 +50,8 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) ret.enableCamera = True + + ret.enableBsm = 720 in fingerprint[0] return ret diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 17e636a04..2ded7b685 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -59,12 +59,9 @@ FINGERPRINTS = { { 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8 }, - { - 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 2015: 8, 2016: 8, 2024: 8 - }, # Based on "8190c7275a24557b|2020-02-24--09-57-23" { - 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 } ], CAR.JEEP_CHEROKEE: [ diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 2a776b45d..756c4b876 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -53,4 +53,4 @@ class CarState(CarStateBase): ("Brake_Lights", "BCM_to_HS_Body", 0.), ] checks = [] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, enforce_checks=False) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 9def1ca86..aec368cd0 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -61,7 +61,7 @@ class CarInterface(CarInterfaceBase): events = self.create_common_events(ret) if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled: - events.add(car.CarEvent.EventName.steerTempUnavailableMute) + events.add(car.CarEvent.EventName.steerTempUnavailable) ret.events = events.to_msg() diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 468b17ba8..0e3e03822 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -2,6 +2,7 @@ import struct import traceback from typing import Any +from collections import defaultdict from tqdm import tqdm @@ -136,30 +137,82 @@ def chunks(l, n=128): yield l[i:i + n] -def match_fw_to_car(fw_versions): - candidates = FW_VERSIONS - invalid = [] - +def build_fw_dict(fw_versions): fw_versions_dict = {} for fw in fw_versions: addr = fw.address sub_addr = fw.subAddress if fw.subAddress != 0 else None fw_versions_dict[(addr, sub_addr)] = fw.fwVersion + return fw_versions_dict + + +def match_fw_to_car_fuzzy(fw_versions_dict, log=True, exclude=None): + """Do a fuzzy FW match. This function will return a match, and the number of firmware version + that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars + the match is rejected.""" + + # These ECUs are known to be shared between models (EPS only between hybrid/ICE version) + # Getting this exactly right isn't crucial, but excluding camera and radar makes it almost + # impossible to get 3 matching versions, even if two models with shared parts are released at the same + # time and only one is in our database. + exclude_types = [Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps] + + # Build lookup table from (addr, subaddr, fw) to list of candidate cars + all_fw_versions = defaultdict(list) + for candidate, fw_by_addr in FW_VERSIONS.items(): + if candidate == exclude: + continue + + for addr, fws in fw_by_addr.items(): + if addr[0] in exclude_types: + continue + for f in fws: + all_fw_versions[(addr[1], addr[2], f)].append(candidate) + + match_count = 0 + candidate = None + for addr, version in fw_versions_dict.items(): + # All cars that have this FW response on the specified address + candidates = all_fw_versions[(addr[0], addr[1], version)] + + if len(candidates) == 1: + match_count += 1 + if candidate is None: + candidate = candidates[0] + # We uniquely matched two different cars. No fuzzy match possible + elif candidate != candidates[0]: + return set() + + if match_count >= 2: + if log: + cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {match_count} matching ECUs") + return set([candidate]) + else: + return set() + + +def match_fw_to_car_exact(fw_versions_dict): + """Do an exact FW match. Returns all cars that match the given + FW versions for a list of "essential" ECUs. If an ECU is not considered + essential the FW version can be missing to get a fingerprint, but if it's present it + needs to match the database.""" + invalid = [] + candidates = FW_VERSIONS for candidate, fws in candidates.items(): for ecu, expected_versions in fws.items(): ecu_type = ecu[0] addr = ecu[1:] found_version = fw_versions_dict.get(addr, None) - ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa, Ecu.electricBrakeBooster] + ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None: continue - # TODO: on some toyota, the engine can show on two different addresses + # On some Toyota models, the engine can show on two different addresses if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS, TOYOTA.AVALON] and found_version is None: continue - # ignore non essential ecus + # Ignore non essential ecus if ecu_type not in ESSENTIAL_ECUS and found_version is None: continue @@ -170,6 +223,21 @@ def match_fw_to_car(fw_versions): return set(candidates.keys()) - set(invalid) +def match_fw_to_car(fw_versions, allow_fuzzy=True): + fw_versions_dict = build_fw_dict(fw_versions) + matches = match_fw_to_car_exact(fw_versions_dict) + + exact_match = True + if allow_fuzzy and len(matches) == 0: + matches = match_fw_to_car_fuzzy(fw_versions_dict) + + # Fuzzy match found + if len(matches) == 1: + exact_match = False + + return exact_match, matches + + def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False, progress=False): ecu_types = {} @@ -264,7 +332,7 @@ if __name__ == "__main__": t = time.time() fw_vers = get_fw_versions(logcan, sendcan, 1, extra=extra, debug=args.debug, progress=True) - candidates = match_fw_to_car(fw_vers) + _, candidates = match_fw_to_car(fw_vers) print() print("Found FW versions") diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index a59740c6f..954b3ec74 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -104,9 +104,29 @@ class CarState(CarStateBase): ("CruiseMainOn", "ECMEngineStatus", 0), ] + checks = [ + ("BCMTurnSignals", 1), + ("ECMPRDNL", 10), + ("PSCMStatus", 10), + ("ESPStatus", 10), + ("BCMDoorBeltStatus", 10), + ("EPBStatus", 20), + ("EBCMWheelSpdFront", 20), + ("EBCMWheelSpdRear", 20), + ("AcceleratorPedal", 33), + ("AcceleratorPedal2", 33), + ("ASCMSteeringButton", 33), + ("ECMEngineStatus", 100), + ("PSCMSteeringAngle", 100), + ("EBCMBrakePedalPosition", 100), + ] + if CP.carFingerprint == CAR.VOLT: signals += [ ("RegenPaddle", "EBCMRegenPaddle", 0), ] + checks += [ + ("EBCMRegenPaddle", 50), + ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], CanBus.POWERTRAIN) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CanBus.POWERTRAIN) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 2f7d8f473..7c28406cb 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -156,7 +156,7 @@ class CarInterface(CarInterfaceBase): if ret.cruiseState.standstill: events.add(EventName.resumeRequired) if self.CS.pcm_acc_status == AccState.FAULTED: - events.add(EventName.controlsFailed) + events.add(EventName.accFaulted) if ret.vEgo < self.CP.minSteerSpeed: events.add(car.CarEvent.EventName.belowSteerSpeed) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index dc2c2f7b7..7e475c568 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -34,7 +34,7 @@ def create_radar_can_parser(car_fingerprint): [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0] * NUM_SLOTS)) - checks = [] + checks = list({(s[1], 14) for s in signals}) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CanBus.OBSTACLE) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 410d34a73..0de09f5fc 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -22,41 +22,43 @@ def calc_cruise_offset(offset, speed): def get_can_signals(CP): # this function generates lists for signal, messages and initial values signals = [ - ("XMISSION_SPEED", "ENGINE_DATA", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("STEER_ANGLE", "STEERING_SENSORS", 0), - ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), - ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0), - ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), - ("LEFT_BLINKER", "SCM_FEEDBACK", 0), - ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), - ("GEAR", "GEARBOX", 0), - ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1), - ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0), - ("BRAKE_PRESSED", "POWERTRAIN_DATA", 0), - ("BRAKE_SWITCH", "POWERTRAIN_DATA", 0), - ("CRUISE_BUTTONS", "SCM_BUTTONS", 0), - ("ESP_DISABLED", "VSA_STATUS", 1), - ("USER_BRAKE", "VSA_STATUS", 0), - ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), - ("STEER_STATUS", "STEER_STATUS", 5), - ("GEAR_SHIFTER", "GEARBOX", 0), - ("PEDAL_GAS", "POWERTRAIN_DATA", 0), - ("CRUISE_SETTING", "SCM_BUTTONS", 0), - ("ACC_STATUS", "POWERTRAIN_DATA", 0), + ("XMISSION_SPEED", "ENGINE_DATA", 0), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), + ("STEER_ANGLE", "STEERING_SENSORS", 0), + ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), + ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0), + ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), + ("LEFT_BLINKER", "SCM_FEEDBACK", 0), + ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), + ("GEAR", "GEARBOX", 0), + ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0), + ("BRAKE_PRESSED", "POWERTRAIN_DATA", 0), + ("BRAKE_SWITCH", "POWERTRAIN_DATA", 0), + ("CRUISE_BUTTONS", "SCM_BUTTONS", 0), + ("ESP_DISABLED", "VSA_STATUS", 1), + ("USER_BRAKE", "VSA_STATUS", 0), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), + ("STEER_STATUS", "STEER_STATUS", 5), + ("GEAR_SHIFTER", "GEARBOX", 0), + ("PEDAL_GAS", "POWERTRAIN_DATA", 0), + ("CRUISE_SETTING", "SCM_BUTTONS", 0), + ("ACC_STATUS", "POWERTRAIN_DATA", 0), ] checks = [ - ("ENGINE_DATA", 100), - ("WHEEL_SPEEDS", 50), - ("STEERING_SENSORS", 100), - ("SEATBELT_STATUS", 10), - ("CRUISE", 10), - ("POWERTRAIN_DATA", 100), - ("VSA_STATUS", 50), + ("ENGINE_DATA", 100), + ("WHEEL_SPEEDS", 50), + ("STEERING_SENSORS", 100), + ("SEATBELT_STATUS", 10), + ("CRUISE", 10), + ("POWERTRAIN_DATA", 100), + ("VSA_STATUS", 50), + ("STEER_STATUS", 100), + ("STEER_MOTOR_TORQUE", 0), # TODO: not on every car ] if CP.carFingerprint == CAR.ODYSSEY_CHN: @@ -84,12 +86,22 @@ def get_can_signals(CP): if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] checks += [("BRAKE_MODULE", 50)] - signals += [("CAR_GAS", "GAS_PEDAL_2", 0), - ("MAIN_ON", "SCM_FEEDBACK", 0), - ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0), - ("EPB_STATE", "EPB_STATUS", 0), - ("CRUISE_SPEED", "ACC_HUD", 0)] - checks += [("GAS_PEDAL_2", 100)] + + signals += [ + ("CAR_GAS", "GAS_PEDAL_2", 0), + ("MAIN_ON", "SCM_FEEDBACK", 0), + ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0), + ("EPB_STATE", "EPB_STATUS", 0), + ("CRUISE_SPEED", "ACC_HUD", 0), + ("ACCEL_COMMAND", "ACC_CONTROL", 0), + ("AEB_STATUS", "ACC_CONTROL", 0), + ] + checks += [ + ("ACC_HUD", 10), + ("EPB_STATUS", 50), + ("GAS_PEDAL_2", 100), + ("ACC_CONTROL", 50), + ] if CP.openpilotLongitudinalControl: signals += [("BRAKE_ERROR_1", "STANDSTILL", 1), ("BRAKE_ERROR_2", "STANDSTILL", 1)] @@ -119,27 +131,43 @@ def get_can_signals(CP): ("DOOR_OPEN_RL", "DOORS_STATUS", 1), ("DOOR_OPEN_RR", "DOORS_STATUS", 1), ("WHEELS_MOVING", "STANDSTILL", 1)] - checks += [("DOORS_STATUS", 3)] + checks += [ + ("DOORS_STATUS", 3), + ("STANDSTILL", 50), + ] if CP.carFingerprint == CAR.CIVIC: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_FEEDBACK", 0), ("IMPERIAL_UNIT", "HUD_SETTING", 0), ("EPB_STATE", "EPB_STATUS", 0)] + checks += [ + ("HUD_SETTING", 50), + ("EPB_STATUS", 50), + ("GAS_PEDAL_2", 100), + ] elif CP.carFingerprint == CAR.ACURA_ILX: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_BUTTONS", 0)] + checks += [ + ("GAS_PEDAL_2", 100), + ] elif CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE): signals += [("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint == CAR.FIT: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_BUTTONS", 0), ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] + checks += [ + ("GAS_PEDAL_2", 100), + ] elif CP.carFingerprint == CAR.HRV: signals += [("CAR_GAS", "GAS_PEDAL", 0), ("MAIN_ON", "SCM_BUTTONS", 0), ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] - + checks += [ + ("GAS_PEDAL", 100), + ] elif CP.carFingerprint == CAR.ODYSSEY: signals += [("MAIN_ON", "SCM_FEEDBACK", 0), ("EPB_STATE", "EPB_STATUS", 0)] @@ -147,6 +175,9 @@ def get_can_signals(CP): elif CP.carFingerprint == CAR.PILOT: signals += [("MAIN_ON", "SCM_BUTTONS", 0), ("CAR_GAS", "GAS_PEDAL_2", 0)] + checks += [ + ("GAS_PEDAL_2", 0), # TODO: fix this freq, seems this signal isn't present at all on some models + ] elif CP.carFingerprint == CAR.ODYSSEY_CHN: signals += [("MAIN_ON", "SCM_BUTTONS", 0), ("EPB_STATE", "EPB_STATUS", 0)] @@ -278,9 +309,9 @@ class CarState(CarStateBase): ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo) if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.ACCORDH, CAR.CRV_HYBRID, CAR.INSIGHT): - ret.brakePressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] != 0 or \ - (self.brake_switch and self.brake_switch_prev and - cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) + ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or + (self.brake_switch and self.brake_switch_prev and + cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)) self.brake_switch_prev = self.brake_switch self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] else: @@ -313,7 +344,7 @@ class CarState(CarStateBase): self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False if self.CP.carFingerprint in HONDA_BOSCH: - ret.stockAeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) + ret.stockAeb = bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) else: ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5) @@ -325,7 +356,7 @@ class CarState(CarStateBase): self.stock_hud = cp_cam.vl["ACC_HUD"] self.stock_brake = cp_cam.vl["BRAKE_COMMAND"] - if self.CP.carFingerprint in (CAR.CRV_5G, ): + if self.CP.enableBsm and self.CP.carFingerprint in (CAR.CRV_5G, ): # BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0 # more info here: https://github.com/commaai/openpilot/pull/1867 ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]['BSM_ALERT'] == 1 @@ -343,10 +374,12 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): signals = [] - if CP.carFingerprint in HONDA_BOSCH: - signals += [("ACCEL_COMMAND", "ACC_CONTROL", 0), - ("AEB_STATUS", "ACC_CONTROL", 0)] - else: + # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering + checks = [(0xe4, 100)] + if CP.carFingerprint in [CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: + checks = [(0x194, 100)] + + if CP.carFingerprint not in HONDA_BOSCH: signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0), ("AEB_REQ_1", "BRAKE_COMMAND", 0), ("FCW", "BRAKE_COMMAND", 0), @@ -355,24 +388,23 @@ class CarState(CarStateBase): ("FCM_OFF_2", "ACC_HUD", 0), ("FCM_PROBLEM", "ACC_HUD", 0), ("ICONS", "ACC_HUD", 0)] - - # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering - checks = [(0xe4, 100)] - if CP.carFingerprint in [CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: - checks = [(0x194, 100)] + checks += [ + ("ACC_HUD", 10), + ("BRAKE_COMMAND", 50), + ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) @staticmethod def get_body_can_parser(CP): - signals = [] - checks = [] - - if CP.carFingerprint == CAR.CRV_5G: - signals += [("BSM_ALERT", "BSM_STATUS_RIGHT", 0), - ("BSM_ALERT", "BSM_STATUS_LEFT", 0)] + if CP.enableBsm and CP.carFingerprint == CAR.CRV_5G: + signals = [("BSM_ALERT", "BSM_STATUS_RIGHT", 0), + ("BSM_ALERT", "BSM_STATUS_LEFT", 0)] + checks = [ + ("BSM_STATUS_LEFT", 3), + ("BSM_STATUS_RIGHT", 3), + ] bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port) return CANParser(DBC[CP.carFingerprint]['body'], signals, checks, bus_body) - return None diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 112bfdbc2..86c39b124 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -135,6 +135,9 @@ class CarInterface(CarInterfaceBase): ret.enableGasInterceptor = 0x201 in fingerprint[0] ret.openpilotLongitudinalControl = ret.enableCamera + if candidate == CAR.CRV_5G: + ret.enableBsm = 0x12f8bfa7 in fingerprint[0] + cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 6acec73d3..45e015af6 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -11,7 +11,7 @@ def _create_nidec_can_parser(car_fingerprint): ['REL_SPEED'] * 16, [0x400] + radar_messages[1:] * 4, [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)) - checks = list(zip([0x445], [20])) + checks = [(s[1], 20) for s in signals] return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index fe9a29080..a63b45976 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -34,27 +34,27 @@ VISUAL_HUD = { VisualAlert.speedTooHigh: 8} class CAR: - ACCORD = "HONDA ACCORD 2018 SPORT 2T" - ACCORD_15 = "HONDA ACCORD 2018 LX 1.5T" - ACCORDH = "HONDA ACCORD 2018 HYBRID TOURING" - CIVIC = "HONDA CIVIC 2016 TOURING" - CIVIC_BOSCH = "HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019" - CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL" - ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS" - CRV = "HONDA CR-V 2016 TOURING" - CRV_5G = "HONDA CR-V 2017 EX" - CRV_EU = "HONDA CR-V 2016 EXECUTIVE" - CRV_HYBRID = "HONDA CR-V 2019 HYBRID" - FIT = "HONDA FIT 2018 EX" - HRV = "HONDA HRV 2019 TOURING" - 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" - INSIGHT = "HONDA INSIGHT 2019 TOURING" + ACCORD = "HONDA ACCORD 2T 2018" + ACCORD_15 = "HONDA ACCORD 1.5T 2018" + ACCORDH = "HONDA ACCORD HYBRID 2018" + CIVIC = "HONDA CIVIC 2016" + CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019" + CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019" + ACURA_ILX = "ACURA ILX 2016" + CRV = "HONDA CR-V 2016" + CRV_5G = "HONDA CR-V 2017" + CRV_EU = "HONDA CR-V EU 2016" + CRV_HYBRID = "HONDA CR-V HYBRID 2019" + FIT = "HONDA FIT 2018" + HRV = "HONDA HRV 2019" + ODYSSEY = "HONDA ODYSSEY 2018" + ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019" + ACURA_RDX = "ACURA RDX 2018" + ACURA_RDX_3G = "ACURA RDX 2020" + PILOT = "HONDA PILOT 2017" + PILOT_2019 = "HONDA PILOT 2019" + RIDGELINE = "HONDA RIDGELINE 2017" + INSIGHT = "HONDA INSIGHT 2019" # diag message that in some Nidec cars only appear with 1s freq if VIN query is performed DIAG_MSGS = {1600: 5, 1601: 8} @@ -169,6 +169,9 @@ FW_VERSIONS = { b'37805-6B2-A650\x00\x00', b'37805-6B2-A660\x00\x00', b'37805-6B2-A720\x00\x00', + b'37805-6B2-A810\x00\x00', + b'37805-6B2-A820\x00\x00', + b'37805-6B2-A920\x00\x00', b'37805-6B2-M520\x00\x00', ], (Ecu.shiftByWire, 0x18da0bf1, None): [ @@ -177,6 +180,7 @@ FW_VERSIONS = { (Ecu.transmission, 0x18da1ef1, None): [ b'28102-6B8-A560\x00\x00', b'28102-6B8-A570\x00\x00', + b'28102-6B8-A700\x00\x00', b'28102-6B8-A800\x00\x00', b'28102-6B8-C570\x00\x00', b'28102-6B8-M520\x00\x00', @@ -185,16 +189,19 @@ FW_VERSIONS = { b'46114-TVA-A060\x00\x00', b'46114-TVA-A080\x00\x00', b'46114-TVA-A120\x00\x00', + b'46114-TVA-A320\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TVA-C040\x00\x00', b'57114-TVA-C050\x00\x00', b'57114-TVA-C060\x00\x00', + b'57114-TVA-C530\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TVA,A150\x00\x00', # modified firmware b'39990-TVA-A150\x00\x00', b'39990-TVA-A160\x00\x00', + b'39990-TVA-A340\x00\x00', b'39990-TVA-X030\x00\x00', ], (Ecu.unknown, 0x18da3af1, None): [ @@ -202,15 +209,21 @@ FW_VERSIONS = { ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TVA-A460\x00\x00', + b'77959-TVA-L420\x00\x00', b'77959-TVA-X330\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TVA-A210\x00\x00', b'78109-TVC-A010\x00\x00', b'78109-TVC-A020\x00\x00', + b'78109-TVC-A030\x00\x00', b'78109-TVC-A110\x00\x00', + b'78109-TVC-A130\x00\x00', b'78109-TVC-A210\x00\x00', + b'78109-TVC-A220\x00\x00', b'78109-TVC-C110\x00\x00', + b'78109-TVC-L010\x00\x00', + b'78109-TVC-L210\x00\x00', b'78109-TVC-M510\x00\x00', ], (Ecu.hud, 0x18da61f1, None): [ @@ -219,20 +232,26 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TVA-A160\x00\x00', b'36802-TVA-A170\x00\x00', + b'36802-TVC-A330\x00\x00', b'36802-TWA-A070\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TVA-A060\x00\x00', + b'36161-TVC-A330\x00\x00', b'36161-TWA-A070\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TVA-A010\x00\x00', + b'38897-TVA-A020\x00\x00', ], }, CAR.ACCORD_15: { (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-6A0-9520\x00\x00', b'37805-6A0-9620\x00\x00', + b'37805-6A0-9720\x00\x00', b'37805-6A0-A540\x00\x00', + b'37805-6A0-A550\x00\x00', b'37805-6A0-A640\x00\x00', b'37805-6A0-A650\x00\x00', b'37805-6A0-A740\x00\x00', @@ -248,6 +267,7 @@ FW_VERSIONS = { b'28101-6A7-A230\x00\x00', b'28101-6A7-A320\x00\x00', b'28101-6A7-A330\x00\x00', + b'28101-6A7-A410\x00\x00', b'28101-6A7-A510\x00\x00', b'28101-6A9-H140\x00\x00', b'28101-6A9-H420\x00\x00', @@ -264,15 +284,19 @@ FW_VERSIONS = { b'46114-TVE-H560\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TBX-H310\x00\x00', b'78109-TVA-A010\x00\x00', + b'78109-TVA-A020\x00\x00', b'78109-TVA-A110\x00\x00', + b'78109-TVA-A120\x00\x00', b'78109-TVA-A210\x00\x00', b'78109-TVA-A220\x00\x00', b'78109-TVA-A310\x00\x00', b'78109-TVA-C010\x00\x00', + b'78109-TVA-L010\x00\x00', + b'78109-TVA-L210\x00\x00', b'78109-TVE-H610\x00\x00', b'78109-TWA-A210\x00\x00', - b'78109-TBX-H310\x00\x00', ], (Ecu.hud, 0x18da61f1, None): [ b'78209-TVA-A010\x00\x00', @@ -288,8 +312,9 @@ FW_VERSIONS = { b'77959-TBX-H230\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ - b'57114-TVA-B050\x00\x00', b'57114-TVA-B040\x00\x00', + b'57114-TVA-B050\x00\x00', + b'57114-TVA-B060\x00\x00', b'57114-TVE-H250\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ @@ -310,39 +335,47 @@ FW_VERSIONS = { CAR.ACCORDH: { (Ecu.gateway, 0x18daeff1, None): [ b'38897-TWA-A120\x00\x00', + b'38897-TWD-J020\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TWA-A040\x00\x00', b'57114-TWA-A050\x00\x00', + b'57114-TWA-B520\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TWA-A440\x00\x00', + b'77959-TWA-L420\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TWA-A010\x00\x00', b'78109-TWA-A020\x00\x00', + b'78109-TWA-A030\x00\x00', b'78109-TWA-A110\x00\x00', b'78109-TWA-A120\x00\x00', b'78109-TWA-A210\x00\x00', b'78109-TWA-A220\x00\x00', - + b'78109-TWA-L010\x00\x00', ], (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TWA-A910\x00\x00', ], (Ecu.hud, 0x18da61f1, None): [ b'78209-TVA-A010\x00\x00', + b'78209-TVA-A110\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TWA-A070\x00\x00', + b'36161-TWA-A330\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TWA-A080\x00\x00', b'36802-TWA-A070\x00\x00', + b'36802-TWA-A330\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TVA-A160\x00\x00', b'39990-TVA-A150\x00\x00', + b'39990-TVA-A340\x00\x00', ], }, CAR.CIVIC: { @@ -352,23 +385,29 @@ FW_VERSIONS = { b'37805-5AA-A670\x00\x00', b'37805-5AA-A680\x00\x00', b'37805-5AA-A810\x00\x00', + b'37805-5AA-C640\x00\x00', b'37805-5AA-C680\x00\x00', b'37805-5AA-C820\x00\x00', b'37805-5AA-L650\x00\x00', b'37805-5AA-L660\x00\x00', b'37805-5AA-L680\x00\x00', b'37805-5AA-L690\x00\x00', + b'37805-5AG-Q710\x00\x00', b'37805-5AJ-A610\x00\x00', b'37805-5AJ-A620\x00\x00', + b'37805-5AJ-L610\x00\x00', b'37805-5BA-A310\x00\x00', b'37805-5BA-A510\x00\x00', b'37805-5BA-A740\x00\x00', b'37805-5BA-A760\x00\x00', + b'37805-5BA-A930\x00\x00', b'37805-5BA-A960\x00\x00', + b'37805-5BA-C860\x00\x00', + b'37805-5BA-L410\x00\x00', + b'37805-5BA-L760\x00\x00', b'37805-5BA-L930\x00\x00', b'37805-5BA-L940\x00\x00', b'37805-5BA-L960\x00\x00', - b'37805-5AG-Q710\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A040\x00\x00', @@ -406,6 +445,7 @@ FW_VERSIONS = { b'78109-TBA-A510\x00\x00', b'78109-TBA-A520\x00\x00', b'78109-TBA-A530\x00\x00', + b'78109-TBA-C520\x00\x00', b'78109-TBC-A310\x00\x00', b'78109-TBC-A320\x00\x00', b'78109-TBC-A510\x00\x00', @@ -414,9 +454,10 @@ FW_VERSIONS = { b'78109-TBC-C510\x00\x00', b'78109-TBC-C520\x00\x00', b'78109-TBC-C530\x00\x00', + b'78109-TBH-A510\x00\x00', b'78109-TBH-A530\x00\x00', - b'78109-TEG-A310\x00\x00', b'78109-TED-Q510\x00\x00', + b'78109-TEG-A310\x00\x00', ], (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TBA-A020\x00\x00', @@ -439,6 +480,9 @@ FW_VERSIONS = { b'37805-5AA-A950\x00\x00', b'37805-5AA-L940\x00\x00', b'37805-5AA-L950\x00\x00', + b'37805-5AG-Z910\x00\x00', + b'37805-5AJ-A750\x00\x00', + b'37805-5AJ-L750\x00\x00', b'37805-5AN-A750\x00\x00', b'37805-5AN-A830\x00\x00', b'37805-5AN-A840\x00\x00', @@ -452,6 +496,9 @@ FW_VERSIONS = { b'37805-5AN-AR20\x00\x00', b'37805-5AN-CH20\x00\x00', b'37805-5AN-E630\x00\x00', + b'37805-5AN-E720\x00\x00', + b'37805-5AN-E820\x00\x00', + b'37805-5AN-J820\x00\x00', b'37805-5AN-L840\x00\x00', b'37805-5AN-L930\x00\x00', b'37805-5AN-L940\x00\x00', @@ -461,15 +508,19 @@ FW_VERSIONS = { b'37805-5AN-LR20\x00\x00', b'37805-5AN-LS20\x00\x00', b'37805-5AW-G720\x00\x00', - b'37805-5AN-E820\x00\x00', b'37805-5AZ-E850\x00\x00', + b'37805-5AZ-G740\x00\x00', + b'37805-5AZ-G840\x00\x00', + b'37805-5BB-A530\x00\x00', + b'37805-5BB-A540\x00\x00', b'37805-5BB-A630\x00\x00', b'37805-5BB-A640\x00\x00', b'37805-5BB-C540\x00\x00', b'37805-5BB-C630\x00\x00', + b'37805-5BB-C640\x00\x00', b'37805-5BB-L540\x00\x00', + b'37805-5BB-L630\x00\x00', b'37805-5BB-L640\x00\x00', - b'37805-5AZ-G740\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A920\x00\x00', @@ -478,6 +529,7 @@ FW_VERSIONS = { b'28101-5CG-C220\x00\x00', b'28101-5CG-C320\x00\x00', b'28101-5CG-G020\x00\x00', + b'28101-5CG-L020\x00\x00', b'28101-5CK-A130\x00\x00', b'28101-5CK-A140\x00\x00', b'28101-5CK-A150\x00\x00', @@ -485,35 +537,45 @@ FW_VERSIONS = { b'28101-5CK-C140\x00\x00', b'28101-5CK-C150\x00\x00', b'28101-5CK-G210\x00\x00', + b'28101-5CK-J710\x00\x00', + b'28101-5CK-Q610\x00\x00', b'28101-5DJ-A610\x00\x00', b'28101-5DJ-A710\x00\x00', b'28101-5DV-E330\x00\x00', b'28101-5DV-E610\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TBG-A330\x00\x00', b'57114-TBG-A340\x00\x00', b'57114-TBG-A350\x00\x00', b'57114-TGG-A340\x00\x00', b'57114-TGG-C320\x00\x00', + b'57114-TGG-G320\x00\x00', b'57114-TGG-L320\x00\x00', b'57114-TGG-L330\x00\x00', + b'57114-TGK-T320\x00\x00', b'57114-TGL-G330\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBA-C020\x00\x00', b'39990-TBA-C120\x00\x00', + b'39990-TEA-T820\x00\x00', b'39990-TEZ-T020\x00\x00', b'39990-TGG-A020\x00\x00', b'39990-TGG-A120\x00\x00', - b'39990-TGN-E120\x00\x00', + b'39990-TGG-J510\x00\x00', b'39990-TGL-E130\x00\x00', + b'39990-TGN-E120\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TBA-A060\x00\x00', + b'77959-TBG-A050\x00\x00', b'77959-TEA-G020\x00\x00', b'77959-TGG-A020\x00\x00', b'77959-TGG-A030\x00\x00', b'77959-TGG-G010\x00\x00', + b'77959-TGG-J320\x00\x00', + b'77959-TGG-Z820\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBA-A110\x00\x00', @@ -521,7 +583,10 @@ FW_VERSIONS = { b'78109-TBA-C340\x00\x00', b'78109-TBA-C910\x00\x00', b'78109-TBC-A740\x00\x00', + b'78109-TBG-A110\x00\x00', + b'78109-TEG-A720\x00\x00', b'78109-TFJ-G020\x00\x00', + b'78109-TGG-9020\x00\x00', b'78109-TGG-A210\x00\x00', b'78109-TGG-A220\x00\x00', b'78109-TGG-A310\x00\x00', @@ -533,17 +598,23 @@ FW_VERSIONS = { b'78109-TGG-A820\x00\x00', b'78109-TGG-C220\x00\x00', b'78109-TGG-G030\x00\x00', + b'78109-TGG-G230\x00\x00', + b'78109-TGG-G410\x00\x00', + b'78109-TGK-Z410\x00\x00', b'78109-TGL-G120\x00\x00', b'78109-TGL-G130\x00\x00', - b'78109-TGG-G410\x00\x00', + b'78109-TGL-G230\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TBA-A150\x00\x00', + b'36802-TBA-A160\x00\x00', b'36802-TFJ-G060\x00\x00', b'36802-TGG-A050\x00\x00', b'36802-TGG-A060\x00\x00', b'36802-TGG-A130\x00\x00', b'36802-TGG-G040\x00\x00', + b'36802-TGG-G130\x00\x00', + b'36802-TGK-Q120\x00\x00', b'36802-TGL-G040\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ @@ -554,13 +625,19 @@ FW_VERSIONS = { b'36161-TGG-A080\x00\x00', b'36161-TGG-A120\x00\x00', b'36161-TGG-G050\x00\x00', + b'36161-TGG-G130\x00\x00', + b'36161-TGK-Q120\x00\x00', b'36161-TGL-G050\x00\x00', b'36161-TGL-G070\x00\x00', + b'36161-TGG-G070\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TBA-A110\x00\x00', b'38897-TBA-A020\x00\x00', ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'39494-TGL-G030\x00\x00', + ], }, CAR.CIVIC_BOSCH_DIESEL: { (Ecu.programmedFuelInjection, 0x18da10f1, None): [ @@ -601,21 +678,42 @@ FW_VERSIONS = { ], }, CAR.CRV: { - (Ecu.vsa, 0x18da28f1, None): [b'57114-T1W-A230\x00\x00',], - (Ecu.srs, 0x18da53f1, None): [b'77959-T0A-A230\x00\x00',], - (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-T1W-A210\x00\x00',], - (Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T1W-A830\x00\x00',], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-T1W-A230\x00\x00', + b'57114-T1W-A240\x00\x00', + b'57114-TFF-A940\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T0A-A230\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T1W-A210\x00\x00', + b'78109-T1W-C210\x00\x00', + b'78109-T1X-A210\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T1W-A830\x00\x00', + b'36161-T1W-C830\x00\x00', + b'36161-T1X-A830\x00\x00', + ], }, CAR.CRV_5G: { (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5PA-3060\x00\x00', b'37805-5PA-3080\x00\x00', + b'37805-5PA-3180\x00\x00', b'37805-5PA-4050\x00\x00', + b'37805-5PA-4150\x00\x00', b'37805-5PA-6520\x00\x00', b'37805-5PA-6530\x00\x00', b'37805-5PA-6630\x00\x00', + b'37805-5PA-6640\x00\x00', + b'37805-5PA-7630\x00\x00', + b'37805-5PA-9630\x00\x00', b'37805-5PA-9640\x00\x00', + b'37805-5PA-9730\x00\x00', b'37805-5PA-9830\x00\x00', + b'37805-5PA-9840\x00\x00', b'37805-5PA-A650\x00\x00', b'37805-5PA-A670\x00\x00', b'37805-5PA-A680\x00\x00', @@ -623,8 +721,13 @@ FW_VERSIONS = { b'37805-5PA-A870\x00\x00', b'37805-5PA-A880\x00\x00', b'37805-5PA-A890\x00\x00', + b'37805-5PA-AB10\x00\x00', + b'37805-5PA-AD10\x00\x00', b'37805-5PA-AF20\x00\x00', + b'37805-5PA-C680\x00\x00', b'37805-5PD-Q630\x00\x00', + b'37805-5PF-F730\x00\x00', + b'37805-5PF-M630\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5RG-A020\x00\x00', @@ -638,19 +741,24 @@ FW_VERSIONS = { b'28101-5RH-A120\x00\x00', b'28101-5RH-A220\x00\x00', b'28101-5RL-Q010\x00\x00', + b'28101-5RM-F010\x00\x00', + b'28101-5RM-K010\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TLA-A040\x00\x00', b'57114-TLA-A050\x00\x00', b'57114-TLA-A060\x00\x00', b'57114-TLB-A830\x00\x00', + b'57114-TMC-Z040\x00\x00', b'57114-TMC-Z050\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ - b'39990-TLA,A040\x00\x00', # modified firmware b'39990-TLA-A040\x00\x00', b'39990-TLA-A110\x00\x00', b'39990-TLA-A220\x00\x00', + b'39990-TLA,A040\x00\x00', # modified firmware + b'39990-TME-T030\x00\x00', + b'39990-TME-T120\x00\x00', b'39990-TMT-T010\x00\x00', ], (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ @@ -664,11 +772,17 @@ FW_VERSIONS = { b'78109-TLA-A120\x00\x00', b'78109-TLA-A210\x00\x00', b'78109-TLA-A220\x00\x00', + b'78109-TLA-C110\x00\x00', b'78109-TLA-C210\x00\x00', + b'78109-TLA-C310\x00\x00', + b'78109-TLB-A020\x00\x00', b'78109-TLB-A110\x00\x00', + b'78109-TLB-A120\x00\x00', b'78109-TLB-A210\x00\x00', b'78109-TLB-A220\x00\x00', b'78109-TMC-Q210\x00\x00', + b'78109-TMM-F210\x00\x00', + b'78109-TMM-M110\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TLA-A010\x00\x00', @@ -679,6 +793,7 @@ FW_VERSIONS = { b'36802-TLA-A040\x00\x00', b'36802-TLA-A050\x00\x00', b'36802-TLA-A060\x00\x00', + b'36802-TMC-Q040\x00\x00', b'36802-TMC-Q070\x00\x00', b'36802-TNY-A030\x00\x00', ], @@ -686,6 +801,8 @@ FW_VERSIONS = { b'36161-TLA-A060\x00\x00', b'36161-TLA-A070\x00\x00', b'36161-TLA-A080\x00\x00', + b'36161-TMC-Q020\x00\x00', + b'36161-TMC-Q030\x00\x00', b'36161-TMC-Q040\x00\x00', b'36161-TNY-A020\x00\x00', ], @@ -696,6 +813,8 @@ FW_VERSIONS = { b'77959-TLA-A410\x00\x00', b'77959-TLA-A420\x00\x00', b'77959-TLA-Q040\x00\x00', + b'77959-TLA-Z040\x00\x00', + b'77959-TMM-F040\x00\x00', ], }, CAR.CRV_EU: { @@ -730,20 +849,23 @@ FW_VERSIONS = { (Ecu.gateway, 0x18daeff1, None): [ b'38897-TMA-H110\x00\x00', b'38897-TPG-A110\x00\x00', + b'38897-TPG-A210\x00\x00', ], (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TMB-H510\x00\x00', b'54008-TMB-H610\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TMB-H040\x00\x00', b'36161-TPA-E050\x00\x00', b'36161-TPG-A030\x00\x00', - b'36161-TMB-H040\x00\x00', + b'36161-TPG-A040\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TMB-H220\x00\x00', b'78109-TPA-G520\x00\x00', b'78109-TPG-A110\x00\x00', - b'78109-TMB-H220\x00\x00', + b'78109-TPG-A210\x00\x00', ], (Ecu.hud, 0x18da61f1, None): [ b'78209-TLA-X010\x00\x00', @@ -754,8 +876,10 @@ FW_VERSIONS = { b'36802-TMB-H040\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ - b'77959-TLA-G220\x00\x00', b'77959-TLA-C320\x00\x00', + b'77959-TLA-C410\x00\x00', + b'77959-TLA-C420\x00\x00', + b'77959-TLA-G220\x00\x00', b'77959-TLA-H240\x00\x00', ], }, @@ -771,6 +895,7 @@ FW_VERSIONS = { b'38897-T5A-J010\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T5A-A410\x00\x00', b'78109-T5A-A420\x00\x00', b'78109-T5A-A910\x00\x00', ], @@ -788,11 +913,15 @@ FW_VERSIONS = { b'38897-THR-A020\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-5MR-A240\x00\x00', b'37805-5MR-A250\x00\x00', b'37805-5MR-A310\x00\x00', + b'37805-5MR-A740\x00\x00', b'37805-5MR-A750\x00\x00', b'37805-5MR-A840\x00\x00', b'37805-5MR-C620\x00\x00', + b'37805-5MR-D530\x00\x00', + b'37805-5MR-K730\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-THR-A020\x00\x00', @@ -801,8 +930,10 @@ FW_VERSIONS = { (Ecu.srs, 0x18da53f1, None): [ b'77959-THR-A010\x00\x00', b'77959-THR-A110\x00\x00', + b'77959-THR-X010\x00\x00', ], (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-THR-A020\x00\x00', b'36161-THR-A030\x00\x00', b'36161-THR-A110\x00\x00', b'36161-THR-A720\x00\x00', @@ -810,8 +941,11 @@ FW_VERSIONS = { b'36161-THR-A810\x00\x00', b'36161-THR-A910\x00\x00', b'36161-THR-C010\x00\x00', + b'36161-THR-D110\x00\x00', + b'36161-THR-K020\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5NZ-A110\x00\x00', b'28101-5NZ-A310\x00\x00', b'28101-5NZ-C310\x00\x00', b'28102-5MX-A001\x00\x00', @@ -821,6 +955,10 @@ FW_VERSIONS = { b'28102-5MX-A900\x00\x00', b'28102-5MX-A910\x00\x00', b'28102-5MX-C001\x00\x00', + b'28102-5MX-D001\x00\x00', + b'28102-5MX-D710\x00\x00', + b'28102-5MX-K610\x00\x00', + b'28103-5NZ-A100\x00\x00', b'28103-5NZ-A300\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ @@ -828,8 +966,11 @@ FW_VERSIONS = { b'57114-THR-A110\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-THR-A220\x00\x00', b'78109-THR-A230\x00\x00', + b'78109-THR-A420\x00\x00', b'78109-THR-A430\x00\x00', + b'78109-THR-A720\x00\x00', b'78109-THR-A820\x00\x00', b'78109-THR-A830\x00\x00', b'78109-THR-AB20\x00\x00', @@ -837,12 +978,17 @@ FW_VERSIONS = { b'78109-THR-AB40\x00\x00', b'78109-THR-AC20\x00\x00', b'78109-THR-AC40\x00\x00', + b'78109-THR-AC50\x00\x00', b'78109-THR-AE20\x00\x00', b'78109-THR-AE40\x00\x00', + b'78109-THR-AK10\x00\x00', b'78109-THR-AL10\x00\x00', b'78109-THR-AN10\x00\x00', b'78109-THR-C330\x00\x00', b'78109-THR-CE20\x00\x00', + b'78109-THR-DA20\x00\x00', + b'78109-THR-DA40\x00\x00', + b'78109-THR-K120\x00\x00', ], (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-THR-A020\x00\x00', @@ -851,57 +997,79 @@ FW_VERSIONS = { CAR.PILOT: { (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TG7-A520\x00\x00', + b'54008-TG7-A530\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ - b'28101-5EZ-A210\x00\x00', - b'28101-5EZ-A100\x00\x00', - b'28101-5EZ-A060\x00\x00', + b'28101-5EY-A050\x00\x00', + b'28101-5EY-A100\x00\x00', b'28101-5EZ-A050\x00\x00', + b'28101-5EZ-A060\x00\x00', + b'28101-5EZ-A100\x00\x00', + b'28101-5EZ-A210\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-RLV-C910\x00\x00', - b'37805-RLV-C520\x00\x00', - b'37805-RLV-C510\x00\x00', + b'37805-RLV-4060\x00\x00', b'37805-RLV-4070\x00\x00', b'37805-RLV-A830\x00\x00', + b'37805-RLV-A840\x00\x00', + b'37805-RLV-C430\x00\x00', + b'37805-RLV-C510\x00\x00', + b'37805-RLV-C520\x00\x00', + b'37805-RLV-C530\x00\x00', + b'37805-RLV-C910\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ - b'39990-TG7-A040\x00\x00', b'39990-TG7-A030\x00\x00', + b'39990-TG7-A040\x00\x00', + b'39990-TG7-A060\x00\x00', ], (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TG7-A520\x00\x00', - b'36161-TG7-A820\x00\x00', b'36161-TG7-A720\x00\x00', + b'36161-TG7-A820\x00\x00', + b'36161-TG7-C520\x00\x00', + b'36161-TG7-D520\x00\x00', + b'36161-TG8-A520\x00\x00', + b'36161-TG8-A720\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TG7-A110\x00\x00', b'77959-TG7-A020\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TG7-A720\x00\x00', - b'78109-TG7-A520\x00\x00', - b'78109-TG7-A420\x00\x00', b'78109-TG7-A040\x00\x00', + b'78109-TG7-A050\x00\x00', + b'78109-TG7-A420\x00\x00', + b'78109-TG7-A520\x00\x00', + b'78109-TG7-A720\x00\x00', + b'78109-TG7-D020\x00\x00', + b'78109-TG8-A420\x00\x00', + b'78109-TG8-A520\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TG7-A130\x00\x00', b'57114-TG7-A140\x00\x00', - b'57114-TG7-A240\x00\x00', b'57114-TG7-A230\x00\x00', + b'57114-TG7-A240\x00\x00', + b'57114-TG8-A140\x00\x00', + b'57114-TG8-A240\x00\x00', ], }, CAR.PILOT_2019: { (Ecu.eps, 0x18da30f1, None): [ b'39990-TG7-A060\x00\x00', + b'39990-TG7-A070\x00\x00', b'39990-TGS-A230\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TG7-A030\x00\x00', + b'38897-TG7-A040\x00\x00', b'38897-TG7-A110\x00\x00', b'38897-TG7-A210\x00\x00', ], (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-TG7-A310\x00\x00', b'36161-TG7-A630\x00\x00', b'36161-TG7-A930\x00\x00', b'36161-TG8-A630\x00\x00', @@ -913,11 +1081,16 @@ FW_VERSIONS = { b'77959-TGS-A010\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TG7-AJ10\x00\x00', b'78109-TG7-AJ20\x00\x00', b'78109-TG7-AK10\x00\x00', b'78109-TG7-AK20\x00\x00', + b'78109-TG7-AM20\x00\x00', b'78109-TG7-AP10\x00\x00', b'78109-TG7-AP20\x00\x00', + b'78109-TG7-AS20\x00\x00', + b'78109-TG7-AU20\x00\x00', + b'78109-TG8-AJ10\x00\x00', b'78109-TG8-AJ20\x00\x00', b'78109-TGS-AK20\x00\x00', b'78109-TGS-AP20\x00\x00', @@ -931,12 +1104,36 @@ FW_VERSIONS = { b'57114-TGT-A530\x00\x00', ], }, + CAR.ACURA_RDX: { + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TX5-A220\x00\x00', + b'57114-TX4-A220\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-TX5-A030\x00\x00', + b'36161-TX4-A030\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TX4-C010\x00\x00', + b'77959-TX4-B010\x00\x00', + b'77959-TX4-C020\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TX5-A310\x00\x00', + b'78109-TX4-A210\x00\x00', + b'78109-TX4-A310\x00\x00', + ], + }, CAR.ACURA_RDX_3G: { (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5YF-A230\x00\x00', + b'37805-5YF-A320\x00\x00', + b'37805-5YF-A330\x00\x00', b'37805-5YF-A420\x00\x00', b'37805-5YF-A430\x00\x00', + b'37805-5YF-A870\x00\x00', b'37805-5YF-C210\x00\x00', + b'37805-5YF-C410\000\000', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TJB-A040\x00\x00', @@ -952,14 +1149,20 @@ FW_VERSIONS = { b'54008-TJB-A520\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ + b'28102-5YK-A630\x00\x00', b'28102-5YK-A700\x00\x00', b'28102-5YK-A711\x00\x00', + b'28102-5YL-A700\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TJB-A240\x00\x00', b'78109-TJB-AB10\x00\x00', b'78109-TJB-AD10\x00\x00', b'78109-TJB-AF10\x00\x00', b'78109-TJB-AW10\x00\x00', + b'78109-TJC-AA10\x00\x00', + b'78109-TJC-AD10\x00\x00', + b'78109-TJB-AS10\000\000', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TJB-A040\x00\x00', @@ -970,12 +1173,14 @@ FW_VERSIONS = { b'46114-TJB-A060\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TJB-A040\x00\x00', b'38897-TJB-A110\x00\x00', b'38897-TJB-A120\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TJB-A030\x00\x00', b'39990-TJB-A040\x00\x00', + b'39990-TJB-A130\x00\x00' ], }, CAR.RIDGELINE: { @@ -1001,6 +1206,7 @@ FW_VERSIONS = { b'78109-T6Z-A510\x00\x00', b'78109-T6Z-A710\x00\x00', b'78109-T6Z-AA10\x00\x00', + b'78109-T6Z-C620\x00\x00', b'78109-TJZ-A510\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ @@ -1073,14 +1279,17 @@ FW_VERSIONS = { b'38897-TX6-A010\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-TV9-A140\x00\x00', b'36161-TX6-A030\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ + b'77959-TX6-A230\x00\x00', b'77959-TX6-C210\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-T3R-A120\x00\x00', b'78109-T3R-A410\x00\x00', + b'78109-TV9-A510\x00\x00', ], }, } diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index b0803698b..c74a83b92 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -34,7 +34,7 @@ class CarState(CarStateBase): ret.steeringTorque = cp.vl["MDPS12"]['CR_Mdps_StrColTq'] ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq'] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - ret.steerWarning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0 + ret.steerWarning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0 or cp.vl["MDPS12"]['CF_Mdps_ToiFlt'] != 0 # cruise state if self.CP.openpilotLongitudinalControl: @@ -125,7 +125,7 @@ class CarState(CarStateBase): ret.stockAeb = cp.vl["SCC12"]['AEB_CmdAct'] != 0 ret.stockFcw = cp.vl["SCC12"]['CF_VSM_Warn'] == 2 - if self.CP.carFingerprint in FEATURES["use_bsm"]: + if self.CP.enableBsm: ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0 @@ -189,12 +189,10 @@ class CarState(CarStateBase): ("ESC_Off_Step", "TCS15", 0), ("AVH_LAMP", "TCS15", 0), - ("CF_Lvr_GearInf", "LVR11", 0), # Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) - ("CR_Mdps_StrColTq", "MDPS12", 0), ("CF_Mdps_ToiActive", "MDPS12", 0), ("CF_Mdps_ToiUnavail", "MDPS12", 0), - ("CF_Mdps_FailStat", "MDPS12", 0), + ("CF_Mdps_ToiFlt", "MDPS12", 0), ("CR_Mdps_OutTq", "MDPS12", 0), ("SAS_Angle", "SAS11", 0), @@ -215,6 +213,7 @@ class CarState(CarStateBase): ("CLU11", 50), ("ESP12", 100), ("CGW1", 10), + ("CGW2", 5), ("CGW4", 5), ("WHL_SPD11", 50), ("SAS11", 100), @@ -226,7 +225,7 @@ class CarState(CarStateBase): ("SCC12", 50), ] - if CP.carFingerprint in FEATURES["use_bsm"]: + if CP.enableBsm: signals += [ ("CF_Lca_IndLeft", "LCA11", 0), ("CF_Lca_IndRight", "LCA11", 0), diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 9af39af08..5334471a7 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -144,13 +144,19 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_SELTOS: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1310. + STD_CARGO_KG - ret.wheelbase = 2.6 - ret.steerRatio = 13.73 # Spec - tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + ret.mass = 1337. + STD_CARGO_KG + ret.wheelbase = 2.63 + ret.steerRatio = 14.56 + tire_stiffness_factor = 1 + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [4.] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG @@ -231,6 +237,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor=tire_stiffness_factor) ret.enableCamera = True + ret.enableBsm = 0x58b in fingerprint[0] return ret diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 54fa98f4f..9f778ebfd 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu # Steer torque limits class CarControllerParams: def __init__(self, CP): - if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020]: + if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS]: self.STEER_MAX = 384 else: self.STEER_MAX = 255 @@ -20,7 +20,7 @@ class CarControllerParams: class CAR: # Hyundai - ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017" + ELANTRA = "HYUNDAI ELANTRA 2017" ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019" @@ -28,7 +28,7 @@ class CAR: IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020" KONA = "HYUNDAI KONA 2020" KONA_EV = "HYUNDAI KONA ELECTRIC 2019" - SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" + SANTA_FE = "HYUNDAI SANTA FE 2019" SONATA = "HYUNDAI SONATA 2020" SONATA_LF = "HYUNDAI SONATA 2019" PALISADE = "HYUNDAI PALISADE 2020" @@ -171,70 +171,103 @@ IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA, CAR.KIA_CEED, C FW_VERSIONS = { CAR.IONIQ_EV_2020: { (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEev SCC F-CUP 1.00 1.01 99110-G7000 ', b'\xf1\x00AEev SCC F-CUP 1.00 1.00 99110-G7200 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7310 4APEC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7560 4APEC101', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.01 95740-G2600 190819', + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.03 95740-G2500 190516', b'\xf1\x00AEE MFC AT EUR RHD 1.00 1.01 95740-G2600 190819', ], }, CAR.IONIQ_EV_LTD: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEev SCC F-CUP 1.00 1.00 96400-G7000 ', + b'\xf1\x00AEev SCC F-CUP 1.00 1.00 96400-G7100 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00AE MDPS C 1.00 1.02 56310G7300\x00 4AEEC102', + b'\xf1\x00AE MDPS C 1.00 1.04 56310/G7501 4AEEC104', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G7200 160418', + b'\xf1\x00AEE MFC AT USA LHD 1.00 1.00 95740-G2400 180222', ], }, CAR.SONATA: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ', - b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ', + b'\xf1\x00DN8_ SCC F-CUP 1.00 1.02 99110-L1000 ', + b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', + b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ', b'\xf1\x00DN89110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa \xf1\xa01.00\xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00', b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa \xf1\xa01.00\xaa\xaa\xaa', - b'\xf1\xa01.00', ], (Ecu.esp, 0x7d1, None): [ + b'\xf1\x00DN ESC \a 106 \a\x01 58910-L0100', b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02', b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100\xf1\xa01.04', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100\xf1\xa01.06', - b'\xf1\x00DN ESC \a 106 \a\x01 58910-L0100', b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', + b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300\xf1\xa01.03', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100\xf1\xa01.06', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100\xf1\xa01.06', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100\xf1\xa01.04', ], (Ecu.engine, 0x7e0, None): [ - b'HM6M2_0a0_BD0', + b'\xf1\x81HM6M1_0a0_F00', + b'\xf1\x82DNBVN5GMCCXXXDCA', + b'\xf1\x82DNCVN5GMCCXXXG2B', b'\xf1\x87391162M003\xf1\xa0000F', b'\xf1\x87391162M003\xf1\xa00240', + b'\xf1\x87391162M013\xf1\xa00240', b'HM6M1_0a0_F00', - b'\xf1\x81HM6M1_0a0_F00', + b'HM6M2_0a0_BD0', ], (Ecu.eps, 0x7d4, None): [ - b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103\xf1\xa01.03', - b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101\xf1\xa01.01', - b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101\xf1\xa01.01', - b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101\xf1\xa01.01', - b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101', b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101', + b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101\xf1\xa01.01', + b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101\xf1\xa01.01', + b'\xf1\x8756310-L0210\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC101\xf1\xa01.01', + b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103\xf1\xa01.03', + b'\xf1\x8756310-L1030\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 4DNDC103\xf1\xa01.03', + b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101\xf1\xa01.01', + b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101\xf1\xa01.01', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422', + b'\xf1\x00DN8 MFC AT RUS LHD 1.00 1.03 99211-L1000 190705', b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.00 99211-L0000 190716', b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016', ], (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', + b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1', + b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00HT6TA260BLHT6TA810A1TDN8M25GS0\x00\x00\x00\x00\x00\x00\xaa\x8c\xd9p', b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', + b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:', + b'\xf1\x87SALDBA3510954GJ3ww\x87xUUuWx\x88\x87\x88\x87w\x88wvfwfc_\xf9\xff\x98wO\xffl\xe0\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3573534GJ3\x89\x98\x89\x88EUuWgwvwwwwww\x88\x87xTo\xfa\xff\x86f\x7f\xffo\x0e\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3601464GJ3\x88\x88\x88\x88ffvggwvwvw\x87gww\x87wvo\xfb\xff\x98\x88\x7f\xffjJ\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3753044GJ3UUeVff\x86hwwwwvwwgvfgfvo\xf9\xfffU_\xffC\xae\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3873834GJ3fefVwuwWx\x88\x97\x88w\x88\x97xww\x87wU_\xfb\xff\x86f\x8f\xffN\x04\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA4525334GJ3\x89\x99\x99\x99fevWh\x88\x86\x88fwvgw\x88\x87xfo\xfa\xffuDo\xff\xd1>\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA4626804GJ3wwww\x88\x87\x88xx\x88\x87\x88wwgw\x88\x88\x98\x88\x95_\xf9\xffuDo\xff|\xe7\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA4803224GJ3wwwwwvwg\x88\x88\x98\x88wwww\x87\x88\x88xu\x9f\xfc\xff\x87f\x8f\xff\xea\xea\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA6347404GJ3wwwwff\x86hx\x88\x97\x88\x88\x88\x88\x88vfgf\x88?\xfc\xff\x86Uo\xff\xec/\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA6901634GJ3UUuWVeVUww\x87wwwwwvUge\x86/\xfb\xff\xbb\x99\x7f\xff]2\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA7077724GJ3\x98\x88\x88\x88ww\x97ygwvwww\x87ww\x88\x87x\x87_\xfd\xff\xba\x99o\xff\x99\x01\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SAMDBA8054504GJ3gw\x87xffvgffffwwwweUVUf?\xfc\xffvU_\xff\xddl\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00', ], }, CAR.SONATA_LF: { @@ -246,10 +279,10 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x81606D5K51\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81606G1051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00LFF LKAS AT USA LHD 1.00 1.01 95740-C1000 E51', - b'\xf1\xa01.01', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', @@ -259,37 +292,88 @@ FW_VERSIONS = { }, CAR.SANTA_FE: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\x00TM__ SCC F-CUP 1.00 1.03 99110-S2000 \xf1\xa01.03', + b'\xf1\x00TM__ SCC F-CUP 1.00 1.01 99110-S2000 \xf1\xa01.01', b'\xf1\x00TM__ SCC F-CUP 1.00 1.02 99110-S2000 \xf1\xa01.02', + b'\xf1\x00TM__ SCC F-CUP 1.00 1.03 99110-S2000 \xf1\xa01.03', ], (Ecu.esp, 0x7d1, None): [ + b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650\xf1\xa01.00', + b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650\xf1\xa01.03', b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650\xf1\xa01.04', b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600\xf1\xa01.00', + b'\xf1\x00TM ESC \x02 102\x18\x07\x01 58910-S2600\xf1\xa01.02', + b'\xf1\x00TM ESC \x02 103\x18\x11\x07 58910-S2600\xf1\xa01.03', + b'\xf1\x00TM ESC \x02 104\x19\x07\x07 58910-S2600\xf1\xa01.04', + b'\xf1\x00TM ESC \x03 103\x18\x11\x07 58910-S2600\xf1\xa01.03', + b'\xf1\x00TM ESC \x0c 103\x18\x11\x08 58910-S2650', + b'\xf1\x00TM ESC \x0c 103\x18\x11\x08 58910-S2650\xf1\xa01.03', ], (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81606EA051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81606G1051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x81606G3051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81606EA051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409', + b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8A12', b'\xf1\x00TM MDPS C 1.00 1.01 56340-S2000 9129', - b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00TM MFC AT USA LHD 1.00 1.00 99211-S2000 180409', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87LBJSGA7082574HG0\x87www\x98\x88\x88\x88\x99\xaa\xb9\x9afw\x86gx\x99\xa7\x89co\xf8\xffvU_\xffR\xaf\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\xa6\xe0\x91', + b'\xf1\x87LBKSGA0458404HG0vfvg\x87www\x89\x99\xa8\x99y\xaa\xa7\x9ax\x88\xa7\x88t_\xf9\xff\x86w\x8f\xff\x15x\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\x00\x00\x00', + b'\xf1\x87LDJUEA6010814HG1\x87w\x87x\x86gvw\x88\x88\x98\x88gw\x86wx\x88\x97\x88\x85o\xf8\xff\x86f_\xff\xd37\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', + b'\xf1\x87LDJUEA6458264HG1ww\x87x\x97x\x87\x88\x88\x99\x98\x89g\x88\x86xw\x88\x97x\x86o\xf7\xffvw\x8f\xff3\x9a\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', + b'\xf1\x87LDKUEA2045844HG1wwww\x98\x88x\x87\x88\x88\xa8\x88x\x99\x97\x89x\x88\xa7\x88U\x7f\xf8\xffvfO\xffC\x1e\xf1\x816W3E0051\x00\x00\xf1\x006W351_C2\x00\x006W3E0051\x00\x00TTM4T20NS3\x00\x00\x00\x00', + b'\xf1\x87LDKUEA9993304HG1\x87www\x97x\x87\x88\x99\x99\xa9\x99x\x99\xa7\x89w\x88\x97x\x86_\xf7\xffwwO\xffl#\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS1R\x7f\x90\n', + b'\xf1\x87LDLUEA6061564HG1\xa9\x99\x89\x98\x87wwwx\x88\x97\x88x\x99\xa7\x89x\x99\xa7\x89sO\xf9\xffvU_\xff<\xde\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', b'\xf1\x87LDLUEA6159884HG1\x88\x87hv\x99\x99y\x97\x89\xaa\xb8\x9ax\x99\x87\x89y\x99\xb7\x99\xa7?\xf7\xff\x97wo\xff\xf3\x05\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', + b'\xf1\x87LDLUEA6852664HG1\x97wWu\x97www\x89\xaa\xc8\x9ax\x99\x97\x89x\x99\xa7\x89SO\xf7\xff\xa8\x88\x7f\xff\x03z\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', + b'\xf1\x87LDLUEA6898374HG1fevW\x87wwwx\x88\x97\x88h\x88\x96\x88x\x88\xa7\x88ao\xf9\xff\x98\x99\x7f\xffD\xe2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', + b'\xf1\x87LDLUEA6898374HG1fevW\x87wwwx\x88\x97\x88h\x88\x96\x88x\x88\xa7\x88ao\xf9\xff\x98\x99\x7f\xffD\xe2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', + b'\xf1\x87SBJWAA5842214GG0\x88\x87\x88xww\x87x\x89\x99\xa8\x99\x88\x99\x98\x89w\x88\x87xw_\xfa\xfffU_\xff\xd1\x8d\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', + b'\xf1\x87SBJWAA5890864GG0\xa9\x99\x89\x98\x98\x87\x98y\x89\x99\xa8\x99w\x88\x87xww\x87wvo\xfb\xffuD_\xff\x9f\xb5\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x00\x00\x00\x00', + b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', + b'\xf1\x87SBJWAA7780564GG0wvwgUUeVwwwwx\x88\x87\x88wwwwd_\xfc\xff\x86f\x7f\xff\xd7*\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0', + b'\xf1\x87SBJWAA8278284GG0ffvgUU\x85Xx\x88\x87\x88x\x88w\x88ww\x87w\x96o\xfd\xff\xa7U_\xff\xf2\xa0\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0', + b'\xf1\x87SBLWAA4363244GG0wvwgwv\x87hgw\x86ww\x88\x87xww\x87wdo\xfb\xff\x86f\x7f\xff3$\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS6\x00\x00\x00\x00', + b'\xf1\x87SBLWAA6622844GG0wwwwff\x86hwwwwx\x88\x87\x88\x88\x88\x88\x88\x98?\xfd\xff\xa9\x88\x7f\xffn\xe5\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7u\x1e{\x1c', + b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2\x00\x00\x00\x00', + b'\xf1\x87SDKXAA2443414GG1vfvgwv\x87h\x88\x88\x88\x88ww\x87wwwww\x99_\xfc\xffvD?\xffl\xd2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4G24NS6\x00\x00\x00\x00', ], }, CAR.KIA_STINGER: { - (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 \xf1\xa01.01'], - (Ecu.engine, 0x7e0, None): [ b'\xf1\x81640E0051\x00\x00\x00\x00\x00\x00\x00\x00',], - (Ecu.eps, 0x7d4, None): [b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5420 4C4VL104'], - (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 95740-J5000 170822'], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 \xf1\xa01.01', + b'\xf1\x00CK__ SCC F_CUP 1.00 1.03 96400-J5100 \xf1\xa01.03', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81606DE051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640E0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x82CKJN3TMSDE0B\x00\x00\x00\x00', + b'\xf1\x82CKKN3TMD_H0A\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5200 4C2CL104', + b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5220 4C2VL104', + b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5420 4C4VL104', + b'\xf1\x00CK MDPS R 1.00 1.06 57700-J5420 4C4VL106', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 95740-J5000 170822', + b'\xf1\x00CK MFC AT USA LHD 1.00 1.04 95740-J5000 180504', + ], (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x87VDHLG17118862DK2\x8awWwgu\x96wVfUVwv\x97xWvfvUTGTx\x87o\xff\xc9\xed\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VCJLE17622572DK0vd6D\x99\x98y\x97vwVffUfvfC%CuT&Dx\x87o\xff{\x1c\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x89E21\x00\x00\x00\x00\x00\x00\x00\xf1\x82SCK0T33NB0', + b'\xf1\x87VDHLG17034412DK2vD6DfVvVTD$D\x99w\x88\x98EDEDeT6DgfO\xff\xc3=\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VDHLG17118862DK2\x8awWwgu\x96wVfUVwv\x97xWvfvUTGTx\x87o\xff\xc9\xed\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VDKLJ18675252DK6\x89vhgwwwwveVU\x88w\x87w\x99vgf\x97vXfgw_\xff\xc2\xfb\xf1\x89E25\x00\x00\x00\x00\x00\x00\x00\xf1\x82TCK0T33NB2', + b'\xf1\x87WAJTE17552812CH4vfFffvfVeT5DwvvVVdFeegeg\x88\x88o\xff\x1a]\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00TCK2T20NB1\x19\xd2\x00\x94', ], }, CAR.KIA_OPTIMA_H: { @@ -301,16 +385,22 @@ FW_VERSIONS = { }, CAR.PALISADE: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04', b'\xf1\000LX2_ SCC F-CUP 1.00 1.05 99110-S8100 \xf1\xa01.05', 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', + b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 \xf1\xa01.05', ], (Ecu.esp, 0x7d1, None): [ 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 \x01 103\x31\t\020 58910-S8360\xf1\xa01.03', + b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330\xf1\xa01.01', b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330', + b'\xf1\x00LX ESC \x0b 103\x19\t\x10 58910-S8360', + b'\xf1\x00LX ESC \x0b 104 \x10\x16 58910-S8360\xf1\xa01.04', + b'\xf1\x00ON ESC \x0b 100\x18\x12\x18 58910-S9360\xf1\xa01.00', + b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360\xf1\xa01.01', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', @@ -319,18 +409,39 @@ FW_VERSIONS = { (Ecu.eps, 0x7d4, None): [ b'\xf1\x00LX2 MDPS C 1,00 1,03 56310-S8020 4LXDC103', # modified firmware b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103', + b'\xf1\x00ON MDPS C 1.00 1.00 56340-S9000 8B13', + b'\xf1\x00ON MDPS C 1.00 1.01 56340-S9000 9201', ], (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', + b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.07 99211-S8100 200422', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.08 99211-S8100 200903', + b'\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 181105', + b'\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 200720', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', + b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', 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\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', b'\xf1\x87LBLUFN731381KF36\xb9\x99\x89\x98\x98\x88\x88\x88\x89\x99\xa8\x99\x88\x99\xa8\x89\x88\x88\x98\x88V\177\xf6\xff\x99w\x8f\xff\xad\xd8\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\000bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', + b'\xf1\x87LDKVBN382172KF26\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\xa7\x89\x87\x88\x98x\x98\x99\xa9\x89\xa5_\xf6\xffDDO\xff\xcd\x16\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', 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\x87LDKVBN540766KF37\x87wgv\x87w\x87xx\x99\x97\x89v\x88\x97h\x88\x88\x88\x88x\x7f\xf6\xffvUo\xff\xd3\x01\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', + b'\xf1\x87LDLVBN645817KF37\x87www\x98\x87xwx\x99\x97\x89\x99\x99\x99\x99g\x88\x96x\xb6_\xf7\xff\x98fo\xff\xe2\x86\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN662115KF37\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\x97\x89x\x99\xa7\x89\x88\x99\xa8\x89\x88\x7f\xf7\xfffD_\xff\xdc\x84\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN667933KF37\xb9\x99\x89\x98\xb9\x99\x99\x99x\x88\x87\x88w\x88\x87x\x88\x88\x98\x88\xcbo\xf7\xffe3/\xffQ!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN673087KF37\x97www\x86fvgx\x99\x97\x89\x99\xaa\xa9\x9ag\x88\x86x\xe9_\xf8\xff\x98w\x7f\xff"\xad\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN681363KF37\x98\x88\x88\x88\x97x\x87\x88y\xaa\xa7\x9a\x88\x88\x98\x88\x88\x88\x88\x88vo\xf6\xffvD\x7f\xff%v\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN713890KF26\xb9\x99\x89\x98\xa9\x99\x99\x99x\x99\x97\x89\x88\x99\xa8\x89\x88\x99\xb8\x89Do\xf7\xff\xa9\x88o\xffs\r\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN750044KF37\xca\xa9\x8a\x98\xa7wwwy\xaa\xb7\x9ag\x88\x96x\x88\x99\xa8\x89\xb9\x7f\xf6\xff\xa8w\x7f\xff\xbe\xde\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN752612KF37\xba\xaa\x8a\xa8\x87w\x87xy\xaa\xa7\x9a\x88\x99\x98\x89x\x88\x97\x88\x96o\xf6\xffvU_\xffh\x1b\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN755553KF37\x87xw\x87\x97w\x87xy\x99\xa7\x99\x99\x99\xa9\x99Vw\x95gwo\xf6\xffwUO\xff\xb5T\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDMVBN778156KF37\x87vWe\xa9\x99\x99\x99y\x99\xb7\x99\x99\x99\x99\x99x\x99\x97\x89\xa8\x7f\xf8\xffwf\x7f\xff\x82_\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b'\xf1\x87LDMVBN780576KF37\x98\x87hv\x97x\x97\x89x\x99\xa7\x89\x88\x99\x98\x89w\x88\x97x\x98\x7f\xf7\xff\xba\x88\x8f\xff\x1e0\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b"\xf1\x87LBLUFN622950KF36\xa8\x88\x88\x88\x87w\x87xh\x99\x96\x89\x88\x99\x98\x89\x88\x99\x98\x89\x87o\xf6\xff\x98\x88o\xffx'\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8", ], }, CAR.VELOSTER: { @@ -367,33 +478,75 @@ FW_VERSIONS = { (Ecu.transmission, 0x7E1, None): [b'\xf1\x816U2V7051\000\000\xf1\0006U2V0_C2\000\0006U2V7051\000\000DCD0T14US1\000\000\000\000', ], (Ecu.esp, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ], }, + CAR.KIA_FORTE: { + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00BD MDPS C 1.00 1.08 56310M6300\x00 4BDDC108', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00BD LKAS AT USA LHD 1.00 1.04 95740-M6000 J33', + ], + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00BD__ SCC H-CUP 1.00 1.02 99110-M6000 \xf1\xa01.02', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x01TBDM1NU06F200H01', + ], + (Ecu.esp, 0x7d1, None): [ + b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00\xf1\xa01.04', + ], + (Ecu.transmission, 0x7e1, None): [ + b"\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\xcf\x1e'\xc3", + ], + }, CAR.KONA_EV: { (Ecu.esp, 0x7D1, None): [ b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05', + b'\xf1\x00OS IEB \x01 212 \x11\x13 58520-K4000\xf1\xa02.12', + b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000\xf1\xa02.12', + b'\xf1\x00OS IEB \x03 210 \x02\x14 58520-K4000\xf1\xa02.10', b'\xf1\x00OS IEB \x03 212 \x11\x13 58520-K4000\xf1\xa02.12', ], - (Ecu.fwdCamera, 0x7C4, None): [b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', ], - (Ecu.eps, 0x7D4, None): [b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', ], - (Ecu.fwdRadar, 0x7D0, None): [b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 \xf1\xa01.01', ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00OE2 LKAS AT EUR LHD 1.00 1.00 95740-K4200 200', + b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', + b'\xf1\x00OSE LKAS AT KOR LHD 1.00 1.00 95740-K4100 W40', + b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4300 W50', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00OS MDPS C 1.00 1.04 56310K4000\x00 4OEDC104', + b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', + ], + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00OSev SCC F-CUP 1.00 1.00 99110-K4100 \xf1\xa01.00', + b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 \xf1\xa01.01', + b'\xf1\x00OSev SCC FNCUP 1.00 1.01 99110-K4000 \xf1\xa01.01', + ], }, CAR.KIA_NIRO_EV: { (Ecu.fwdRadar, 0x7D0, None): [ - b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 \xf1\xa01.03', + b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 \xf1\xa01.00', + b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 \xf1\xa01.03', + b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 \xf1\xa01.01', + b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 \xf1\xa01.00', + b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 \xf1\xa01.00', b'\xf1\x8799110Q4500\xf1\000DEev SCC F-CUP 1.00 1.00 99110-Q4500 \xf1\xa01.00', ], (Ecu.esp, 0x7D1, None): [ - b'\xf1\xa01.06', - b'\xf1\xa01.07', + b'\xf1\x00OS IEB \r 212 \x11\x13 58520-K4000\xf1\xa02.12', + b'\xf1\x00OS IEB \r 212 \x11\x13 58520-K4000', ], (Ecu.eps, 0x7D4, None): [ b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105', b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105', + b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', ], (Ecu.fwdCamera, 0x7C4, None): [ - b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821', - b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211', b'\xf1\000DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706', + b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211', + b'\xf1\x00DEE MFC AT USA LHD 1.00 1.00 99211-Q4000 191211', + b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821', + b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', ], }, CAR.KIA_SELTOS: { @@ -416,11 +569,14 @@ FW_VERSIONS = { CAR.KIA_OPTIMA: { (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 '], (Ecu.esp, 0x7d1, None): [b'\xf1\x00JF ESC \v 11 \x18\x030 58920-D5180',], - (Ecu.engine, 0x7e0, None): [b'\x01TJFAJNU06F201H03'], + (Ecu.engine, 0x7e0, None): [ + b'\x01TJFAJNU06F201H03', + b'\xf1\x89F1JF600AISEIU702\xf1\x82F1JF600AISEIU702', + ], (Ecu.eps, 0x7d4, None): [b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409'], (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.02 95895-D5000 h31'], (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW'], - } + }, } CHECKSUM = { @@ -436,10 +592,6 @@ FEATURES = { # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS]), - - "use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, - CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA, CAR.IONIQ_EV_2020, - CAR.SANTA_FE, CAR.KIA_NIRO_EV, CAR.KIA_SELTOS]), } EV_HYBRID = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV]) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4d2de08e8..f049434bf 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -13,6 +13,9 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName + +# WARNING: this value was determined based on the model's training distribution, +# model predictions above this speed can be unpredictable MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 135 + 4 = 86 mph # generic car and radar interfaces @@ -119,7 +122,10 @@ class CarInterfaceBase(): if cs_out.steerError: events.add(EventName.steerUnavailable) elif cs_out.steerWarning: - events.add(EventName.steerTempUnavailable) + if cs_out.steeringPressed: + events.add(EventName.steerTempUnavailableUserOverride) + else: + events.add(EventName.steerTempUnavailable) # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. # Optionally allow to press gas at zero speed to resume. diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 36a0495fd..5d9e9c9bb 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -23,9 +23,9 @@ class CarController(): CS.out.steeringTorque, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer - if CS.out.standstill and frame % 20 == 0: + if CS.out.standstill and frame % 5 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds - # Send Resume button at 5hz if we're engaged at standstill to support full stop and go! + # Send Resume button at 20hz if we're engaged at standstill to support full stop and go! # TODO: improve the resume trigger logic by looking at actual radar data can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.RESUME)) else: diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index dabbd2129..456f7f672 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -13,8 +13,10 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): lnv = 0 er2 = int(lkas["ERR_BIT_2"]) - steering_angle = int(lkas["STEERING_ANGLE"]) - b2 = int(lkas["ANGLE_ENABLED"]) + # Some older models do have these, newer models don't. + # Either way, they all work just fine if set to zero. + steering_angle = 0 + b2 = 0 tmp = steering_angle + 2048 ahi = tmp >> 10 diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 3060cef5f..60b9a8b58 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -61,7 +61,7 @@ FINGERPRINTS = { CAR.Mazda3: [ # Mazda 3 2017 { - 19: 5, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 628: 8, 832: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1070: 8, 1078: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1169: 8, 1170: 8, 1173: 8, 1177: 8, 1180: 8, 1182: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1275: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1456: 8, 1479: 8, 2015: 8, 2024: 8, 2025: 8 + 19: 5, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 138: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 522: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 628: 8, 832: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1070: 8, 1078: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1169: 8, 1170: 8, 1173: 8, 1177: 8, 1180: 8, 1182: 8, 1183: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8,1240: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1246: 8, 1247: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1275: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1456: 8, 1479: 8, 2015: 8, 2024: 8, 2025: 8 }, # Mazda 6 2017 GT diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 67ee422bc..45a4e6935 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -146,10 +146,13 @@ class CarState(CarStateBase): checks = [ # sig_address, frequency + ("STEER_ANGLE_SENSOR", 100), ("WHEEL_SPEEDS_REAR", 50), ("WHEEL_SPEEDS_FRONT", 50), - ("STEER_ANGLE_SENSOR", 100), + ("ESP", 25), + ("GEARBOX", 25), ("DOORS_LIGHTS", 10), + ("LIGHTS", 10), ] if CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: @@ -201,6 +204,9 @@ class CarState(CarStateBase): checks += [ ("BRAKE_PEDAL", 100), ("CRUISE_THROTTLE", 50), + ("CANCEL_MSG", 50), + ("HUD_SETTINGS", 25), + ("SEATBELT", 10), ] if CP.carFingerprint == CAR.ALTIMA: @@ -241,6 +247,7 @@ class CarState(CarStateBase): ("CRUISE_ON", "PRO_PILOT", 0), ] checks = [ + ("LKAS", 100), ("PRO_PILOT", 100), ] else: @@ -327,7 +334,11 @@ class CarState(CarStateBase): ] checks = [ + ("PROPILOT_HUD_INFO_MSG", 2), + ("LKAS_SETTINGS", 10), ("CRUISE_STATE", 50), + ("PROPILOT_HUD", 50), + ("LKAS", 100), ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) @@ -341,7 +352,9 @@ class CarState(CarStateBase): signals += [ ("CRUISE_ON", "PRO_PILOT", 0), ] - + checks += [ + ("PRO_PILOT", 100), + ] elif CP.carFingerprint == CAR.ALTIMA: signals += [ ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 39ce707d5..1bd8d8d80 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -39,9 +39,6 @@ FINGERPRINTS = { }, ], CAR.LEAF_IC: [ - { - 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, 643: 5, 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, 943: 8, 944: 1, 976: 6, 1001: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6 - }, { 2: 5, 42: 6, 264: 3, 282: 8, 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, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 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 }, diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 1ad6bc75f..1eb7699f7 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -37,8 +37,9 @@ class CarState(CarStateBase): 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) + if self.CP.enableBsm: + 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) can_gear = int(cp.vl["Transmission"]['Gear']) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) @@ -102,10 +103,6 @@ class CarState(CarStateBase): ("DOOR_OPEN_RL", "BodyInfo", 1), ("Units", "Dash_State", 1), ("Gear", "Transmission", 0), - ("L_ADJACENT", "BSD_RCTA", 0), - ("R_ADJACENT", "BSD_RCTA", 0), - ("L_APPROACHING", "BSD_RCTA", 0), - ("R_APPROACHING", "BSD_RCTA", 0), ] checks = [ @@ -116,8 +113,21 @@ class CarState(CarStateBase): ("Wheel_Speeds", 50), ("Transmission", 100), ("Steering_Torque", 50), + ("Dash_State", 1), + ("BodyInfo", 1), ] + if CP.enableBsm: + signals += [ + ("L_ADJACENT", "BSD_RCTA", 0), + ("R_ADJACENT", "BSD_RCTA", 0), + ("L_APPROACHING", "BSD_RCTA", 0), + ("R_APPROACHING", "BSD_RCTA", 0), + ] + checks += [ + ("BSD_RCTA", 17), + ] + if CP.carFingerprint in PREGLOBAL_CARS: signals += [ ("LKA_Lockout", "Steering_Torque", 0), diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 0cfb6fb49..bc5502f48 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -27,6 +27,7 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = candidate in PREGLOBAL_CARS ret.enableCamera = True + ret.enableBsm = 0x228 in fingerprint[0] ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 60d5f5ed7..7f603cb42 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -1,70 +1,69 @@ #!/usr/bin/env python3 import unittest import importlib +from parameterized import parameterized + +from cereal import car from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.car_helpers import interfaces from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS -from cereal import car - - class TestCarInterfaces(unittest.TestCase): - def test_car_interfaces(self): - all_cars = all_known_cars() - for car_name in all_cars: - print(car_name) - fingerprint = FINGERPRINTS[car_name][0] + @parameterized.expand([(car,) for car in all_known_cars()]) + def test_car_interfaces(self, car_name): + print(car_name) + fingerprint = FINGERPRINTS[car_name][0] - CarInterface, CarController, CarState = interfaces[car_name] - fingerprints = { - 0: fingerprint, - 1: fingerprint, - 2: fingerprint, - } + CarInterface, CarController, CarState = interfaces[car_name] + fingerprints = { + 0: fingerprint, + 1: fingerprint, + 2: fingerprint, + } - car_fw = [] + car_fw = [] - car_params = CarInterface.get_params(car_name, fingerprints, car_fw) - car_interface = CarInterface(car_params, CarController, CarState) - assert car_params - assert car_interface + car_params = CarInterface.get_params(car_name, fingerprints, car_fw) + car_interface = CarInterface(car_params, CarController, CarState) + assert car_params + assert car_interface - self.assertGreater(car_params.mass, 1) - self.assertGreater(car_params.steerRateCost, 1e-3) + self.assertGreater(car_params.mass, 1) + self.assertGreater(car_params.steerRateCost, 1e-3) - if car_params.steerControlType != car.CarParams.SteerControlType.angle: - tuning = car_params.lateralTuning.which() - if tuning == 'pid': - self.assertTrue(len(car_params.lateralTuning.pid.kpV)) - elif tuning == 'lqr': - self.assertTrue(len(car_params.lateralTuning.lqr.a)) - elif tuning == 'indi': - self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) + if car_params.steerControlType != car.CarParams.SteerControlType.angle: + tuning = car_params.lateralTuning.which() + if tuning == 'pid': + self.assertTrue(len(car_params.lateralTuning.pid.kpV)) + elif tuning == 'lqr': + self.assertTrue(len(car_params.lateralTuning.lqr.a)) + elif tuning == 'indi': + self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) - # Run car interface - CC = car.CarControl.new_message() - for _ in range(10): - car_interface.update(CC, []) - car_interface.apply(CC) - car_interface.apply(CC) + # Run car interface + CC = car.CarControl.new_message() + for _ in range(10): + car_interface.update(CC, []) + car_interface.apply(CC) + car_interface.apply(CC) - CC = car.CarControl.new_message() - CC.enabled = True - for _ in range(10): - car_interface.update(CC, []) - car_interface.apply(CC) - car_interface.apply(CC) + CC = car.CarControl.new_message() + CC.enabled = True + for _ in range(10): + car_interface.update(CC, []) + car_interface.apply(CC) + car_interface.apply(CC) - # Test radar interface - RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % car_params.carName).RadarInterface - radar_interface = RadarInterface(car_params) - assert radar_interface + # Test radar interface + RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % car_params.carName).RadarInterface + radar_interface = RadarInterface(car_params) + assert radar_interface - # Run radar interface once - radar_interface.update([]) - if not car_params.radarOffCan and hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): - radar_interface._update([radar_interface.trigger_msg]) + # Run radar interface once + radar_interface.update([]) + if not car_params.radarOffCan and hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): + radar_interface._update([radar_interface.trigger_msg]) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 7152aa73a..c961288aa 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -3,8 +3,8 @@ from common.numpy_fast import clip from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ - create_fcw_command -from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, CarControllerParams + create_fcw_command, create_lta_steer_command +from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -99,6 +99,8 @@ class CarController(): # on consecutive messages if Ecu.fwdCamera in self.fake_ecus: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) + if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: + can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2)) # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # if frame % 2 == 0: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 42bce1c48..ca2a2bcf8 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR +from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR class CarState(CarStateBase): @@ -90,17 +90,14 @@ class CarState(CarStateBase): ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] in [1, 2, 3, 4, 5, 6] - if self.CP.carFingerprint == CAR.PRIUS: - ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 - else: - ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) + ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0 # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] - if self.CP.carFingerprint in TSS2_CAR: + if self.CP.enableBsm: ret.leftBlindspot = (cp.vl["BSM"]['L_ADJACENT'] == 1) or (cp.vl["BSM"]['L_APPROACHING'] == 1) ret.rightBlindspot = (cp.vl["BSM"]['R_ADJACENT'] == 1) or (cp.vl["BSM"]['R_APPROACHING'] == 1) @@ -140,13 +137,18 @@ class CarState(CarStateBase): ] checks = [ + ("GEAR_PACKET", 1), + ("LIGHT_STALK", 1), + ("STEERING_LEVERS", 0.15), + ("SEATS_DOORS", 3), + ("ESP_CONTROL", 3), + ("EPS_STATUS", 25), ("BRAKE_MODULE", 40), ("GAS_PEDAL", 33), ("WHEEL_SPEEDS", 80), ("STEER_ANGLE_SENSOR", 80), ("PCM_CRUISE", 33), ("STEER_TORQUE_SENSOR", 50), - ("EPS_STATUS", 25), ] if CP.carFingerprint == CAR.LEXUS_IS: @@ -159,20 +161,22 @@ class CarState(CarStateBase): signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) checks.append(("PCM_CRUISE_2", 33)) - if CP.carFingerprint == CAR.PRIUS: - signals += [("STATE", "AUTOPARK_STATUS", 0)] - # add gas interceptor reading if we are using it if CP.enableGasInterceptor: signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) checks.append(("GAS_SENSOR", 50)) - if CP.carFingerprint in TSS2_CAR: - signals += [("L_ADJACENT", "BSM", 0)] - signals += [("L_APPROACHING", "BSM", 0)] - signals += [("R_ADJACENT", "BSM", 0)] - signals += [("R_APPROACHING", "BSM", 0)] + if CP.enableBsm: + signals += [ + ("L_ADJACENT", "BSM", 0), + ("L_APPROACHING", "BSM", 0), + ("R_ADJACENT", "BSM", 0), + ("R_APPROACHING", "BSM", 0), + ] + checks += [ + ("BSM", 1) + ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @@ -186,7 +190,8 @@ class CarState(CarStateBase): # use steering message to check if panda is connected to frc checks = [ - ("STEERING_LKA", 42) + ("STEERING_LKA", 42), + ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 082284a44..6a7201def 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, NO_DSU_CAR, FINGERPRINTS, CarControllerParams -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, CarControllerParams +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase @@ -25,7 +25,7 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 0.4 # Improved longitudinal tune - if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2]: + if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: ret.longitudinalTuning.deadzoneBP = [0., 8.05] ret.longitudinalTuning.deadzoneV = [.0, .14] ret.longitudinalTuning.kpBP = [0., 5., 20.] @@ -278,7 +278,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00007 - elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX]: + elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.66 @@ -298,6 +298,16 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.15]] ret.lateralTuning.pid.kf = 0.00007818594 + elif candidate == CAR.MIRAI: + stop_and_go = True + ret.safetyParam = 73 + ret.wheelbase = 2.91 + ret.steerRatio = 14.8 + tire_stiffness_factor = 0.8 + ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] + ret.lateralTuning.pid.kf = 0.00006 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 @@ -311,15 +321,15 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor=tire_stiffness_factor) ret.enableCamera = True + ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it smartDsu = 0x2FF in fingerprint[0] - # TODO: use FW query for the enableDsu flag # In TSS2 cars the camera does long control - ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in NO_DSU_CAR + found_ecus = [fw.ecu for fw in car_fw] + ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) ret.enableGasInterceptor = 0x201 in fingerprint[0] # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR) - cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 9c857f965..2e3d767ff 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -21,10 +21,10 @@ class CAR: RAV4H = "TOYOTA RAV4 HYBRID 2017" RAV4 = "TOYOTA RAV4 2017" COROLLA = "TOYOTA COROLLA 2017" - LEXUS_RX = "LEXUS RX 350 2016" + LEXUS_RX = "LEXUS RX 2016" LEXUS_RXH = "LEXUS RX HYBRID 2017" - LEXUS_RX_TSS2 = "LEXUS RX350 2020" - LEXUS_RXH_TSS2 = "LEXUS RX450 HYBRID 2020" + LEXUS_RX_TSS2 = "LEXUS RX 2020" + LEXUS_RXH_TSS2 = "LEXUS RX HYBRID 2020" CHR = "TOYOTA C-HR 2018" CHRH = "TOYOTA C-HR HYBRID 2018" CAMRY = "TOYOTA CAMRY 2018" @@ -40,14 +40,16 @@ class CAR: COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019" LEXUS_ES_TSS2 = "LEXUS ES 2019" - LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019" - LEXUS_ESH = "LEXUS ES 300H 2018" - SIENNA = "TOYOTA SIENNA XLE 2018" - LEXUS_IS = "LEXUS IS300 2018" - LEXUS_CTH = "LEXUS CT 200H 2018" + LEXUS_ESH_TSS2 = "LEXUS ES HYBRID 2019" + LEXUS_ESH = "LEXUS ES HYBRID 2018" + SIENNA = "TOYOTA SIENNA 2018" + LEXUS_IS = "LEXUS IS 2018" + LEXUS_CTH = "LEXUS CT HYBRID 2018" RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019" - LEXUS_NXH = "LEXUS NX300H 2018" - LEXUS_NX = "LEXUS NX300 2018" + LEXUS_NXH = "LEXUS NX HYBRID 2018" + LEXUS_NX = "LEXUS NX 2018" + LEXUS_NX_TSS2 = "LEXUS NX 2020" + MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5 # addr: (ecu, cars, bus, 1/freq*100, vl) STATIC_MSGS = [ @@ -71,11 +73,6 @@ STATIC_MSGS = [ (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), ] -ECU_FINGERPRINT = { - Ecu.fwdCamera: [0x2e4], # steer torque cmd - Ecu.dsu: [0x283], # accel cmd -} - FINGERPRINTS = { CAR.RAV4: [{ @@ -287,28 +284,38 @@ FINGERPRINTS = { CAR.LEXUS_NX: [{ 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 658: 8, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.LEXUS_NX_TSS2: [{ + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 3, 818: 8, 822: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 877: 8, 889: 8, 891: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 987: 8, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1172: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1208: 8, 1212: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1585: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1775: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], CAR.PRIUS_TSS2: [{ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 800: 8, 810: 2, 814: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1593: 8, 1595: 8, 1649: 8, 1653: 8, 1654: 8, 1655: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], + CAR.MIRAI: [{ + 15: 8, 36: 8, 37: 8, 164: 8, 166: 8, 170: 8, 180: 8, 203: 8, 295: 8, 401: 8, 426: 6, 466: 8, 467: 8, 494: 8, 495: 8, 550: 8, 552: 4, 560: 7, 562: 8, 581: 5, 608: 8, 610: 8, 643: 7, 664: 8, 665: 8, 666: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 789: 8, 791: 8, 800: 8, 810: 2, 812: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 870: 7, 871: 2, 877: 8, 881: 8, 889: 8, 891: 8, 892: 8, 893: 8, 894: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 955: 8, 956: 8, 971: 7, 983: 8, 984: 8, 987: 8, 998: 5, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1081: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1593: 8, 1595: 8, 1649: 8, 1653: 8, 1654: 8, 1655: 8, 1677: 8, 1745: 8, 1769: 8, 1770: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1872: 8, 1880: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2008: 8, 2009: 8, 2015: 8, 2016: 8, 2017: 8 }] } # Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, - CAR.LEXUS_NX, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.LEXUS_ESH] + CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.LEXUS_ESH, CAR.MIRAI] FW_VERSIONS = { CAR.AVALON: { (Ecu.esp, 0x7b0, None): [ + b'F152607060\x00\x00\x00\x00\x00\x00', b'F152607110\x00\x00\x00\x00\x00\x00', + b'F152607140\x00\x00\x00\x00\x00\x00', b'F152607171\x00\x00\x00\x00\x00\x00', b'F152607180\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'881510701300\x00\x00\x00\x00', b'881510703200\x00\x00\x00\x00', + b'881510705100\x00\x00\x00\x00', b'881510705200\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ + b'8965B41051\x00\x00\x00\x00\x00\x00', b'8965B41080\x00\x00\x00\x00\x00\x00', b'8965B41090\x00\x00\x00\x00\x00\x00', ], @@ -370,6 +377,7 @@ FW_VERSIONS = { b'F152606230\x00\x00\x00\x00\x00\x00', b'F152606270\x00\x00\x00\x00\x00\x00', b'F152606290\x00\x00\x00\x00\x00\x00', + b'F152606410\x00\x00\x00\x00\x00\x00', b'F152633540\x00\x00\x00\x00\x00\x00', b'F152633A10\x00\x00\x00\x00\x00\x00', b'F152633A20\x00\x00\x00\x00\x00\x00', @@ -395,8 +403,8 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F0601200 ', b'8646F0601300 ', + b'8646F0601400 ', b'8646F0603400 ', - b'8821F0604100 ', b'8646F0605000 ', b'8646F0606000 ', b'8646F0606100 ', @@ -405,8 +413,11 @@ FW_VERSIONS = { }, CAR.CAMRYH: { (Ecu.engine, 0x700, None): [ + b'\x018966333N1100\x00\x00\x00\x00', b'\x018966333N4300\x00\x00\x00\x00', b'\x018966333X0000\x00\x00\x00\x00', + b'\x018966333X4000\x00\x00\x00\x00', + b'\x01896633T16000\x00\x00\x00\x00', b'\x028966306B2100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306B2300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306N8100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', @@ -433,10 +444,13 @@ FW_VERSIONS = { b'8821F0601200 ', b'8821F0601300 ', b'8821F0603400 ', + b'8821F0604000 ', b'8821F0604200 ', + b'8821F0605200 ', b'8821F0606200 ', b'8821F0607200 ', b'8821F0608000 ', + b'8821F0608200 ', b'8821F0609000 ', b'8821F0609100 ', ], @@ -454,10 +468,13 @@ FW_VERSIONS = { b'8821F0601200 ', b'8821F0601300 ', b'8821F0603400 ', + b'8821F0604000 ', b'8821F0604200 ', + b'8821F0605200 ', b'8821F0606200 ', b'8821F0607200 ', b'8821F0608000 ', + b'8821F0608200 ', b'8821F0609000 ', b'8821F0609100 ', ], @@ -465,6 +482,7 @@ FW_VERSIONS = { b'8646F0601200 ', b'8646F0601300 ', b'8646F0601400 ', + b'8646F0603400 ', b'8646F0603500 ', b'8646F0604100 ', b'8646F0605000 ', @@ -504,12 +522,14 @@ FW_VERSIONS = { ], (Ecu.engine, 0x700, None): [ b'\x018966306Q6000\x00\x00\x00\x00', + b'\x018966306Q7000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 15): [ b'\x018821F6201200\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 109): [ b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', ], }, CAR.CHR: { @@ -570,48 +590,57 @@ FW_VERSIONS = { }, CAR.CHRH: { (Ecu.engine, 0x700, None): [ - b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', - b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x0289663F405000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F418000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x0189663F438000\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ + b'F152610013\x00\x00\x00\x00\x00\x00', b'F152610040\x00\x00\x00\x00\x00\x00', b'F152610190\x00\x00\x00\x00\x00\x00', - b'F152610013\x00\x00\x00\x00\x00\x00', + b'F152610200\x00\x00\x00\x00\x00\x00', + b'F152610230\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ + b'8821FF402300 ', + b'8821FF402400 ', b'8821FF404000 ', b'8821FF406000 ', b'8821FF407100 ', ], (Ecu.eps, 0x7a1, None): [ b'8965B10011\x00\x00\x00\x00\x00\x00', + b'8965B10020\x00\x00\x00\x00\x00\x00', b'8965B10040\x00\x00\x00\x00\x00\x00', b'8965B10050\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821FF402300 ', + b'8821FF402400 ', b'8821FF404000 ', b'8821FF406000 ', b'8821FF407100 ', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646FF402100 ', b'8646FF404000 ', - b'8821FF406000 ', + b'8646FF406000 ', b'8646FF407000 ', ], }, CAR.COROLLA: { (Ecu.engine, 0x7e0, None): [ - b'\x01896630E88000\x00\x00\x00\x00', - b'\x0330ZC1200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230ZC2100\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230ZC2200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230ZC2300\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230ZC3000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC3100\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230ZC3200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230ZC3300\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0330ZC1200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'881510201100\x00\x00\x00\x00', @@ -633,11 +662,11 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F0201101\x00\x00\x00\x00', b'8646F0201200\x00\x00\x00\x00', - b'8646F0E01300\x00\x00\x00\x00', ], }, CAR.COROLLA_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x01896630ZG2000\x00\x00\x00\x00', b'\x01896630ZG5000\x00\x00\x00\x00', b'\x01896630ZG5100\x00\x00\x00\x00', b'\x01896630ZG5200\x00\x00\x00\x00', @@ -645,35 +674,39 @@ FW_VERSIONS = { b'\x01896630ZP2000\x00\x00\x00\x00', b'\x01896630ZQ5000\x00\x00\x00\x00', b'\x018966312L8000\x00\x00\x00\x00', + b'\x018966312M9000\x00\x00\x00\x00', b'\x018966312P9000\x00\x00\x00\x00', b'\x018966312P9100\x00\x00\x00\x00', b'\x018966312P9200\x00\x00\x00\x00', + b'\x018966312Q2300\x00\x00\x00\x00', b'\x018966312R0100\x00\x00\x00\x00', b'\x018966312R1000\x00\x00\x00\x00', b'\x018966312R1100\x00\x00\x00\x00', b'\x018966312R3100\x00\x00\x00\x00', + b'\x018966312S5000\x00\x00\x00\x00', b'\x018966312S7000\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', - b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ - b'\x018965B1255000\x00\x00\x00\x00', - b'8965B12361\x00\x00\x00\x00\x00\x00', b'\x018965B12350\x00\x00\x00\x00\x00\x00', b'\x018965B12470\x00\x00\x00\x00\x00\x00', b'\x018965B12490\x00\x00\x00\x00\x00\x00', b'\x018965B12500\x00\x00\x00\x00\x00\x00', b'\x018965B12520\x00\x00\x00\x00\x00\x00', b'\x018965B12530\x00\x00\x00\x00\x00\x00', + b'\x018965B1255000\x00\x00\x00\x00', + b'8965B12361\x00\x00\x00\x00\x00\x00', + b'8965B58040\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ - b'F152602191\x00\x00\x00\x00\x00\x00', b'\x01F152602280\x00\x00\x00\x00\x00\x00', b'\x01F152602560\x00\x00\x00\x00\x00\x00', b'\x01F152602590\x00\x00\x00\x00\x00\x00', @@ -685,6 +718,9 @@ FW_VERSIONS = { b'\x01F152612B60\x00\x00\x00\x00\x00\x00', b'\x01F152612B61\x00\x00\x00\x00\x00\x00', b'\x01F152612B90\x00\x00\x00\x00\x00\x00', + b'\x01F152612C00\x00\x00\x00\x00\x00\x00', + b'F152602191\x00\x00\x00\x00\x00\x00', + b'F152658320\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -701,13 +737,16 @@ FW_VERSIONS = { b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, CAR.COROLLAH_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896630ZJ1000\x00\x00\x00\x00', b'\x01896630ZU8000\x00\x00\x00\x00', - b'\x018966342M5000\x00\x00\x00\x00', + b'\x01896637624000\x00\x00\x00\x00', + b'\x01896637626000\x00\x00\x00\x00', + b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -718,8 +757,10 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B12361\x00\x00\x00\x00\x00\x00', b'8965B12451\x00\x00\x00\x00\x00\x00', + b'8965B76012\x00\x00\x00\x00\x00\x00', b'\x018965B12350\x00\x00\x00\x00\x00\x00', b'\x018965B12470\x00\x00\x00\x00\x00\x00', + b'\x018965B12490\x00\x00\x00\x00\x00\x00', b'\x018965B12500\x00\x00\x00\x00\x00\x00', b'\x018965B12520\x00\x00\x00\x00\x00\x00', b'\x018965B12530\x00\x00\x00\x00\x00\x00', @@ -731,10 +772,13 @@ FW_VERSIONS = { b'F152612700\x00\x00\x00\x00\x00\x00', b'F152612790\x00\x00\x00\x00\x00\x00', b'F152612800\x00\x00\x00\x00\x00\x00', + b'F152612820\x00\x00\x00\x00\x00\x00', b'F152612840\x00\x00\x00\x00\x00\x00', + b'F152612A00\x00\x00\x00\x00\x00\x00', b'F152612A10\x00\x00\x00\x00\x00\x00', b'F152642540\x00\x00\x00\x00\x00\x00', - b'F152612A00\x00\x00\x00\x00\x00\x00', + b'F152676293\x00\x00\x00\x00\x00\x00', + b'F152676303\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -743,28 +787,35 @@ FW_VERSIONS = { b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F7603100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', ], }, CAR.HIGHLANDER: { (Ecu.engine, 0x700, None): [ b'\x01896630E09000\x00\x00\x00\x00', + b'\x01896630E43000\x00\x00\x00\x00', b'\x01896630E43100\x00\x00\x00\x00', b'\x01896630E43200\x00\x00\x00\x00', b'\x01896630E44200\x00\x00\x00\x00', b'\x01896630E45000\x00\x00\x00\x00', b'\x01896630E45100\x00\x00\x00\x00', b'\x01896630E45200\x00\x00\x00\x00', + b'\x01896630E46200\x00\x00\x00\x00', b'\x01896630E74000\x00\x00\x00\x00', + b'\x01896630E75000\x00\x00\x00\x00', b'\x01896630E76000\x00\x00\x00\x00', + b'\x01896630E77000\x00\x00\x00\x00', b'\x01896630E83000\x00\x00\x00\x00', b'\x01896630E84000\x00\x00\x00\x00', b'\x01896630E85000\x00\x00\x00\x00', + b'\x01896630E86000\x00\x00\x00\x00', b'\x01896630E88000\x00\x00\x00\x00', b'\x01896630EA0000\x00\x00\x00\x00', ], @@ -797,6 +848,7 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'\x0230E40000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230E40100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230EA2000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230EA2100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -819,13 +871,15 @@ FW_VERSIONS = { b'\x01F15260E110\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ + b'\x01896630E62100\x00\x00\x00\x00', b'\x01896630E62200\x00\x00\x00\x00', b'\x01896630E64100\x00\x00\x00\x00', b'\x01896630E64200\x00\x00\x00\x00', + b'\x01896630EB1000\x00\x00\x00\x00', + b'\x01896630EB1100\x00\x00\x00\x00', b'\x01896630EB2000\x00\x00\x00\x00', b'\x01896630EB2100\x00\x00\x00\x00', - b'\x01896630EB1100\x00\x00\x00\x00', - b'\x01896630EB1000\x00\x00\x00\x00', + b'\x01896630EB2200\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', @@ -863,29 +917,35 @@ FW_VERSIONS = { CAR.LEXUS_IS: { (Ecu.engine, 0x700, None): [ b'\x018966353M7100\x00\x00\x00\x00', + b'\x018966353Q2000\x00\x00\x00\x00', b'\x018966353Q2300\x00\x00\x00\x00', b'\x018966353R1100\x00\x00\x00\x00', b'\x018966353R7100\x00\x00\x00\x00', b'\x018966353R8100\x00\x00\x00\x00', + b'\x018966353Q4000\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ + b'\x0232480000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02353P9000\x00\x00\x00\x00\x00\x00\x00\x00553C1000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ - b'F152653330\x00\x00\x00\x00\x00\x00', b'F152653301\x00\x00\x00\x00\x00\x00', + b'F152653310\x00\x00\x00\x00\x00\x00', + b'F152653330\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ + b'881515306200\x00\x00\x00\x00', b'881515306400\x00\x00\x00\x00', b'881515306500\x00\x00\x00\x00', - b'881515306200\x00\x00\x00\x00', + b'881515307400\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B53270\x00\x00\x00\x00\x00\x00', b'8965B53271\x00\x00\x00\x00\x00\x00', b'8965B53280\x00\x00\x00\x00\x00\x00', b'8965B53281\x00\x00\x00\x00\x00\x00', + b'8965B53311\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702300\x00\x00\x00\x00', @@ -902,23 +962,30 @@ FW_VERSIONS = { b'\x02896634761000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634761100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634761200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634762000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634763000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634763100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634765000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634765100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634769100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634769200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634770000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634774000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634774100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634774200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634782000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634784000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347A0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966347A5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966347A8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', - b'\x02896634765100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347B0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x03896634759100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', b'\x03896634759300\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701002\x00\x00\x00\x00', b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', + b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', b'\x03896634760100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', b'\x03896634760200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', b'\x03896634760200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', @@ -927,9 +994,11 @@ FW_VERSIONS = { b'\x03896634768000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', b'\x03896634768100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', b'\x03896634785000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4705001\x00\x00\x00\x00', + b'\x03896634785000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', b'\x03896634786000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4705001\x00\x00\x00\x00', b'\x03896634786000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', b'\x03896634789000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', + b'\x038966347A3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', b'\x038966347A3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707001\x00\x00\x00\x00', b'\x038966347B6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', b'\x038966347B7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', @@ -951,6 +1020,7 @@ FW_VERSIONS = { b'F152647417\x00\x00\x00\x00\x00\x00', b'F152647470\x00\x00\x00\x00\x00\x00', b'F152647490\x00\x00\x00\x00\x00\x00', + b'F152647683\x00\x00\x00\x00\x00\x00', b'F152647684\x00\x00\x00\x00\x00\x00', b'F152647862\x00\x00\x00\x00\x00\x00', b'F152647863\x00\x00\x00\x00\x00\x00', @@ -970,7 +1040,6 @@ FW_VERSIONS = { b'8821F4702300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ - b'8646F4201200\x00\x00\x00\x00', b'8646F4701300\x00\x00\x00\x00', b'8646F4702001\x00\x00\x00\x00', b'8646F4702100\x00\x00\x00\x00', @@ -991,9 +1060,10 @@ FW_VERSIONS = { b'\x02342Q4000\x00\x00\x00\x00\x00\x00\x00\x0054215000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ + b'8965B42063\x00\x00\x00\x00\x00\x00', + b'8965B42073\x00\x00\x00\x00\x00\x00', b'8965B42082\x00\x00\x00\x00\x00\x00', b'8965B42083\x00\x00\x00\x00\x00\x00', - b'8965B42063\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F15260R102\x00\x00\x00\x00\x00\x00', @@ -1015,6 +1085,7 @@ FW_VERSIONS = { b'8646F4201200\x00\x00\x00\x00', b'8646F4202001\x00\x00\x00\x00', b'8646F4202100\x00\x00\x00\x00', + b'8646F4204000\x00\x00\x00\x00', ], }, CAR.RAV4H: { @@ -1022,15 +1093,17 @@ FW_VERSIONS = { b'\x02342N9000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02342N9100\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02342P0000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', - b'\x02342Q2000\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ + b'8965B42102\x00\x00\x00\x00\x00\x00', b'8965B42103\x00\x00\x00\x00\x00\x00', + b'8965B42112\x00\x00\x00\x00\x00\x00', b'8965B42162\x00\x00\x00\x00\x00\x00', b'8965B42163\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152642090\x00\x00\x00\x00\x00\x00', + b'F152642110\x00\x00\x00\x00\x00\x00', b'F152642120\x00\x00\x00\x00\x00\x00', b'F152642400\x00\x00\x00\x00\x00\x00', ], @@ -1055,28 +1128,35 @@ FW_VERSIONS = { CAR.RAV4_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896630R58000\x00\x00\x00\x00', + b'\x01896630R58100\x00\x00\x00\x00', b'\x018966342E2000\x00\x00\x00\x00', b'\x018966342M8000\x00\x00\x00\x00', + b'\x018966342S9000\x00\x00\x00\x00', b'\x018966342T1000\x00\x00\x00\x00', 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'\x018966342V3000\x00\x00\x00\x00', b'\x018966342V3100\x00\x00\x00\x00', b'\x018966342V3200\x00\x00\x00\x00', - b'\x018966342X5000\x00\x00\x00\x00', b'\x01896634A05000\x00\x00\x00\x00', b'\x01896634A19000\x00\x00\x00\x00', b'\x01896634A19100\x00\x00\x00\x00', b'\x01896634A20000\x00\x00\x00\x00', + b'\x01896634A20100\x00\x00\x00\x00', b'\x01896634A22000\x00\x00\x00\x00', + b'\x01896634A22100\x00\x00\x00\x00', + b'\x01896634A30000\x00\x00\x00\x00', b'\x01896634A44000\x00\x00\x00\x00', b'\x01896634A45000\x00\x00\x00\x00', b'\x01896634A46000\x00\x00\x00\x00', + b'\x028966342M7000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00', b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x02896634A18100\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', ], @@ -1088,9 +1168,11 @@ FW_VERSIONS = { b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x01F152642561\x00\x00\x00\x00\x00\x00', b'\x01F152642700\x00\x00\x00\x00\x00\x00', + b'\x01F152642701\x00\x00\x00\x00\x00\x00', b'\x01F152642710\x00\x00\x00\x00\x00\x00', b'\x01F152642711\x00\x00\x00\x00\x00\x00', b'\x01F152642750\x00\x00\x00\x00\x00\x00', + b'\x01F152642751\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B42170\x00\x00\x00\x00\x00\x00', @@ -1172,23 +1254,37 @@ FW_VERSIONS = { }, CAR.LEXUS_ES_TSS2: { (Ecu.engine, 0x700, None): [ - b'\x018966333T5100\x00\x00\x00\x00', + b'\x01896630EC9100\x00\x00\x00\x00', b'\x018966333T5000\x00\x00\x00\x00', + b'\x018966333T5100\x00\x00\x00\x00', + b'\x018966333X6000\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'\x01F152606281\x00\x00\x00\x00\x00\x00', + b'\x01F152606340\x00\x00\x00\x00\x00\x00', + b'\x01F15260E031\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33252\x00\x00\x00\x00\x00\x00', + b'8965B33590\x00\x00\x00\x00\x00\x00', + b'8965B48271\x00\x00\x00\x00\x00\x00', ], - (Ecu.esp, 0x7b0, None): [b'\x01F152606281\x00\x00\x00\x00\x00\x00'], - (Ecu.eps, 0x7a1, None): [b'8965B33252\x00\x00\x00\x00\x00\x00'], (Ecu.fwdRadar, 0x750, 0xf): [ - b'\x018821F3301200\x00\x00\x00\x00', b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, CAR.SIENNA: { (Ecu.engine, 0x700, None): [ b'\x01896630832100\x00\x00\x00\x00', + b'\x01896630832200\x00\x00\x00\x00', b'\x01896630838000\x00\x00\x00\x00', b'\x01896630838100\x00\x00\x00\x00', b'\x01896630842000\x00\x00\x00\x00', @@ -1201,6 +1297,7 @@ FW_VERSIONS = { ], (Ecu.eps, 0x7a1, None): [ b'8965B45070\x00\x00\x00\x00\x00\x00', + b'8965B45080\x00\x00\x00\x00\x00\x00', b'8965B45082\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ @@ -1218,27 +1315,51 @@ FW_VERSIONS = { b'8646F0801100\x00\x00\x00\x00', ], }, + CAR.LEXUS_CTH: { + (Ecu.dsu, 0x791, None): [ + b'881517601100\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152676144\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0237635000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F7601100\x00\x00\x00\x00', + ], + }, CAR.LEXUS_ESH_TSS2: { (Ecu.engine, 0x700, None): [ b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152633423\x00\x00\x00\x00\x00\x00', b'F152633680\x00\x00\x00\x00\x00\x00', + b'F152633681\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B33252\x00\x00\x00\x00\x00\x00', b'8965B33590\x00\x00\x00\x00\x00\x00', + b'8965B33690\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', b'\x018821F3301300\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, CAR.LEXUS_ESH: { @@ -1248,13 +1369,18 @@ FW_VERSIONS = { (Ecu.esp, 0x7b0, None): [ b'F152633171\x00\x00\x00\x00\x00\x00', ], + (Ecu.dsu, 0x791, None): [ + b'881513310400\x00\x00\x00\x00', + ], (Ecu.eps, 0x7a1, None): [ b'8965B33512\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4701100\x00\x00\x00\x00', b'8821F4701300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F3302001\x00\x00\x00\x00', b'8646F3302200\x00\x00\x00\x00', ], }, @@ -1262,8 +1388,10 @@ FW_VERSIONS = { (Ecu.engine, 0x700, None): [ b'\x01896637851000\x00\x00\x00\x00', b'\x01896637852000\x00\x00\x00\x00', + b'\x01896637854000\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ + b'F152678130\x00\x00\x00\x00\x00\x00', b'F152678140\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ @@ -1274,12 +1402,30 @@ FW_VERSIONS = { b'8965B78080\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702100\x00\x00\x00\x00', b'8821F4702300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F7801100\x00\x00\x00\x00', ], }, + CAR.LEXUS_NX_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x018966378B2100\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'\x01F152678221\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B78120\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b"\x018821F3301400\x00\x00\x00\x00", + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + ], + }, CAR.LEXUS_NXH: { (Ecu.engine, 0x7e0, None): [ b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1310,17 +1456,19 @@ FW_VERSIONS = { }, CAR.LEXUS_RX: { (Ecu.engine, 0x700, None): [ + b'\x01896630E36200\x00\x00\x00\x00', + b'\x01896630E36300\x00\x00\x00\x00', b'\x01896630E37200\x00\x00\x00\x00', + b'\x01896630E37300\x00\x00\x00\x00', b'\x01896630E41000\x00\x00\x00\x00', b'\x01896630E41100\x00\x00\x00\x00', b'\x01896630E41200\x00\x00\x00\x00', - b'\x01896630E37300\x00\x00\x00\x00', - b'\x01896630E36300\x00\x00\x00\x00', - b'\x018966348R8500\x00\x00\x00\x00', - b'\x018966348W1300\x00\x00\x00\x00', + b'\x01896630EA4100\x00\x00\x00\x00', b'\x01896630EA4300\x00\x00\x00\x00', b'\x01896630EA6300\x00\x00\x00\x00', - b'\x01896630EA4100\x00\x00\x00\x00', + b'\x018966348R1300\x00\x00\x00\x00', + b'\x018966348R8500\x00\x00\x00\x00', + b'\x018966348W1300\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152648472\x00\x00\x00\x00\x00\x00', @@ -1359,9 +1507,12 @@ FW_VERSIONS = { }, CAR.LEXUS_RXH: { (Ecu.engine, 0x7e0, None): [ + b'\x02348J7000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348N0000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348Q4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348Q4100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348T1100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348T3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348V6000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348Z3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -1370,6 +1521,7 @@ FW_VERSIONS = { b'F152648501\x00\x00\x00\x00\x00\x00', b'F152648502\x00\x00\x00\x00\x00\x00', b'F152648504\x00\x00\x00\x00\x00\x00', + b'F152648740\x00\x00\x00\x00\x00\x00', b'F152648A30\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ @@ -1380,6 +1532,7 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B0E011\x00\x00\x00\x00\x00\x00', b'8965B0E012\x00\x00\x00\x00\x00\x00', + b'8965B48111\x00\x00\x00\x00\x00\x00', b'8965B48112\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -1390,6 +1543,7 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F4801200\x00\x00\x00\x00', + b'8646F4802001\x00\x00\x00\x00', b'8646F4802100\x00\x00\x00\x00', b'8646F4802200\x00\x00\x00\x00', b'8646F4809000\x00\x00\x00\x00', @@ -1403,6 +1557,7 @@ FW_VERSIONS = { b'\x01896630EA9000\x00\x00\x00\x00', b'\x01896630ED0000\x00\x00\x00\x00', b'\x018966348W9000\x00\x00\x00\x00', + b'\x01896634D12100\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'\x01F152648801\x00\x00\x00\x00\x00\x00', @@ -1445,11 +1600,13 @@ FW_VERSIONS = { }, CAR.PRIUS_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x028966347C8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00', b'\x038966347C5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152647500\x00\x00\x00\x00\x00\x00', + b'F152647510\x00\x00\x00\x00\x00\x00', b'F152647520\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -1462,6 +1619,13 @@ FW_VERSIONS = { b'\x028646F4707000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, + CAR.MIRAI: { + (Ecu.esp, 0x7D1, None): [b'\x01898A36203000\x00\x00\x00\x00',], + (Ecu.esp, 0x7B0, None): [b'\x01F15266203200\x00\x00\x00\x00',], # a second ESP ECU + (Ecu.eps, 0x7A1, None): [b'\x028965B6204100\x00\x00\x00\x008965B6203100\x00\x00\x00\x00',], + (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F6201200\x00\x00\x00\x00',], + (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',], + }, } STEER_THRESHOLD = 100 @@ -1498,13 +1662,16 @@ DBC = { CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'), CAR.LEXUS_NX: dbc_dict('lexus_nx300_2018_pt_generated', 'toyota_adas'), + CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.MIRAI: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), } # Toyota/Lexus Safety Sense 2.0 and 2.5 TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, - CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2]) + CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, + CAR.MIRAI, CAR.LEXUS_NX_TSS2]) NO_DSU_CAR = TSS2_CAR | set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5bb5dcc10..1317ee769 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -20,7 +20,7 @@ class CarController(): self.steer_rate_limited = False - def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, leftLaneVisible, rightLaneVisible): + def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ P = CarControllerParams @@ -110,16 +110,18 @@ class CarController(): # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz. if frame % P.LDW_STEP == 0: - hcaEnabled = True if enabled and not CS.out.standstill else False - if visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired: hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, hcaEnabled, - CS.out.steeringPressed, hud_alert, leftLaneVisible, - rightLaneVisible)) + + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled, + CS.out.steeringPressed, hud_alert, left_lane_visible, + right_lane_visible, CS.ldw_lane_warning_left, + CS.ldw_lane_warning_right, CS.ldw_side_dlc_tlc, + CS.ldw_dlc, CS.ldw_tlc, CS.out.standstill, + left_lane_depart, right_lane_depart)) #-------------------------------------------------------------------------- # # diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 0010c11e6..3897588f6 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -16,7 +16,7 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["EV_Gearshift"]['GearPosition'] self.buttonStates = BUTTON_STATES.copy() - def update(self, pt_cp, trans_type): + def update(self, pt_cp, cam_cp, trans_type): ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS @@ -80,8 +80,17 @@ class CarState(CarStateBase): # detection box is dynamic based on speed and road curvature. # Refer to VW Self Study Program 890253: Volkswagen Driver Assist Systems, # pages 32-35. - ret.leftBlindspot = bool(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"]) - ret.rightBlindspot = bool(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_re"]) + if self.CP.enableBsm: + ret.leftBlindspot = bool(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"]) + ret.rightBlindspot = bool(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_re"]) + + # Consume factory LDW data relevant for factory SWA (Lane Change Assist) + # and capture it for forwarding to the blind spot radar controller + self.ldw_lane_warning_left = bool(cam_cp.vl["LDW_02"]["LDW_SW_Warnung_links"]) + self.ldw_lane_warning_right = bool(cam_cp.vl["LDW_02"]["LDW_SW_Warnung_rechts"]) + self.ldw_side_dlc_tlc = bool(cam_cp.vl["LDW_02"]["LDW_Seite_DLCTLC"]) + self.ldw_dlc = cam_cp.vl["LDW_02"]["LDW_DLC"] + self.ldw_tlc = cam_cp.vl["LDW_02"]["LDW_TLC"] # Stock FCW is considered active if the release bit for brake-jerk warning # is set. Stock AEB considered active if the partial braking or target @@ -180,8 +189,6 @@ class CarState(CarStateBase): ("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied ("TSK_Status", "TSK_06", 0), # ACC engagement status from drivetrain coordinator ("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator - ("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status - ("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) ("ACC_Wunschgeschw", "ACC_02", 0), # ACC set speed ("AWV2_Freigabe", "ACC_10", 0), # FCW brake jerk release ("ANB_Teilbremsung_Freigabe", "ACC_10", 0), # AEB partial braking release @@ -197,10 +204,6 @@ class CarState(CarStateBase): ("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type ("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type ("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter - ("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blind spot object info, left - ("SWA_Warnung_SWA_li", "SWA_01", 0), # Blind spot object warning, left - ("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blind spot object info, right - ("SWA_Warnung_SWA_re", "SWA_01", 0), # Blind spot object warning, right ] checks = [ @@ -210,10 +213,10 @@ class CarState(CarStateBase): ("ESP_19", 100), # From J104 ABS/ESP controller ("ESP_05", 50), # From J104 ABS/ESP controller ("ESP_21", 50), # From J104 ABS/ESP controller - ("ACC_06", 50), # From J428 ACC radar control module ("ACC_10", 50), # From J428 ACC radar control module ("Motor_20", 50), # From J623 Engine control module ("TSK_06", 50), # From J623 Engine control module + ("ESP_02", 50), ("GRA_ACC_01", 33), # From J??? steering wheel control buttons ("ACC_02", 17), # From J428 ACC radar control module ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) @@ -224,6 +227,17 @@ class CarState(CarStateBase): ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM ] + if CP.enableBsm: + signals += [ + ("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_01", 0), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_01", 0), # Blind spot object warning, right + ] + checks += [ + ("SWA_01", 20), + ] + if CP.transmissionType == TransmissionType.automatic: signals += [("GE_Fahrstufe", "Getriebe_11", 0)] # Auto trans gear selector position checks += [("Getriebe_11", 20)] # From J743 Auto transmission control module @@ -237,15 +251,16 @@ class CarState(CarStateBase): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.pt) - # A single signal is monitored from the camera CAN bus, and then ignored, - # so the presence of CAN traffic can be verified with cam_cp.valid. - @staticmethod def get_cam_can_parser(CP): signals = [ # sig_name, sig_address, default - ("LDW_Status_LED_gruen", "LDW_02", 0), # Lane Assist status LED + ("LDW_SW_Warnung_links", "LDW_02", 0), # Blind spot in warning mode on left side due to lane departure + ("LDW_SW_Warnung_rechts", "LDW_02", 0), # Blind spot in warning mode on right side due to lane departure + ("LDW_Seite_DLCTLC", "LDW_02", 0), # Direction of most likely lane departure (left or right) + ("LDW_DLC", "LDW_02", 0), # Lane departure, distance to line crossing + ("LDW_TLC", "LDW_02", 0), # Lane departure, time to line crossing ] checks = [ diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 21721a0c0..787899359 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -57,7 +57,12 @@ class CarInterface(CarInterfaceBase): # Per-chassis tuning values, override tuning defaults here if desired - if candidate == CAR.GOLF_MK7: + if candidate == CAR.ATLAS_MK1: + # Averages of all CA Atlas variants + ret.mass = 2011 + STD_CARGO_KG + ret.wheelbase = 2.98 + + elif candidate == CAR.GOLF_MK7: # Averages of all AU Golf variants ret.mass = 1397 + STD_CARGO_KG ret.wheelbase = 2.62 @@ -77,10 +82,10 @@ class CarInterface(CarInterfaceBase): ret.mass = 1715 + STD_CARGO_KG ret.wheelbase = 2.74 - elif candidate == CAR.AUDI_A3: - # Temporarily carry forward old tuning values while we test vehicle identification - ret.mass = 1500 + STD_CARGO_KG - ret.wheelbase = 2.64 + elif candidate == CAR.AUDI_A3_MK3: + # Averages of all 8V A3 variants + ret.mass = 1335 + STD_CARGO_KG + ret.wheelbase = 2.61 elif candidate == CAR.SEAT_ATECA_MK1: # Averages of all 5F Ateca variants @@ -104,7 +109,8 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.45 - ret.enableCamera = True # Stock camera detection doesn't apply to VW + ret.enableCamera = True + ret.enableBsm = 0x30F in fingerprint[0] # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase @@ -127,7 +133,7 @@ class CarInterface(CarInterfaceBase): self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - ret = self.CS.update(self.cp, self.CP.transmissionType) + ret = self.CS.update(self.cp, self.cp_cam, self.CP.transmissionType) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False @@ -167,8 +173,9 @@ class CarInterface(CarInterfaceBase): def apply(self, c): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.hudControl.visualAlert, - c.hudControl.audibleAlert, c.hudControl.leftLaneVisible, - c.hudControl.rightLaneVisible) + c.hudControl.rightLaneVisible, + c.hudControl.leftLaneDepart, + c.hudControl.rightLaneDepart) self.frame += 1 return can_sends diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 8d85cb47a..ec2d879fa 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -55,17 +55,21 @@ MQB_LDW_MESSAGES = { # FW_VERSIONS for that existing CAR. class CAR: + ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 Jetta PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 Passat and variants TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants + AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN" # Chassis NS, Mk1 Skoda Kodiaq SKODA_SCALA_MK1 = "SKODA SCALA 1ST GEN" # Chassis NW, Mk1 Skoda Scala and Skoda Kamiq SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN" # Chassis 3V/NP, Mk3 Skoda Superb and variants - AUDI_A3 = "AUDI A3" # Chassis 8V/FF, Mk3 Audi A3 and variants FINGERPRINTS = { + CAR.ATLAS_MK1: [{ + 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 418: 8, 427: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 927: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1156: 8, 1157: 8, 1158: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1471: 4, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1635: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8, 1792: 8, 1872: 8, 1879: 8, 1976: 8, 1977: 8, 1985: 8, 2015: 8 + }], CAR.GOLF_MK7: [{ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 927: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1529: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8 }], @@ -78,7 +82,7 @@ FINGERPRINTS = { CAR.TIGUAN_MK2: [{ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 376: 8, 418: 8, 427: 8, 573: 8, 679: 8, 681: 8, 684: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 828: 8, 870: 8, 879: 8, 884: 8, 888: 8, 891: 8, 896: 8, 897: 8, 898: 8, 901: 8, 913: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1156: 8, 1157: 8, 1158: 8, 1162: 8, 1175: 8, 1312: 8, 1343: 8, 1385: 8, 1413: 8, 1440: 5, 1471: 4, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1635: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8 }], - CAR.AUDI_A3: [{ + CAR.AUDI_A3_MK3: [{ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 285: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 295: 8, 299: 8, 302: 8, 346: 8, 418: 8, 427: 8, 506: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 792: 8, 802: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 846: 8, 847: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1624: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8, 1792: 8, 1872: 8, 1976: 8, 1977: 8, 1982: 8, 1985: 8 }], CAR.SEAT_ATECA_MK1: [{ @@ -95,24 +99,27 @@ FINGERPRINTS = { }], } -IGNORED_FINGERPRINTS = [CAR.JETTA_MK7, CAR.PASSAT_MK8, CAR.TIGUAN_MK2, CAR.SEAT_ATECA_MK1, CAR.SKODA_KODIAQ_MK1, CAR.SKODA_SCALA_MK1, CAR.SKODA_SUPERB_MK3] +# All VW should be here +IGNORED_FINGERPRINTS = [CAR.ATLAS_MK1, CAR.GOLF_MK7, CAR.JETTA_MK7, CAR.PASSAT_MK8, + CAR.TIGUAN_MK2, CAR.AUDI_A3_MK3, CAR.SEAT_ATECA_MK1, + CAR.SKODA_KODIAQ_MK1, CAR.SKODA_SCALA_MK1, CAR.SKODA_SUPERB_MK3 ] FW_VERSIONS = { - CAR.AUDI_A3: { + CAR.ATLAS_MK1: { (Ecu.engine, 0x7e0, None): [ - b'\xf1\x875G0906259L \xf1\x890002', + b'\xf1\x8703H906026AA\xf1\x899970', ], (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x870D9300013B \xf1\x894931', + b'\xf1\x8709G927158DR\xf1\x893536', ], (Ecu.srs, 0x715, None): [ - b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221', + b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900', ], (Ecu.eps, 0x712, None): [ - b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1', + b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1', ], (Ecu.fwdRadar, 0x757, None): [ - b'\xf1\x875Q0907572G \xf1\x890571', + b'\xf1\x875Q0907572P \xf1\x890682', ], }, CAR.GOLF_MK7: { @@ -123,57 +130,83 @@ FW_VERSIONS = { b'\xf1\x8704E906023BN\xf1\x894518', b'\xf1\x8704E906027GR\xf1\x892394', b'\xf1\x8704L906026NF\xf1\x899528', + b'\xf1\x8704L906056CR\xf1\x895813', b'\xf1\x8704L906056HE\xf1\x893758', b'\xf1\x870EA906016A \xf1\x898343', + b'\xf1\x870EA906016F \xf1\x895002', b'\xf1\x870EA906016S \xf1\x897207', b'\xf1\x875G0906259 \xf1\x890007', b'\xf1\x875G0906259J \xf1\x890002', b'\xf1\x875G0906259L \xf1\x890002', + b'\xf1\x875G0906259N \xf1\x890003', b'\xf1\x875G0906259Q \xf1\x890002', + b'\xf1\x875G0906259Q \xf1\x892313', + b'\xf1\x878V0906259J \xf1\x890003', b'\xf1\x878V0906259P \xf1\x890001', b'\xf1\x878V0906259Q \xf1\x890002', + b'\xf1\x878V0906264F \xf1\x890003', + b'\xf1\x878V0906264L \xf1\x890002', + b'\xf1\x878V0906264M \xf1\x890001', ], (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x870CW300045 \xf1\x894531', - b'\xf1\x870D9300040S \xf1\x894311', - b'\xf1\x870CW300047D \xf1\x895261', - b'\xf1\x870D9300012 \xf1\x894913', + b'\xf1\x8709G927749AP\xf1\x892943', b'\xf1\x870CW300042F \xf1\x891604', - b'\xf1\x870DD300046F \xf1\x891601', + b'\xf1\x870CW300045 \xf1\x894531', + b'\xf1\x870CW300047D \xf1\x895261', + b'\xf1\x870CW300048J \xf1\x890611', + b'\xf1\x870D9300012 \xf1\x894913', + b'\xf1\x870D9300014M \xf1\x895004', b'\xf1\x870D9300020S \xf1\x895201', - b'\xf1\x870GC300012A \xf1\x891403', - b'\xf1\x870GC300043T \xf1\x899999', - b'\xf1\x870GC300020G \xf1\x892404', - b'\xf1\x870GC300014B \xf1\x892405', + b'\xf1\x870D9300040S \xf1\x894311', b'\xf1\x870DD300045K \xf1\x891120', + b'\xf1\x870DD300046F \xf1\x891601', + b'\xf1\x870GC300012A \xf1\x891403', + b'\xf1\x870GC300014B \xf1\x892401', + b'\xf1\x870GC300014B \xf1\x892405', + b'\xf1\x870GC300020G \xf1\x892401', + b'\xf1\x870GC300020G \xf1\x892403', + b'\xf1\x870GC300020G \xf1\x892404', + b'\xf1\x870GC300043T \xf1\x899999', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\0211413001113120043114317121C111C9113', + b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\0211413001113120053114317121C111C9113', + b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120043114317121C111C9113', + b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120043114417121411149113', + b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120053114317121C111C9113', + b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02314160011123300314211012230229333463100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\023141600111233003142405A2252229333463100', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023271112111312--071104171825102591131211', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023271212111312--071104171838103891131211', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023341512112212--071104172328102891131211', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13272512111312--07110417182C102C91131211', b'\xf1\x875Q0959655M \xf1\x890361\xf1\x82\0211413001112120041114115121611169112', - b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02324230011211200621143171724112491132111', b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011211200621143171717111791132111', - b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011211200061104171717101791132111', b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02324230011211200061104171724102491132111', - b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\0211413001113120053114317121C111C9113', - b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\0211413001113120043114317121C111C9113', - b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120053114317121C111C9113', - b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120043114417121411149113', - b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02314160011123300314211012230229333463100', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02324230011211200621143171724112491132111', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011211200061104171717101791132111', + b'\xf1\x875Q0959655T \xf1\x890830\xf1\x82\x13271100111312--071104171826102691131211', ], (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\00561A01612A0', b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566A0J612A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A00514A1', + b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\00571A0J714A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1', + b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\00571A01A18A1', b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\00571A0JA16A1', b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820519A9040203', - b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00522A00402A0', - b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511A00403A0', - b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516A07A02A1', b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\00521A00441A1', b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00641A1', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A00642A1', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A07B05A1', + b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00522A00402A0', + b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511A00403A0', + b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516A00604A1', + b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516A07A02A1', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521A20B03A1', + b'\xf1\x875QM909144A \xf1\x891072\xf1\x82\x0521A20B03A1', + b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\00571A01A18A1', b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A17A1', ], (Ecu.fwdRadar, 0x757, None): [ @@ -182,6 +215,7 @@ FW_VERSIONS = { b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\00101', b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\00101', b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101', + b'\xf1\x875Q0907572G \xf1\x890571', b'\xf1\x875Q0907572H \xf1\x890620', b'\xf1\x875Q0907572J \xf1\x890654', b'\xf1\x875Q0907572P \xf1\x890682', @@ -189,20 +223,23 @@ FW_VERSIONS = { }, CAR.JETTA_MK7: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906024AK\xf1\x899937', + b'\xf1\x8704E906024AS\xf1\x899912', b'\xf1\x8704E906024B \xf1\x895594', b'\xf1\x8704E906024L \xf1\x895595', - b'\xf1\x8704E906024AK\xf1\x899937', b'\xf1\x875G0906259T \xf1\x890003', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709S927158BS\xf1\x893642', b'\xf1\x8709S927158R \xf1\x893552', b'\xf1\x8709S927158R \xf1\x893587', b'\xf1\x870GC300020N \xf1\x892803', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AG\xf1\x890336\xf1\x82\02314171231313500314611011630169333463100', - b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02311170031313300314240011150119333433100', b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314643011650169333463100', + b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314642011650169333463100', + b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02311170031313300314240011150119333433100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\00521A10A01A1', @@ -221,20 +258,20 @@ FW_VERSIONS = { b'\xf1\x8704L906026GA\xf1\x892013', ], (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x870DD300045T \xf1\x891601', b'\xf1\x870D9300014L \xf1\x895002', + b'\xf1\x870DD300045T \xf1\x891601', ], (Ecu.srs, 0x715, None): [ - b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111', b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1', ], (Ecu.fwdRadar, 0x757, None): [ - b'\xf1\x875Q0907572R \xf1\x890771', b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x875Q0907572R \xf1\x890771', ], }, CAR.TIGUAN_MK2: { @@ -254,6 +291,40 @@ FW_VERSIONS = { b'\xf1\x872Q0907572R \xf1\x890372', ], }, + CAR.AUDI_A3_MK3: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906023AN\xf1\x893695', + b'\xf1\x8704E906023AR\xf1\x893440', + b'\xf1\x8704E906023BL\xf1\x895190', + b'\xf1\x8704L997022N \xf1\x899459', + b'\xf1\x875G0906259L \xf1\x890002', + b'\xf1\x878V0906264B \xf1\x890003', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300048 \xf1\x895201', + b'\xf1\x870D9300013B \xf1\x894931', + b'\xf1\x870D9300041N \xf1\x894512', + b'\xf1\x870DD300046A \xf1\x891602', + b'\xf1\x870DD300046F \xf1\x891602', + b'\xf1\x870DD300046G \xf1\x891601', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AM\xf1\x890315\xf1\x82\x1311111111111111311411011231129321212100', + b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\023111112111111--171115141112221291163221', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13121111111111--341117141212231291163221', + b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112111104110411111521159114', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00803A0', + b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516G00804A1', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\00101', + b'\xf1\x875Q0907572G \xf1\x890571', + ], + }, CAR.SEAT_ATECA_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906027KA\xf1\x893749', @@ -275,20 +346,25 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906027DD\xf1\x893123', b'\xf1\x875NA907115E \xf1\x890003', + b'\xf1\x8704L906026DE\xf1\x895418', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300012M \xf1\x892107', + b'\xf1\x870DL300012N \xf1\x892110', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\0161213001211001205212111052100', + b'\xf1\x873Q0959655CQ\xf1\x890720\xf1\x82\x0e1213111211001205212112052111', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6060405', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G600', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572R \xf1\x890372', + b'\xf1\x872Q0907572Q \xf1\x890342', ], }, CAR.SKODA_SCALA_MK1: { @@ -311,26 +387,38 @@ FW_VERSIONS = { CAR.SKODA_SUPERB_MK3: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906026KB\xf1\x894071', + b'\xf1\x873G0906259B \xf1\x890002', + b'\xf1\x8704L906026FP\xf1\x891196', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870D9300012 \xf1\x894940', + b'\xf1\x870D9300011T \xf1\x894801', ], - # Only onboarded Superb so far is a manual (Ecu.transmission, 0x7e1, None): [], (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100', + b'\xf1\x875Q0959655AK\xf1\x890130\xf1\x82\022111200111121001121110012211292221111', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522UZ070303', b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700', + b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514UZ070203', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x873Q0907572B \xf1\x890194', b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x873Q0907572B \xf1\x890192', ], }, } DBC = { + CAR.ATLAS_MK1: dbc_dict('vw_mqb_2010', None), CAR.GOLF_MK7: dbc_dict('vw_mqb_2010', None), CAR.JETTA_MK7: dbc_dict('vw_mqb_2010', None), CAR.PASSAT_MK8: dbc_dict('vw_mqb_2010', None), CAR.TIGUAN_MK2: dbc_dict('vw_mqb_2010', None), - CAR.AUDI_A3: dbc_dict('vw_mqb_2010', None), + CAR.AUDI_A3_MK3: dbc_dict('vw_mqb_2010', None), CAR.SEAT_ATECA_MK1: dbc_dict('vw_mqb_2010', None), CAR.SKODA_KODIAQ_MK1: dbc_dict('vw_mqb_2010', None), CAR.SKODA_SCALA_MK1: dbc_dict('vw_mqb_2010', None), diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index 4d0eaec62..0e35d21d6 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -15,23 +15,26 @@ def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled): } return packer.make_can_msg("HCA_01", bus, values, idx) -def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, leftLaneVisible, rightLaneVisible): - - if hca_enabled: - leftlanehud = 3 if leftLaneVisible else 1 - rightlanehud = 3 if rightLaneVisible else 1 - else: - leftlanehud = 2 if leftLaneVisible else 1 - rightlanehud = 2 if rightLaneVisible else 1 +def create_mqb_hud_control(packer, bus, enabled, steering_pressed, hud_alert, left_lane_visible, right_lane_visible, + ldw_lane_warning_left, ldw_lane_warning_right, ldw_side_dlc_tlc, ldw_dlc, ldw_tlc, + standstill, left_lane_depart, right_lane_depart): + # Lane color reference: + # 0 (LKAS disabled) - off + # 1 (LKAS enabled, no lane detected) - dark gray + # 2 (LKAS enabled, lane detected) - light gray on VW, green or white on Audi depending on year or virtual cockpit. On a color MFD on a 2015 A3 TDI it is white, virtual cockpit on a 2018 A3 e-Tron its green. + # 3 (LKAS enabled, lane departure detected) - white on VW, red on Audi values = { - "LDW_SW_Warnung_links": 0, # FIXME: to be store-and-forwarded from the stock camera - "LDW_SW_Warnung_rechts": 1, # FIXME: to be store-and-forwarded from the stock camera - "LDW_Status_LED_gelb": 1 if hca_enabled and steering_pressed else 0, - "LDW_Status_LED_gruen": 1 if hca_enabled and not steering_pressed else 0, - "LDW_Lernmodus_links": leftlanehud, - "LDW_Lernmodus_rechts": rightlanehud, + "LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0, + "LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Lernmodus_links": 3 if left_lane_depart else 1 + left_lane_visible, + "LDW_Lernmodus_rechts": 3 if right_lane_depart else 1 + right_lane_visible, "LDW_Texte": hud_alert, + "LDW_SW_Warnung_links": ldw_lane_warning_left, + "LDW_SW_Warnung_rechts": ldw_lane_warning_right, + "LDW_Seite_DLCTLC": ldw_side_dlc_tlc, + "LDW_DLC": ldw_dlc, + "LDW_TLC": ldw_tlc } return packer.make_can_msg("LDW_02", bus, values) diff --git a/selfdrive/clocksd/clocksd.cc b/selfdrive/clocksd/clocksd.cc index 32d8c508c..c6ad4061a 100644 --- a/selfdrive/clocksd/clocksd.cc +++ b/selfdrive/clocksd/clocksd.cc @@ -1,8 +1,8 @@ -#include #include -#include +#include #include #include +#include // Apple doesn't have timerfd #ifdef __APPLE__ @@ -14,9 +14,9 @@ #include #include -#include "messaging.hpp" -#include "common/timing.h" -#include "common/util.h" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" ExitHandler do_exit; diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index fb36e2b9f..5d5900500 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -28,6 +28,8 @@ if arch == "aarch64": 'touch.c', ] _gpu_libs = ['gui', 'adreno_utils'] +elif arch == "larch64": + _gpu_libs = ["GLESv2"] else: _gpu_libs = ["GL"] diff --git a/selfdrive/common/clutil.cc b/selfdrive/common/clutil.cc index c6a2b49dc..526c66b22 100644 --- a/selfdrive/common/clutil.cc +++ b/selfdrive/common/clutil.cc @@ -1,12 +1,15 @@ -#include "clutil.h" +#include "selfdrive/common/clutil.h" + #include #include #include #include -#include + #include +#include #include -#include "util.h" + +#include "selfdrive/common/util.h" namespace { // helper functions diff --git a/selfdrive/common/framebuffer.cc b/selfdrive/common/framebuffer.cc index 62ac7053e..79051f7e8 100644 --- a/selfdrive/common/framebuffer.cc +++ b/selfdrive/common/framebuffer.cc @@ -1,15 +1,15 @@ -#include "framebuffer.h" -#include "util.h" +#include "selfdrive/common/framebuffer.h" + #include #include +#include "selfdrive/common/util.h" + #include #include #include #include - - #include #include @@ -40,7 +40,7 @@ void FrameBuffer::swap() { bool set_brightness(int brightness) { char bright[64]; snprintf(bright, sizeof(bright), "%d", brightness); - return 0 == write_file("/sys/class/leds/lcd-backlight/brightness", bright, strlen(bright)); + return 0 == util::write_file("/sys/class/leds/lcd-backlight/brightness", bright, strlen(bright)); } void FrameBuffer::set_power(int mode) { diff --git a/selfdrive/common/framebuffer.h b/selfdrive/common/framebuffer.h index d60424d6a..d7054374c 100644 --- a/selfdrive/common/framebuffer.h +++ b/selfdrive/common/framebuffer.h @@ -1,5 +1,7 @@ #pragma once + #include + #include "hardware/hwcomposer_defs.h" bool set_brightness(int brightness); diff --git a/selfdrive/common/glutil.cc b/selfdrive/common/glutil.cc index 77cb82627..7e53975e1 100644 --- a/selfdrive/common/glutil.cc +++ b/selfdrive/common/glutil.cc @@ -1,9 +1,10 @@ -#include -#include -#include -#include +#include "selfdrive/common/glutil.h" -#include "glutil.h" +#include +#include +#include + +#include static GLuint load_shader(GLenum shaderType, const char *src) { GLint status = 0, len = 0; diff --git a/selfdrive/common/glutil.h b/selfdrive/common/glutil.h index dff87821f..7180f855c 100644 --- a/selfdrive/common/glutil.h +++ b/selfdrive/common/glutil.h @@ -1,12 +1,13 @@ #pragma once -#ifdef __APPLE__ - #include -#else - #include -#endif #include +#ifdef __APPLE__ +#include +#else +#include +#endif + class GLShader { public: GLShader(const char *vert_src, const char *frag_src); diff --git a/selfdrive/common/gpio.cc b/selfdrive/common/gpio.cc index 8a72388d4..8a2885316 100644 --- a/selfdrive/common/gpio.cc +++ b/selfdrive/common/gpio.cc @@ -1,8 +1,10 @@ -#include "gpio.h" -#include "util.h" +#include "selfdrive/common/gpio.h" + #include -#include #include +#include + +#include "selfdrive/common/util.h" // We assume that all pins have already been exported on boot, // and that we have permission to write to them. @@ -15,7 +17,7 @@ int gpio_init(int pin_nr, bool output){ return -1; } const char *value = output ? "out" : "in"; - return write_file(pin_dir_path, (void*)value, strlen(value)); + return util::write_file(pin_dir_path, (void*)value, strlen(value)); } int gpio_set(int pin_nr, bool high){ @@ -25,5 +27,5 @@ int gpio_set(int pin_nr, bool high){ if(pin_val_path_len <= 0){ return -1; } - return write_file(pin_val_path, (void*)(high ? "1" : "0"), 1); + return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1); } diff --git a/selfdrive/common/i2c.cc b/selfdrive/common/i2c.cc index 95d0aedd5..c2842948b 100644 --- a/selfdrive/common/i2c.cc +++ b/selfdrive/common/i2c.cc @@ -1,12 +1,14 @@ -#include "i2c.h" +#include "selfdrive/common/i2c.h" + +#include +#include +#include +#include #include -#include #include -#include -#include -#include -#include "common/swaglog.h" + +#include "selfdrive/common/swaglog.h" #define UNUSED(x) (void)(x) diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index fbbd9ce46..b6282000e 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -19,27 +19,27 @@ const double X_IDXS[TRAJECTORY_SIZE] = { 0. , 0.1875, 0.75 , 1.6875, #ifdef __cplusplus -#include "common/mat.h" -#ifdef QCOM2 -const mat3 fcam_intrinsic_matrix = (mat3){{ - 2648.0, 0.0, 1928.0/2, - 0.0, 2648.0, 1208.0/2, - 0.0, 0.0, 1.0 -}}; -#else -const mat3 fcam_intrinsic_matrix = (mat3){{ - 910., 0., 1164.0/2, - 0., 910., 874.0/2, - 0., 0., 1. -}}; -#endif +#include "selfdrive/common/mat.h" +#include "selfdrive/hardware/hw.h" +const mat3 fcam_intrinsic_matrix = + Hardware::TICI() ? (mat3){{2648.0, 0.0, 1928.0 / 2, + 0.0, 2648.0, 1208.0 / 2, + 0.0, 0.0, 1.0}} + : (mat3){{910., 0., 1164.0 / 2, + 0., 910., 874.0 / 2, + 0., 0., 1.}}; + +// without unwarp, focal length is for center portion only +const mat3 ecam_intrinsic_matrix = + Hardware::TICI() ? (mat3){{620.0, 0.0, 1928.0 / 2, + 0.0, 620.0, 1208.0 / 2, + 0.0, 0.0, 1.0}} + : (mat3){{0., 0., 0., + 0., 0., 0., + 0., 0., 0.}}; static inline mat3 get_model_yuv_transform(bool bayer = true) { -#ifndef QCOM2 - float db_s = 0.5; // debayering does a 2x downscale -#else - float db_s = 1.0; -#endif + float db_s = Hardware::TICI() ? 1.0 : 0.5; // debayering does a 2x downscale on EON const mat3 transform = (mat3){{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, @@ -49,4 +49,3 @@ static inline mat3 get_model_yuv_transform(bool bayer = true) { } #endif - diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 930f3f0df..87d338ed7 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -1,45 +1,49 @@ -#include "common/params.h" +#include "selfdrive/common/params.h" #ifndef _GNU_SOURCE #define _GNU_SOURCE #endif // _GNU_SOURCE +#include #include #include -#include -#include +#include #include #include +#include -#include -#include -#include #include -#include +#include +#include -#include "common/util.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" +// keep trying if x gets interrupted by a signal +#define HANDLE_EINTR(x) \ + ({ \ + decltype(x) ret; \ + int try_cnt = 0; \ + do { \ + ret = (x); \ + } while (ret == -1 && errno == EINTR && try_cnt++ < 100); \ + ret; \ + }) -#if defined(QCOM) || defined(QCOM2) -const std::string default_params_path = "/data/params"; -#else -const std::string default_params_path = util::getenv_default("HOME", "/.comma/params", "/data/params"); -#endif - -#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 +namespace { +const std::string default_params_path = Hardware::PC() ? util::getenv_default("HOME", "/.comma/params", "/data/params") + : "/data/params"; +const std::string persistent_params_path = Hardware::PC() ? default_params_path : "/persist/comma/params"; 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 fd = open(path, O_RDONLY, 0755); +int fsync_dir(const char* path){ + int fd = HANDLE_EINTR(open(path, O_RDONLY, 0755)); if (fd < 0){ return -1; } @@ -52,7 +56,8 @@ static int fsync_dir(const char* path){ return result; } -static int mkdir_p(std::string path) { +// TODO: replace by std::filesystem::create_directories +int mkdir_p(std::string path) { char * _path = (char *)path.c_str(); mode_t prev_mask = umask(0); @@ -73,281 +78,250 @@ static int mkdir_p(std::string path) { return 0; } -static int ensure_dir_exists(std::string path) { - // TODO: replace by std::filesystem::create_directories - return mkdir_p(path.c_str()); +bool ensure_params_path(const std::string ¶m_path, const std::string &key_path) { + // Make sure params path exists + if (!util::file_exists(param_path) && mkdir_p(param_path) != 0) { + return false; + } + + // See if the symlink exists, otherwise create it + if (!util::file_exists(key_path)) { + // 1) Create temp folder + // 2) Set permissions + // 3) Symlink it to temp link + // 4) Move symlink to /d + + std::string tmp_path = param_path + "/.tmp_XXXXXX"; + // this should be OK since mkdtemp just replaces characters in place + char *tmp_dir = mkdtemp((char *)tmp_path.c_str()); + if (tmp_dir == NULL) { + return false; + } + + if (chmod(tmp_dir, 0777) != 0) { + return false; + } + + std::string link_path = std::string(tmp_dir) + ".link"; + if (symlink(tmp_dir, link_path.c_str()) != 0) { + return false; + } + + // don't return false if it has been created by other + if (rename(link_path.c_str(), key_path.c_str()) != 0 && errno != EEXIST) { + return false; + } + } + + // Ensure permissions are correct in case we didn't create the symlink + return chmod(key_path.c_str(), 0777) == 0; } +class FileLock { + public: + FileLock(const std::string& file_name, int op) : fn_(file_name), op_(op) {} -Params::Params(bool persistent_param){ - params_path = persistent_param ? persistent_params_path : default_params_path; + void lock() { + fd_ = HANDLE_EINTR(open(fn_.c_str(), O_CREAT, 0775)); + if (fd_ < 0) { + LOGE("Failed to open lock file %s, errno=%d", fn_.c_str(), errno); + return; + } + if (HANDLE_EINTR(flock(fd_, op_)) < 0) { + close(fd_); + LOGE("Failed to lock file %s, errno=%d", fn_.c_str(), errno); + } + } + + void unlock() { close(fd_); } + +private: + int fd_ = -1, op_; + std::string fn_; +}; + +std::unordered_map keys = { + {"AccessToken", CLEAR_ON_MANAGER_START}, + {"ApiCache_DriveStats", PERSISTENT}, + {"ApiCache_Device", PERSISTENT}, + {"ApiCache_Owner", PERSISTENT}, + {"AthenadPid", PERSISTENT}, + {"CalibrationParams", PERSISTENT}, + {"CarBatteryCapacity", PERSISTENT}, + {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION}, + {"CarParamsCache", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, + {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION}, + {"CommunityFeaturesToggle", PERSISTENT}, + {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION}, + {"EnableLteOnroad", PERSISTENT}, + {"EndToEndToggle", PERSISTENT}, + {"CompletedTrainingVersion", PERSISTENT}, + {"DisablePowerDown", PERSISTENT}, + {"DisableUpdates", PERSISTENT}, + {"EnableWideCamera", PERSISTENT}, + {"DoUninstall", CLEAR_ON_MANAGER_START}, + {"DongleId", PERSISTENT}, + {"GitDiff", PERSISTENT}, + {"GitBranch", PERSISTENT}, + {"GitCommit", PERSISTENT}, + {"GitRemote", PERSISTENT}, + {"GithubSshKeys", PERSISTENT}, + {"GithubUsername", PERSISTENT}, + {"HardwareSerial", PERSISTENT}, + {"HasAcceptedTerms", PERSISTENT}, + {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, + {"IMEI", PERSISTENT}, + {"IsLdwEnabled", PERSISTENT}, + {"IsMetric", PERSISTENT}, + {"IsOffroad", CLEAR_ON_MANAGER_START}, + {"IsRHD", PERSISTENT}, + {"IsTakingSnapshot", CLEAR_ON_MANAGER_START}, + {"IsUpdateAvailable", CLEAR_ON_MANAGER_START}, + {"IsUploadRawEnabled", PERSISTENT}, + {"LastAthenaPingTime", PERSISTENT}, + {"LastGPSPosition", PERSISTENT}, + {"LastUpdateException", PERSISTENT}, + {"LastUpdateTime", PERSISTENT}, + {"LiveParameters", PERSISTENT}, + {"OpenpilotEnabledToggle", PERSISTENT}, + {"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, + {"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, + {"PandaDongleId", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, + {"Passive", PERSISTENT}, + {"RecordFront", PERSISTENT}, + {"RecordFrontLock", PERSISTENT}, // for the internal fleet + {"ReleaseNotes", PERSISTENT}, + {"ShouldDoUpdate", CLEAR_ON_MANAGER_START}, + {"SubscriberInfo", PERSISTENT}, + {"SshEnabled", PERSISTENT}, + {"TermsVersion", PERSISTENT}, + {"Timezone", PERSISTENT}, + {"TrainingVersion", PERSISTENT}, + {"UpdateAvailable", CLEAR_ON_MANAGER_START}, + {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, + {"Version", PERSISTENT}, + {"VisionRadarToggle", PERSISTENT}, + {"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, + {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START}, + {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START}, + {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, + {"Offroad_PandaFirmwareMismatch", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, + {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START}, + {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START}, + {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START}, + {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, + {"Offroad_HardwareUnsupported", CLEAR_ON_MANAGER_START}, + {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, + {"ForcePowerDown", CLEAR_ON_MANAGER_START}, +}; + +} // namespace + +Params::Params(bool persistent_param) : Params(persistent_param ? persistent_params_path : default_params_path) {} + +Params::Params(const std::string &path) : params_path(path) { + if (!ensure_params_path(params_path, params_path + "/d")) { + throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno)); + } } -Params::Params(std::string path) { - params_path = path; +bool Params::checkKey(const std::string &key) { + return keys.find(key) != keys.end(); } -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) { +int Params::put(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 // 3) fsync() the temp file // 4) rename the temp file to the real name // 5) fsync() the containing directory + std::string tmp_path = params_path + "/.tmp_value_XXXXXX"; + int tmp_fd = mkstemp((char*)tmp_path.c_str()); + if (tmp_fd < 0) return -1; - int lock_fd = -1; - int tmp_fd = -1; - int result; - std::string path; - std::string tmp_path; - ssize_t bytes_written; - - // Make sure params path exists - result = ensure_dir_exists(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.c_str(), &st) == -1) { - // Create temp folder - 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.c_str(), 0777); - if (result < 0) { - goto cleanup; + int result = -1; + do { + // Write value to temp. + ssize_t bytes_written = HANDLE_EINTR(write(tmp_fd, value, value_size)); + if (bytes_written < 0 || (size_t)bytes_written != value_size) { + result = -20; + break; } - // Symlink it to temp link - tmp_path = tmp_dir + ".link"; - result = symlink(tmp_dir.c_str(), tmp_path.c_str()); - if (result < 0) { - goto cleanup; - } + // change permissions to 0666 for apks + if ((result = fchmod(tmp_fd, 0666)) < 0) break; + // fsync to force persist the changes. + if ((result = fsync(tmp_fd)) < 0) break; - // Move symlink to /d + FileLock file_lock(params_path + "/.lock", LOCK_EX); + std::lock_guard lk(file_lock); + + // Move temp into place. + std::string path = params_path + "/d/" + std::string(key); + if ((result = rename(tmp_path.c_str(), path.c_str())) < 0) break; + + // fsync parent directory path = params_path + "/d"; - result = rename(tmp_path.c_str(), path.c_str()); - if (result < 0) { - goto cleanup; - } - } else { - // Ensure permissions are correct in case we didn't create the symlink - result = chmod(path.c_str(), 0777); - if (result < 0) { - goto cleanup; - } - } + result = fsync_dir(path.c_str()); + } while(0); - // Write value to temp. - 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 < 0 || (size_t)bytes_written != value_size) { - result = -20; - goto cleanup; - } - - // Build lock path - path = params_path + "/.lock"; - lock_fd = open(path.c_str(), O_CREAT, 0775); - - // Build key path - path = params_path + "/d/" + std::string(key); - - // Take lock. - result = flock(lock_fd, LOCK_EX); - if (result < 0) { - goto cleanup; - } - - // change permissions to 0666 for apks - result = fchmod(tmp_fd, 0666); - if (result < 0) { - goto cleanup; - } - - // fsync to force persist the changes. - result = fsync(tmp_fd); - if (result < 0) { - goto cleanup; - } - - // Move temp into place. - result = rename(tmp_path.c_str(), path.c_str()); - if (result < 0) { - goto cleanup; - } - - // fsync parent directory - path = params_path + "/d"; - result = fsync_dir(path.c_str()); - if (result < 0) { - goto cleanup; - } - -cleanup: - // Release lock. - if (lock_fd >= 0) { - close(lock_fd); - } - if (tmp_fd >= 0) { - if (result < 0) { - remove(tmp_path.c_str()); - } - close(tmp_fd); - } + close(tmp_fd); + remove(tmp_path.c_str()); return result; } -int Params::delete_db_value(std::string key) { - int lock_fd = -1; - int result; - std::string path; - - // Build lock path, and open lockfile - path = params_path + "/.lock"; - lock_fd = open(path.c_str(), O_CREAT, 0775); - - // Take lock. - result = flock(lock_fd, LOCK_EX); - if (result < 0) { - goto cleanup; - } - +int Params::remove(const char *key) { + FileLock file_lock(params_path + "/.lock", LOCK_EX); + std::lock_guard lk(file_lock); // Delete value. - path = params_path + "/d/" + key; - result = remove(path.c_str()); + std::string path = params_path + "/d/" + key; + int result = ::remove(path.c_str()); if (result != 0) { result = ERR_NO_VALUE; - goto cleanup; + return result; } - // fsync parent directory path = params_path + "/d"; - result = fsync_dir(path.c_str()); - if (result < 0) { - goto cleanup; - } - -cleanup: - // Release lock. - if (lock_fd >= 0) { - close(lock_fd); - } - return result; + return fsync_dir(path.c_str()); } -std::string Params::get(std::string key, bool block){ - char* value; - size_t size; - int r; - - if (block) { - r = read_db_value_blocking((const char*)key.c_str(), &value, &size); +std::string Params::get(const char *key, bool block) { + std::string path = params_path + "/d/" + key; + if (!block) { + return util::read_file(path); } else { - r = read_db_value((const char*)key.c_str(), &value, &size); - } + // blocking read until successful + 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); - 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; -} - -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) { - break; - } else { - util::sleep_for(100); // 0.1 s + std::string value; + while (!params_do_exit) { + if (value = util::read_file(path); !value.empty()) { + break; + } + util::sleep_for(100); // 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 + std::signal(SIGINT, prev_handler_sigint); + std::signal(SIGTERM, prev_handler_sigterm); + return value; + } } -int Params::read_db_all(std::map *params) { - int err = 0; - - std::string lock_path = params_path + "/.lock"; - - int lock_fd = open(lock_path.c_str(), 0); - if (lock_fd < 0) return -1; - - err = flock(lock_fd, LOCK_SH); - if (err < 0) { - close(lock_fd); - return err; - } +int Params::readAll(std::map *params) { + FileLock file_lock(params_path + "/.lock", LOCK_SH); + std::lock_guard lk(file_lock); std::string key_path = params_path + "/d"; - DIR *d = opendir(key_path.c_str()); - if (!d) { - close(lock_fd); - return -1; - } - - struct dirent *de = NULL; - 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(key_path + "/" + key); - - (*params)[key] = value; - } - - closedir(d); - - close(lock_fd); - return 0; + return util::read_files_in_dir(key_path, params); } -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); - if (result == 0) { - bytes.assign(value, value+sz); - free(value); +void Params::clearAll(ParamKeyType key_type) { + for (auto &[key, type] : keys) { + if (type & key_type) { + remove(key); + } } - return bytes; -} - -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 8da077cfd..589775c3c 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -1,46 +1,76 @@ #pragma once + #include + #include +#include #include -#include #define ERR_NO_VALUE -33 +enum ParamKeyType { + PERSISTENT = 0x02, + CLEAR_ON_MANAGER_START = 0x04, + CLEAR_ON_PANDA_DISCONNECT = 0x08, + CLEAR_ON_IGNITION = 0x10, + ALL = 0x02 | 0x04 | 0x08 | 0x10 +}; + class Params { private: std::string params_path; public: Params(bool persistent_param = false); - Params(std::string path); + Params(const 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); + bool checkKey(const std::string &key); - // 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 + int remove(const char *key); + inline int remove(const std::string &key) { + return remove (key.c_str()); + } + void clearAll(ParamKeyType type); - // 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); + // read all values + int readAll(std::map *params); - // 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); + // helpers for reading values + std::string get(const char *key, bool block = false); - int read_db_all(std::map *params); - std::vector read_db_bytes(const char* param_name); - bool read_db_bool(const char* param_name); + inline std::string get(const std::string &key, bool block = false) { + return get(key.c_str(), block); + } - std::string get(std::string key, bool block=false); + template + std::optional get(const char *key, bool block = false) { + std::istringstream iss(get(key, block)); + T value{}; + iss >> value; + return iss.fail() ? std::nullopt : std::optional(value); + } + + inline bool getBool(const std::string &key) { + return getBool(key.c_str()); + } + + inline bool getBool(const char *key) { + return get(key) == "1"; + } + + // helpers for writing values + int put(const char* key, const char* val, size_t value_size); + + inline int put(const std::string &key, const std::string &val) { + return put(key.c_str(), val.data(), val.size()); + } + + inline int putBool(const char *key, bool val) { + return put(key, val ? "1" : "0", 1); + } + + inline int putBool(const std::string &key, bool val) { + return putBool(key.c_str(), val); + } }; diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc index 5c69c0a94..80d2379d9 100644 --- a/selfdrive/common/swaglog.cc +++ b/selfdrive/common/swaglog.cc @@ -2,22 +2,22 @@ #define _GNU_SOURCE #endif -#include -#include -#include - -#include -#include - -#include "json11.hpp" - -#include "common/util.h" -#include "common/version.h" - #include "swaglog.h" +#include +#include +#include +#include + +#include +#include "json11.hpp" + +#include "selfdrive/common/util.h" +#include "selfdrive/common/version.h" +#include "selfdrive/hardware/hw.h" + class LogState { -public: + public: LogState() = default; ~LogState(); std::mutex lock; @@ -71,9 +71,9 @@ static void cloudlog_init() { s.ctx_j["dirty"] = !getenv("CLEAN"); // device type - if (util::file_exists("/EON")) { + if (Hardware::EON()) { cloudlog_bind_locked("device", "eon"); - } else if (util::file_exists("/TICI")) { + } else if (Hardware::TICI()) { cloudlog_bind_locked("device", "tici"); } else { cloudlog_bind_locked("device", "pc"); @@ -89,8 +89,7 @@ void log(int levelnum, const char* filename, int lineno, const char* func, const printf("%s: %s\n", filename, msg); } char levelnum_c = levelnum; - zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE); - zmq_send(s.sock, log_s.c_str(), log_s.length(), ZMQ_NOBLOCK); + zmq_send(s.sock, (levelnum_c + log_s).c_str(), log_s.length() + 1, ZMQ_NOBLOCK); } void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, diff --git a/selfdrive/common/timing.h b/selfdrive/common/timing.h index 48d031e51..b940cc924 100644 --- a/selfdrive/common/timing.h +++ b/selfdrive/common/timing.h @@ -1,5 +1,4 @@ -#ifndef COMMON_TIMING_H -#define COMMON_TIMING_H +#pragma once #include #include @@ -50,5 +49,3 @@ static inline uint64_t nanos_monotonic_raw() { clock_gettime(CLOCK_MONOTONIC_RAW, &t); return t.tv_sec * 1000000000ULL + t.tv_nsec; } - -#endif diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c index 0ab1e38c0..eef83d8f2 100644 --- a/selfdrive/common/touch.c +++ b/selfdrive/common/touch.c @@ -1,14 +1,14 @@ +#include "selfdrive/common/touch.h" + +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include #include -#include - -#include "touch.h" +#include /* this macro is used to tell if "bit" is set in "array" * it selects a byte from the array, and does a boolean AND diff --git a/selfdrive/common/util.cc b/selfdrive/common/util.cc index a9db1a156..4d366a039 100644 --- a/selfdrive/common/util.cc +++ b/selfdrive/common/util.cc @@ -1,6 +1,8 @@ +#include "selfdrive/common/util.h" + #include -#include "common/util.h" +#include #ifdef __linux__ #include @@ -9,47 +11,7 @@ #define __USE_GNU #endif #include -#endif // __linux__ - -void* read_file(const char* path, size_t* out_len) { - FILE* f = fopen(path, "r"); - if (!f) { - return NULL; - } - fseek(f, 0, SEEK_END); - long f_len = ftell(f); - rewind(f); - - // malloc one extra byte so the file will always be NULL terminated - // cl_cached_program_from_file relies on this - char* buf = (char*)malloc(f_len+1); - assert(buf); - - size_t num_read = fread(buf, f_len, 1, f); - fclose(f); - - if (num_read != 1) { - free(buf); - return NULL; - } - - buf[f_len] = '\0'; - if (out_len) { - *out_len = f_len; - } - - return buf; -} - -int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) { - int fd = open(path, flags, mode); - if (fd == -1) { - return -1; - } - ssize_t n = write(fd, data, size); - close(fd); - return (n >= 0 && (size_t)n == size) ? 0 : -1; -} +#endif // __linux__ void set_thread_name(const char* name) { #ifdef __linux__ @@ -84,3 +46,56 @@ int set_core_affinity(int core) { return -1; #endif } + +namespace util { + +std::string read_file(const std::string& fn) { + std::ifstream ifs(fn, std::ios::binary | std::ios::ate); + if (ifs) { + std::ifstream::pos_type pos = ifs.tellg(); + if (pos != std::ios::beg) { + std::string result; + result.resize(pos); + ifs.seekg(0, std::ios::beg); + ifs.read(result.data(), pos); + if (ifs) { + return result; + } + } + } + ifs.close(); + + // fallback for files created on read, e.g. procfs + std::ifstream f(fn); + std::stringstream buffer; + buffer << f.rdbuf(); + return buffer.str(); +} + +int read_files_in_dir(std::string path, std::map *contents) { + DIR *d = opendir(path.c_str()); + if (!d) return -1; + + struct dirent *de = NULL; + while ((de = readdir(d))) { + if (isalnum(de->d_name[0])) { + (*contents)[de->d_name] = util::read_file(path + "/" + de->d_name); + } + } + + closedir(d); + return 0; +} + +int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) { + int fd = open(path, flags, mode); + if (fd == -1) { + return -1; + } + ssize_t n = write(fd, data, size); + close(fd); + return (n >= 0 && (size_t)n == size) ? 0 : -1; +} + + +} // namespace util diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index b75c2f434..d4d27fb90 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -1,37 +1,26 @@ #pragma once -#include -#include -#include -#include -#include #include +#include +#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #ifndef sighandler_t typedef void (*sighandler_t)(int sig); #endif -#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) - -#undef ALIGN -#define ALIGN(x, align) (((x) + (align)-1) & ~((align)-1)) - -// Reads a file into a newly allocated buffer. -// -// Returns NULL on failure, otherwise the NULL-terminated file contents. -// The result must be freed by the caller. -void* read_file(const char* path, size_t* out_len); -int write_file(const char* path, const void* data, size_t size, int flags=O_WRONLY, mode_t mode=0777); - void set_thread_name(const char* name); int set_realtime_priority(int level); @@ -64,12 +53,11 @@ inline std::string string_format(const std::string& format, Args... args) { return std::string(buf.get(), buf.get() + size - 1); } -inline std::string read_file(const std::string &fn) { - std::ifstream t(fn); - std::stringstream buffer; - buffer << t.rdbuf(); - return buffer.str(); -} +std::string read_file(const std::string &fn); + +int read_files_in_dir(std::string path, std::map *contents); + +int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0777); inline std::string tohex(const uint8_t* buf, size_t buf_size) { std::unique_ptr hexbuf(new char[buf_size*2+1]); @@ -161,3 +149,19 @@ struct unique_fd { operator int() const { return fd_; } int fd_; }; + +class FirstOrderFilter { +public: + FirstOrderFilter(float x0, float ts, float dt) { + k_ = (dt / ts) / (1.0 + dt / ts); + x_ = x0; + } + inline float update(float x) { + x_ = (1. - k_) * x_ + k_ * x; + return x_; + } + inline void reset(float x) { x_ = x; } + +private: + float x_, k_; +}; diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 6646026bc..10379fb4d 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.8.3-9896438d-2021-03-29T23:54:19" +#define COMMA_VERSION "0.8.4-a7b3df5df-2021-05-15T01:20:46" diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc index fdd7d07c7..770e18a10 100644 --- a/selfdrive/common/visionimg.cc +++ b/selfdrive/common/visionimg.cc @@ -1,11 +1,12 @@ +#include "selfdrive/common/visionimg.h" + #include -#include "common/visionimg.h" #ifdef QCOM +#include #include #include #include -#include #define GL_GLEXT_PROTOTYPES #include using namespace android; diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h index 20a476c1a..cf3b36b79 100644 --- a/selfdrive/common/visionimg.h +++ b/selfdrive/common/visionimg.h @@ -3,9 +3,9 @@ #include "visionbuf.h" #ifdef __APPLE__ - #include +#include #else - #include +#include #endif #ifdef QCOM diff --git a/selfdrive/common/watchdog.cc b/selfdrive/common/watchdog.cc index 2e8afb391..fe95e23fd 100644 --- a/selfdrive/common/watchdog.cc +++ b/selfdrive/common/watchdog.cc @@ -1,10 +1,12 @@ -#include -#include +#include "selfdrive/common/watchdog.h" + #include -#include "common/timing.h" -#include "common/util.h" -#include "common/watchdog.h" +#include +#include + +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // + @@ -12,6 +14,6 @@ bool watchdog_kick(){ std::string fn = watchdog_fn_prefix + std::to_string(getpid()); std::string cur_t = std::to_string(nanos_since_boot()); - int r = write_file(fn.c_str(), cur_t.data(), cur_t.length(), O_WRONLY | O_CREAT); + int r = util::write_file(fn.c_str(), cur_t.data(), cur_t.length(), O_WRONLY | O_CREAT); return r == 0; } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b575d6eb9..661d87536 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -45,7 +45,7 @@ EventName = car.CarEvent.EventName class Controls: def __init__(self, sm=None, pm=None, can_sock=None): - config_realtime_process(3, Priority.CTRL_HIGH) + config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH) # Setup sockets self.pm = pm @@ -58,7 +58,8 @@ class Controls: ignore = ['driverCameraState', 'managerState'] if SIMULATION else None self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', - 'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'], ignore_alive=ignore) + 'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'], + ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) self.can_sock = can_sock if can_sock is None: @@ -73,25 +74,29 @@ class Controls: # read params params = Params() - self.is_metric = params.get("IsMetric", encoding='utf8') == "1" - self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" - community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" - openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" - passive = params.get("Passive", encoding='utf8') == "1" or not openpilot_enabled_toggle + self.is_metric = params.get_bool("IsMetric") + self.is_ldw_enabled = params.get_bool("IsLdwEnabled") + self.enable_lte_onroad = params.get_bool("EnableLteOnroad") + community_feature_toggle = params.get_bool("CommunityFeaturesToggle") + openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") + passive = params.get_bool("Passive") or not openpilot_enabled_toggle # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' + fuzzy_fingerprint = self.CP.fuzzyFingerprint + # If stock camera is disconnected, we loaded car controls and it's not dashcam mode controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly - community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle + community_feature = self.CP.communityFeature or fuzzy_fingerprint + community_feature_disallowed = community_feature and (not community_feature_toggle) self.read_only = not car_recognized or not controller_available or \ self.CP.dashcamOnly or community_feature_disallowed if self.read_only: self.CP.safetyModel = car.CarParams.SafetyModel.noOutput - # Write CarParams for radard and boardd safety mode + # Write CarParams for radard cp_bytes = self.CP.to_bytes() params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) @@ -112,6 +117,7 @@ class Controls: elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP) + self.initialized = False self.state = State.disabled self.enabled = False self.active = False @@ -129,14 +135,10 @@ class Controls: self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = False - self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED - self.sm['deviceState'].freeSpacePercent = 100 - self.sm['driverMonitoringState'].events = [] - self.sm['driverMonitoringState'].awarenessStatus = 1. - self.sm['driverMonitoringState'].faceDetected = False + # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True - self.startup_event = get_startup_event(car_recognized, controller_available) + self.startup_event = get_startup_event(car_recognized, controller_available, fuzzy_fingerprint) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) @@ -163,6 +165,11 @@ class Controls: self.events.add(self.startup_event) self.startup_event = None + # Don't add any more events if not initialized + if not self.initialized: + self.events.add(EventName.controlsInitializing) + return + # Create events for battery, temperature, disk space, and memory if self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError: # at zero percent battery, while discharging, OP should not allowed @@ -206,12 +213,11 @@ class Controls: LaneChangeState.laneChangeFinishing]: self.events.add(EventName.laneChange) - if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL): + if self.can_rcv_error or not CS.canValid: self.events.add(EventName.canError) - safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel - safety_mismatch = safety_mismatch or self.sm['pandaState'].safetyParam != self.CP.safetyParam - if (safety_mismatch and self.sm.frame > 2 / DT_CTRL) or self.mismatch_counter >= 200: + safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel or self.sm['pandaState'].safetyParam != self.CP.safetyParam + if safety_mismatch or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) if not self.sm['liveParameters'].valid: @@ -244,10 +250,11 @@ class Controls: # TODO: fix simulator if not SIMULATION: if not NOSENSOR: - if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and not TICI: + if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and \ + (not TICI or self.enable_lte_onroad): # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes self.events.add(EventName.noGps) - if not self.sm.all_alive(['roadCameraState', 'driverCameraState']) and (self.sm.frame > 5 / DT_CTRL): + if not self.sm.all_alive(['roadCameraState', 'driverCameraState']): self.events.add(EventName.cameraMalfunction) if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) @@ -271,6 +278,11 @@ class Controls: self.sm.update(0) + all_valid = CS.canValid and self.sm.all_alive_and_valid() + if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 2.0): + self.initialized = True + Params().put_bool("ControlsReady", True) + # Check for CAN timeout if not can_strs: self.can_error_counter += 1 @@ -478,7 +490,7 @@ class Controls: self.AM.process_alerts(self.sm.frame, clear_event) CC.hudControl.visualAlert = self.AM.visual_alert - if not self.read_only: + if not self.read_only and self.initialized: # send car controls over can can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) @@ -578,7 +590,7 @@ class Controls: self.update_events(CS) - if not self.read_only: + if not self.read_only and self.initialized: # Update control state self.state_transition(CS) self.prof.checkpoint("State transition") diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 79642ccef..f8aaab74e 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -40,5 +40,9 @@ "Offroad_HardwareUnsupported": { "text": "White and grey panda are unsupported. Upgrade to comma two or black panda.", "severity": 0 + }, + "Offroad_UnofficialHardware": { + "text": "Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, contact support@comma.ai.", + "severity": 1 } } diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 12fd6519b..87ea3d6fa 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -171,7 +171,7 @@ class EngagementAlert(Alert): class NormalPermanentAlert(Alert): def __init__(self, alert_text_1, alert_text_2): super().__init__(alert_text_1, alert_text_2, - AlertStatus.normal, AlertSize.mid, + AlertStatus.normal, AlertSize.mid if len(alert_text_2) else AlertSize.small, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), # ********** alert callback functions ********** @@ -208,6 +208,13 @@ def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: boo text = "Main Switch Off" return NoEntryAlert(text, duration_hud_alert=0.) +def startup_fuzzy_fingerprint_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: + return Alert( + "WARNING: No Exact Match on Car Model", + f"Closest Match: {CP.carFingerprint.title()[:40]}", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.) + EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = { # ********** events with no alerts ********** @@ -221,6 +228,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1), }, + EventName.controlsInitializing: { + ET.NO_ENTRY: NoEntryAlert("Controls Initializing"), + }, + EventName.startup: { ET.PERMANENT: Alert( "Be ready to take over at any time", @@ -253,6 +264,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), }, + EventName.startupFuzzyFingerprint: { + ET.PERMANENT: startup_fuzzy_fingerprint_alert, + }, + EventName.dashcamMode: { ET.PERMANENT: Alert( "Dashcam Mode", @@ -272,8 +287,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.communityFeatureDisallowed: { # LOW priority to overcome Cruise Error ET.PERMANENT: Alert( - "Community Feature Detected", - "Enable Community Features in Developer Settings", + "openpilot Not Available", + "Enable Community Features in Settings to Engage", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2), }, @@ -292,6 +307,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo "Stock AEB: Risk of Collision", AlertStatus.critical, AlertSize.full, Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.), + ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"), }, EventName.stockFcw: { @@ -300,6 +316,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo "Stock FCW: Risk of Collision", AlertStatus.critical, AlertSize.full, Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.), + ET.NO_ENTRY: NoEntryAlert("Stock FCW: Risk of Collision"), }, EventName.fcw: { @@ -330,6 +347,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo EventName.vehicleModelInvalid: { ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"), + ET.SOFT_DISABLE: SoftDisableAlert("Vehicle Parameter Identification Failed"), ET.WARNING: Alert( "Vehicle Parameter Identification Failed", "", @@ -337,12 +355,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1), }, - EventName.steerTempUnavailableMute: { + EventName.steerTempUnavailableUserOverride: { ET.WARNING: Alert( - "TAKE CONTROL", "Steering Temporarily Unavailable", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .2, .2, .2), + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 1., 1.), }, EventName.preDriverDistracted: { @@ -518,11 +536,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo }, EventName.steerTempUnavailable: { - ET.WARNING: Alert( - "TAKE CONTROL", - "Steering Temporarily Unavailable", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 1.), + ET.SOFT_DISABLE: SoftDisableAlert("Steering Temporarily Unavailable"), ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable", duration_hud_alert=0.), }, @@ -648,9 +662,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo audible_alert=AudibleAlert.chimeDisengage), }, - EventName.controlsFailed: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Failed"), - ET.NO_ENTRY: NoEntryAlert("Controls Failed"), + EventName.accFaulted: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Faulted"), + ET.PERMANENT: NormalPermanentAlert("Cruise Faulted", ""), + ET.NO_ENTRY: NoEntryAlert("Cruise Faulted"), }, EventName.controlsMismatch: { @@ -734,7 +749,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo "Speed Too High", "Model uncertain at this speed", AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.none, 2.2, 3., 4.), + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), ET.NO_ENTRY: Alert( "Speed Too High", "Slow down to engage", diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 200ab5d3a..79f23294b 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -17,9 +17,8 @@ else: PATH_OFFSET = 0.0 - class LanePlanner: - def __init__(self): + def __init__(self, wide_camera=False): self.ll_t = np.zeros((TRAJECTORY_SIZE,)) self.ll_x = np.zeros((TRAJECTORY_SIZE,)) self.lll_y = np.zeros((TRAJECTORY_SIZE,)) @@ -38,6 +37,8 @@ class LanePlanner: self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. + self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET + self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET def parse_model(self, md): if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE: @@ -45,8 +46,8 @@ class LanePlanner: # left and right ll x is the same self.ll_x = md.laneLines[1].x # only offset left and right lane lines; offsetting path does not make sense - self.lll_y = np.array(md.laneLines[1].y) - CAMERA_OFFSET - self.rll_y = np.array(md.laneLines[2].y) - CAMERA_OFFSET + self.lll_y = np.array(md.laneLines[1].y) - self.camera_offset + self.rll_y = np.array(md.laneLines[2].y) - self.camera_offset self.lll_prob = md.laneLineProbs[1] self.rll_prob = md.laneLineProbs[2] self.lll_std = md.laneLineStds[1] @@ -59,7 +60,7 @@ class LanePlanner: def get_d_path(self, v_ego, path_t, path_xyz): # Reduce reliance on lanelines that are too far apart or # will be in a few seconds - path_xyz[:,1] -= PATH_OFFSET + path_xyz[:, 1] -= self.path_offset l_prob, r_prob = self.lll_prob, self.rll_prob width_pts = self.rll_y - self.lll_y prob_mods = [] diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index ca4be4c25..171e7231c 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -1,6 +1,6 @@ #include #include -#include "common/modeldata.h" +#include "selfdrive/common/modeldata.h" #define deg2rad(d) (d/180.0*M_PI) diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index 8da44f4f1..6765fe57b 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -29,7 +29,7 @@ typedef struct { double cost; } log_t; -void init_weights(double pathCost, double headingCost, double steerRateCost){ +void set_weights(double pathCost, double headingCost, double steerRateCost){ int i; const int STEP_MULTIPLIER = 3.0; @@ -44,7 +44,7 @@ void init_weights(double pathCost, double headingCost, double steerRateCost){ acadoVariables.WN[(NYN+1)*1] = headingCost * STEP_MULTIPLIER; } -void init(double pathCost, double headingCost, double steerRateCost){ +void init(){ acado_initializeSolver(); int i; @@ -58,8 +58,6 @@ void init(double pathCost, double headingCost, double steerRateCost){ /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; - - init_weights(pathCost, headingCost, steerRateCost); } int run_mpc(state_t * x0, log_t * solution, double v_ego, diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 7ef94fc59..2ca3db3a2 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -22,8 +22,8 @@ typedef struct { double cost; } log_t; -void init(double pathCost, double headingCost, double steerRateCost); -void init_weights(double pathCost, double headingCost, double steerRateCost); +void init(); +void set_weights(double pathCost, double headingCost, double steerRateCost); int run_mpc(state_t * x0, log_t * solution, double v_ego, double rotation_radius, double target_y[N+1], double target_psi[N+1]); diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 3b7772df0..71913636e 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -1,7 +1,6 @@ import os import math import numpy as np -from common.params import Params from common.realtime import sec_since_boot, DT_MDL from common.numpy_fast import interp, clip from selfdrive.swaglog import cloudlog @@ -17,7 +16,7 @@ LaneChangeDirection = log.LateralPlan.LaneChangeDirection LOG_MPC = os.environ.get('LOG_MPC', False) -LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS +LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. # this corresponds to 80deg/s and 20deg/s steering angle in a toyota corolla MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992] @@ -46,15 +45,15 @@ DESIRES = { class LateralPlanner(): - def __init__(self, CP): - self.LP = LanePlanner() + def __init__(self, CP, use_lanelines=True, wide_camera=False): + self.use_lanelines = use_lanelines + self.LP = LanePlanner(wide_camera) self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost self.setup_mpc() self.solution_invalid_cnt = 0 - self.use_lanelines = Params().get('EndToEndToggle') != b'1' self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 @@ -63,13 +62,14 @@ class LateralPlanner(): self.desire = log.LateralPlan.Desire.none self.path_xyz = np.zeros((TRAJECTORY_SIZE,3)) + self.path_xyz_stds = np.ones((TRAJECTORY_SIZE,3)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) self.y_pts = np.zeros(TRAJECTORY_SIZE) def setup_mpc(self): self.libmpc = libmpc_py.libmpc - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) + self.libmpc.init() self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -94,16 +94,13 @@ class LateralPlanner(): self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) self.plan_yaw = list(md.orientation.z) + if len(md.orientation.xStd) == TRAJECTORY_SIZE: + self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN - if sm['carState'].leftBlinker: - self.lane_change_direction = LaneChangeDirection.left - elif sm['carState'].rightBlinker: - self.lane_change_direction = LaneChangeDirection.right - if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none @@ -120,6 +117,11 @@ class LateralPlanner(): # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + if sm['carState'].leftBlinker: + self.lane_change_direction = LaneChangeDirection.left + elif sm['carState'].rightBlinker: + self.lane_change_direction = LaneChangeDirection.right + self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 @@ -162,8 +164,13 @@ class LateralPlanner(): self.LP.rll_prob *= self.lane_change_ll_prob if self.use_lanelines: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) + self.libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) else: d_path_xyz = self.path_xyz + path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0) * MPC_COST_LAT.PATH + # Heading cost is useful at low speed, otherwise end of plan can be off-heading + heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0]) + self.libmpc.set_weights(path_cost, heading_cost, CP.steerRateCost) y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) self.y_pts = y_pts @@ -207,7 +214,7 @@ class LateralPlanner(): mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) t = sec_since_boot() if mpc_nans: - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) + self.libmpc.init() self.cur_state.curvature = measured_curvature if t > self.last_cloudlog_t + 5.0: diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ceed00c73..a2c650fa1 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -5,23 +5,27 @@ from common.realtime import Priority, config_realtime_process from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.longitudinal_planner import Planner from selfdrive.controls.lib.lateral_planner import LateralPlanner +from selfdrive.hardware import TICI import cereal.messaging as messaging def plannerd_thread(sm=None, pm=None): - - config_realtime_process(2, Priority.CTRL_LOW) + config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") - CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + params = Params() + CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) + use_lanelines = not params.get_bool('EndToEndToggle') + wide_camera = params.get_bool('EnableWideCamera') if TICI else False + longitudinal_planner = Planner(CP) - lateral_planner = LateralPlanner(CP) + lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'], - poll=['radarState', 'modelV2']) + poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) if pm is None: pm = messaging.PubMaster(['longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc']) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 8f23d9d73..244d02d23 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -12,6 +12,7 @@ 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 from selfdrive.swaglog import cloudlog +from selfdrive.hardware import TICI class KalmanParams(): @@ -173,7 +174,7 @@ class RadarD(): # fuses camera and radar data for best lead detection def radard_thread(sm=None, pm=None, can_sock=None): - config_realtime_process(2, Priority.CTRL_LOW) + config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") @@ -188,7 +189,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: - sm = messaging.SubMaster(['modelV2', 'carState']) + sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 3bc000d2e..b6faa23af 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -1,77 +1,27 @@ """Install exception handler for process crash.""" -import os -import sys -import threading -import capnp -from selfdrive.version import version, dirty, origin, branch - -from selfdrive.hardware import PC from selfdrive.swaglog import cloudlog +from selfdrive.version import version -if os.getenv("NOLOG") or os.getenv("NOCRASH") or PC: - def capture_exception(*args, **kwargs): - pass +import sentry_sdk +from sentry_sdk.integrations.threading import ThreadingIntegration - def bind_user(**kwargs): - pass +def capture_exception(*args, **kwargs): + cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) - def bind_extra(**kwargs): - pass + try: + sentry_sdk.capture_exception(*args, **kwargs) + sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 + except Exception: + cloudlog.exception("sentry exception") - def install(): - pass -else: - from raven import Client - from raven.transport.http import HTTPTransport +def bind_user(**kwargs): + sentry_sdk.set_user(kwargs) - tags = { - 'dirty': dirty, - 'origin': origin, - 'branch': branch - } - client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924', - install_sys_hook=False, transport=HTTPTransport, release=version, tags=tags) +def bind_extra(**kwargs): + for k, v in kwargs.items(): + sentry_sdk.set_tag(k, v) - def capture_exception(*args, **kwargs): - exc_info = sys.exc_info() - if not exc_info[0] is capnp.lib.capnp.KjException: - client.captureException(*args, **kwargs) - cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) - - def bind_user(**kwargs): - client.user_context(kwargs) - - def bind_extra(**kwargs): - client.extra_context(kwargs) - - def install(): - """ - Workaround for `sys.excepthook` thread bug from: - http://bugs.python.org/issue1230540 - Call once from the main thread before creating any threads. - Source: https://stackoverflow.com/a/31622038 - """ - # installs a sys.excepthook - __excepthook__ = sys.excepthook - - def handle_exception(*exc_info): - if exc_info[0] not in (KeyboardInterrupt, SystemExit): - capture_exception() - __excepthook__(*exc_info) - sys.excepthook = handle_exception - - init_original = threading.Thread.__init__ - - def init(self, *args, **kwargs): - init_original(self, *args, **kwargs) - run_original = self.run - - def run_with_except_hook(*args2, **kwargs2): - try: - run_original(*args2, **kwargs2) - except Exception: - sys.excepthook(*sys.exc_info()) - - self.run = run_with_except_hook - - threading.Thread.__init__ = init +def init(): + sentry_sdk.init("https://a8dc76b5bfb34908a601d67e2aa8bcf9@o33823.ingest.sentry.io/77924", + default_integrations=False, integrations=[ThreadingIntegration(propagate_hub=True)], + release=version) diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py new file mode 100755 index 000000000..732c1af1c --- /dev/null +++ b/selfdrive/debug/count_events.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +import sys +from collections import Counter +from pprint import pprint +from tqdm import tqdm + +from tools.lib.route import Route +from tools.lib.logreader import LogReader + +if __name__ == "__main__": + r = Route(sys.argv[1]) + + cnt: Counter = Counter() + for q in tqdm(r.qlog_paths()): + lr = LogReader(q) + car_events = [m for m in lr if m.which() == 'carEvents'] + for car_event in car_events: + for e in car_event.carEvents: + cnt[e.name] += 1 + pprint(cnt) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index fa7c266c3..ffff3d6ce 100755 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -31,9 +31,6 @@ PRINT_INTERVAL = 5 SLEEP_INTERVAL = 0.2 monitored_proc_names = [ - # offroad APK - 'ai.comma.plus.offroad', - # android procs 'SurfaceFlinger', 'sensors.qcom' ] + list(managed_processes.keys()) diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 9999d8657..98e7eafec 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -5,17 +5,22 @@ import time +from cereal import car import cereal.messaging as messaging from selfdrive.car.honda.interface import CarInterface from selfdrive.controls.lib.events import ET, EVENTS, Events from selfdrive.controls.lib.alertmanager import AlertManager +EventName = car.CarEvent.EventName -def cycle_alerts(duration=200, is_metric=False): +def cycle_alerts(duration=2000, is_metric=False): alerts = list(EVENTS.keys()) print(alerts) - CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") + #alerts = [EventName.preDriverDistracted, EventName.promptDriverDistracted, EventName.driverDistracted] + alerts = [EventName.preLaneChangeLeft, EventName.preLaneChangeRight] + + CP = CarInterface.get_params("HONDA CIVIC 2016") sm = messaging.SubMaster(['deviceState', 'pandaState', 'roadCameraState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman']) diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 908b129c7..ce96b360f 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -38,7 +38,7 @@ if __name__ == "__main__": values = [s.strip().split(".") for s in args.values.split(",")] while 1: - polld = poller.poll(1000) + polld = poller.poll(100) for sock in polld: msg = sock.receive() evt = log.Event.from_bytes(msg) diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py index 4ca69f5e9..118a980ee 100755 --- a/selfdrive/debug/filter_log_message.py +++ b/selfdrive/debug/filter_log_message.py @@ -49,7 +49,7 @@ if __name__ == "__main__": logs = None if len(args.route): r = Route(args.route[0]) - logs = r.log_paths() # TODO: switch to qlogs when logs are in there + logs = r.qlog_paths() if len(args.route) == 2 and logs: n = int(args.route[1]) diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index 1c7f0077e..5a3ccc957 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -7,14 +7,25 @@ import os import traceback from tqdm import tqdm from tools.lib.logreader import LogReader -from selfdrive.car.fw_versions import match_fw_to_car +from tools.lib.route import Route +from selfdrive.car.fw_versions import match_fw_to_car_exact, match_fw_to_car_fuzzy, build_fw_dict from selfdrive.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS from selfdrive.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS from selfdrive.car.hyundai.values import FW_VERSIONS as HYUNDAI_FW_VERSIONS +from selfdrive.car.volkswagen.values import FW_VERSIONS as VW_FW_VERSIONS from selfdrive.car.toyota.values import FINGERPRINTS as TOYOTA_FINGERPRINTS from selfdrive.car.honda.values import FINGERPRINTS as HONDA_FINGERPRINTS from selfdrive.car.hyundai.values import FINGERPRINTS as HYUNDAI_FINGERPRINTS +from selfdrive.car.volkswagen.values import FINGERPRINTS as VW_FINGERPRINTS + +NO_API = "NO_API" in os.environ +SUPPORTED_CARS = list(TOYOTA_FINGERPRINTS.keys()) + list(HONDA_FINGERPRINTS.keys()) + list(HYUNDAI_FINGERPRINTS.keys())+ list(VW_FINGERPRINTS.keys()) + +try: + from xx.pipeline.c.CarState import migration +except ImportError: + migration = {} if __name__ == "__main__": @@ -30,25 +41,37 @@ if __name__ == "__main__": mismatches = defaultdict(list) - wrong = 0 - good = 0 + not_fingerprinted = 0 + solved_by_fuzzy = 0 + + good_exact = 0 + wrong_fuzzy = 0 + good_fuzzy = 0 dongles = [] for route in tqdm(routes): route = route.rstrip() dongle_id, time = route.split('|') - qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2" if dongle_id in dongles: continue + if NO_API: + qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2" + else: + route = Route(route) + qlog_path = route.qlog_paths()[0] + + if qlog_path is None: + continue + try: lr = LogReader(qlog_path) + dongles.append(dongle_id) for msg in lr: if msg.which() == "pandaState": - if msg.pandaState.pandaType not in ['uno', 'blackPanda']: - dongles.append(dongle_id) + if msg.pandaState.pandaType not in ['uno', 'blackPanda', 'dos']: break elif msg.which() == "carParams": @@ -58,24 +81,37 @@ if __name__ == "__main__": if len(car_fw) == 0: break - dongles.append(dongle_id) live_fingerprint = msg.carParams.carFingerprint + live_fingerprint = migration.get(live_fingerprint, live_fingerprint) if args.car is not None: live_fingerprint = args.car - if live_fingerprint not in list(TOYOTA_FINGERPRINTS.keys()) + list(HONDA_FINGERPRINTS.keys()) + list(HYUNDAI_FINGERPRINTS.keys()): + if live_fingerprint not in SUPPORTED_CARS: break - candidates = match_fw_to_car(car_fw) - if (len(candidates) == 1) and (list(candidates)[0] == live_fingerprint): - good += 1 - print("Correct", live_fingerprint, dongle_id) + fw_versions_dict = build_fw_dict(car_fw) + exact_matches = match_fw_to_car_exact(fw_versions_dict) + fuzzy_matches = match_fw_to_car_fuzzy(fw_versions_dict) + + if (len(exact_matches) == 1) and (list(exact_matches)[0] == live_fingerprint): + good_exact += 1 + print(f"Correct! Live: {live_fingerprint} - Fuzzy: {fuzzy_matches}") + + # Check if fuzzy match was correct + if len(fuzzy_matches) == 1: + if list(fuzzy_matches)[0] != live_fingerprint: + wrong_fuzzy += 1 + print(f"{dongle_id}|{time}") + print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint) + else: + good_fuzzy += 1 break print(f"{dongle_id}|{time}") print("Old style:", live_fingerprint, "Vin", msg.carParams.carVin) - print("New style:", candidates) + print("New style (exact):", exact_matches) + print("New style (fuzzy):", fuzzy_matches) for version in car_fw: subaddr = None if version.subAddress == 0 else hex(version.subAddress) @@ -83,7 +119,7 @@ if __name__ == "__main__": print("Mismatches") found = False - for car_fws in [TOYOTA_FW_VERSIONS, HONDA_FW_VERSIONS, HYUNDAI_FW_VERSIONS]: + for car_fws in [TOYOTA_FW_VERSIONS, HONDA_FW_VERSIONS, HYUNDAI_FW_VERSIONS, VW_FW_VERSIONS]: if live_fingerprint in car_fws: found = True expected = car_fws[live_fingerprint] @@ -111,19 +147,24 @@ if __name__ == "__main__": mismatches[live_fingerprint].append(mismatch) print() - wrong += 1 + not_fingerprinted += 1 + + if len(fuzzy_matches) == 1: + if list(fuzzy_matches)[0] == live_fingerprint: + solved_by_fuzzy += 1 + else: + wrong_fuzzy += 1 + print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint) + break except Exception: traceback.print_exc() except KeyboardInterrupt: break - print(f"Fingerprinted: {good} - Not fingerprinted: {wrong}") - print(f"Number of dongle ids checked: {len(dongles)}") print() - # Print FW versions that need to be added seperated out by car and address - for car, m in mismatches.items(): + for car, m in sorted(mismatches.items()): print(car) addrs = defaultdict(list) for (addr, sub_addr, version) in m: @@ -135,3 +176,15 @@ if __name__ == "__main__": print(f" {v},") print(" ]") print() + + print() + print(f"Number of dongle ids checked: {len(dongles)}") + print(f"Fingerprinted: {good_exact}") + print(f"Not fingerprinted: {not_fingerprinted}") + print(f" of which had a fuzzy match: {solved_by_fuzzy}") + + print() + print(f"Correct fuzzy matches: {good_fuzzy}") + print(f"Wrong fuzzy matches: {wrong_fuzzy}") + print() + diff --git a/selfdrive/hardware/base.h b/selfdrive/hardware/base.h index c82610d93..d9ce5c48c 100644 --- a/selfdrive/hardware/base.h +++ b/selfdrive/hardware/base.h @@ -3,20 +3,23 @@ #include #include - // no-op base hw class class HardwareNone { public: static constexpr float MAX_VOLUME = 0; static constexpr float MIN_VOLUME = 0; - static std::string get_os_version() { return "openpilot for PC"; }; + static std::string get_os_version() { return ""; } - static void reboot() {}; - static void poweroff() {}; - static void set_brightness(int percent) {}; - static void set_display_power(bool on) {}; + static void reboot() {} + static void poweroff() {} + static void set_brightness(int percent) {} + static void set_display_power(bool on) {} - static bool get_ssh_enabled() { return false; }; - static void set_ssh_enabled(bool enabled) {}; + static bool get_ssh_enabled() { return false; } + static void set_ssh_enabled(bool enabled) {} + + static bool PC() { return false; } + static bool EON() { return false; } + static bool TICI() { return false; } }; diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py index 14a1a6f81..edfd02aad 100644 --- a/selfdrive/hardware/base.py +++ b/selfdrive/hardware/base.py @@ -105,3 +105,7 @@ class HardwareBase: @abstractmethod def set_screen_brightness(self, percentage): pass + + @abstractmethod + def set_power_save(self, enabled): + pass diff --git a/selfdrive/hardware/eon/hardware.h b/selfdrive/hardware/eon/hardware.h index bcf99a628..afd1f77be 100644 --- a/selfdrive/hardware/eon/hardware.h +++ b/selfdrive/hardware/eon/hardware.h @@ -15,6 +15,7 @@ public: static constexpr float MAX_VOLUME = 1.0; static constexpr float MIN_VOLUME = 0.5; + static bool EON() { return true; } static std::string get_os_version() { return "NEOS " + util::read_file("/VERSION"); }; @@ -47,6 +48,13 @@ public: int ret = std::system("dumpsys SurfaceFlinger --list | grep -Fq 'com.android.settings'"); launched_activity = ret == 0; } + + static void close_activities() { + if(launched_activity){ + std::system("pm disable com.android.settings && pm enable com.android.settings"); + } + } + static void launch_activity(std::string activity, std::string opts = "") { if (!launched_activity) { std::string cmd = "am start -n " + activity + " " + opts + diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py index 2ef2a8f47..4f9a260cd 100644 --- a/selfdrive/hardware/eon/hardware.py +++ b/selfdrive/hardware/eon/hardware.py @@ -81,7 +81,7 @@ class Android(HardwareBase): def get_serial(self): ret = getprop("ro.serialno") - if ret == "": + if len(ret) == 0: ret = "cccccccc" return ret @@ -352,3 +352,6 @@ class Android(HardwareBase): def set_screen_brightness(self, percentage): with open("/sys/class/leds/lcd-backlight/brightness", "w") as f: f.write(str(int(percentage * 2.55))) + + def set_power_save(self, enabled): + pass diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h index 0e0980f15..8112ba1c6 100644 --- a/selfdrive/hardware/hw.h +++ b/selfdrive/hardware/hw.h @@ -9,5 +9,10 @@ #include "selfdrive/hardware/tici/hardware.h" #define Hardware HardwareTici #else -#define Hardware HardwareNone +class HardwarePC : public HardwareNone { +public: + static std::string get_os_version() { return "openpilot for PC"; } + static bool PC() { return true; } +}; +#define Hardware HardwarePC #endif diff --git a/selfdrive/hardware/pc/hardware.py b/selfdrive/hardware/pc/hardware.py index 7ee40bd6b..edbc4e07b 100644 --- a/selfdrive/hardware/pc/hardware.py +++ b/selfdrive/hardware/pc/hardware.py @@ -79,3 +79,6 @@ class Pc(HardwareBase): def set_screen_brightness(self, percentage): pass + + def set_power_save(self, enabled): + pass diff --git a/selfdrive/hardware/tici/agnos.json b/selfdrive/hardware/tici/agnos.json index 1fa725b97..216b3b661 100644 --- a/selfdrive/hardware/tici/agnos.json +++ b/selfdrive/hardware/tici/agnos.json @@ -1,18 +1,18 @@ [ { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate-staging/system-990a4362f863f860d70440016005b434bcd17130547deafed96c720d0fc8495e.img.xz", - "hash": "f30370eda7253029ec1e4322fbb060ab69f541e4d5176c41541df608183cee4b", - "hash_raw": "990a4362f863f860d70440016005b434bcd17130547deafed96c720d0fc8495e", + "url": "https://commadist.azureedge.net/agnosupdate-staging/system-53d8287459de23ee65d00f004abd4a13aabdec39ff8d692e33ef4fb274b18439.img.xz", + "hash": "2d31d71fc8571de26fcb75e7ddd270ac60200bd9ab321bf03bee47bf13822b08", + "hash_raw": "53d8287459de23ee65d00f004abd4a13aabdec39ff8d692e33ef4fb274b18439", "size": 10737418240, "sparse": true }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate-staging/boot-b8aebb538f030bbd8e69c227bee33f78ac3be0affe55dc76ab7e87e3f030eceb.img.xz", - "hash": "b8aebb538f030bbd8e69c227bee33f78ac3be0affe55dc76ab7e87e3f030eceb", - "hash_raw": "b8aebb538f030bbd8e69c227bee33f78ac3be0affe55dc76ab7e87e3f030eceb", - "size": 14768128, + "url": "https://commadist.azureedge.net/agnosupdate-staging/boot-60440a9427db1f255281b78e47dff79bb6b039f4cb99f5600fd9d962f6b2b360.img.xz", + "hash": "60440a9427db1f255281b78e47dff79bb6b039f4cb99f5600fd9d962f6b2b360", + "hash_raw": "60440a9427db1f255281b78e47dff79bb6b039f4cb99f5600fd9d962f6b2b360", + "size": 14772224, "sparse": false } ] diff --git a/selfdrive/hardware/tici/hardware.h b/selfdrive/hardware/tici/hardware.h index 034e01a86..0ff73b840 100644 --- a/selfdrive/hardware/tici/hardware.h +++ b/selfdrive/hardware/tici/hardware.h @@ -3,15 +3,15 @@ #include #include -#include "selfdrive/common/util.h" #include "selfdrive/common/params.h" +#include "selfdrive/common/util.h" #include "selfdrive/hardware/base.h" class HardwareTici : public HardwareNone { public: static constexpr float MAX_VOLUME = 0.5; static constexpr float MIN_VOLUME = 0.4; - + static bool TICI() { return true; } static std::string get_os_version() { return "AGNOS " + util::read_file("/VERSION"); }; @@ -27,6 +27,6 @@ public: }; static void set_display_power(bool on) {}; - static bool get_ssh_enabled() { return Params().read_db_bool("SshEnabled"); }; - static void set_ssh_enabled(bool enabled) { Params().write_db_value("SshEnabled", (enabled ? "1" : "0")); }; + static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }; + static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }; }; diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py index bdf2170c2..f277944aa 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/selfdrive/hardware/tici/hardware.py @@ -1,6 +1,7 @@ import os import subprocess from pathlib import Path +from smbus2 import SMBus from cereal import log from selfdrive.hardware.base import HardwareBase, ThermalConfig @@ -27,6 +28,14 @@ NetworkStrength = log.DeviceState.NetworkStrength MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5 MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14 +AMP_I2C_BUS = 0 +AMP_ADDRESS = 0x10 + +def write_amplifier_reg(reg, val, offset, mask): + with SMBus(AMP_I2C_BUS) as bus: + v = bus.read_byte_data(AMP_ADDRESS, reg, force=True) + v = (v & (~mask)) | ((val << offset) & mask) + bus.write_byte_data(AMP_ADDRESS, reg, v, force=True) class Tici(HardwareBase): def __init__(self): @@ -191,3 +200,17 @@ class Tici(HardwareBase): f.write(str(int(percentage * 10.23))) except Exception: pass + + def set_power_save(self, enabled): + # amplifier, 100mW at idle + write_amplifier_reg(0x51, 0b0 if enabled else 0b1, 7, 0b10000000) + + # offline big cluster, leave core 4 online for boardd + for i in range(5, 8): + # TODO: fix permissions with udev + val = "0" if enabled else "1" + os.system(f"sudo su -c 'echo {val} > /sys/devices/system/cpu/cpu{i}/online'") + +if __name__ == "__main__": + import sys + Tici().set_power_save(bool(int(sys.argv[1]))) diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 6ea757462..526890278 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -1,4 +1,5 @@ ubloxd ubloxd_test params_learner -paramsd \ No newline at end of file +paramsd +locationd diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 43e5d1fec..4b7fba19b 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,8 +1,22 @@ -Import('env', 'common', 'cereal', 'messaging') +Import('env', 'common', 'cereal', 'messaging', 'libkf', 'transformations') -loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread'] +loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread'] -env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs) +if GetOption('kaitai'): + generated = Dir('generated').srcnode().abspath + cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" + env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd) + env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) -if GetOption("test"): - env.Program("ubloxd_test", ["ubloxd_test.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs) +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs) + +ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") +locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] +lenv = env.Clone() +lenv["_LIBFLAGS"] += f' {libkf[0].get_labspath()}' +locationd = lenv.Program("locationd", locationd_sources, LIBS=loc_libs + transformations) +lenv.Depends(locationd, libkf) + +if File("liblocationd.cc").exists(): + liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations) + lenv.Depends(liblocationd, libkf) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index f9b857fbc..e8e4f32df 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -13,6 +13,7 @@ import json import numpy as np import cereal.messaging as messaging from cereal import car, log +from selfdrive.hardware import TICI from common.params import Params, put_nonblocking from common.transformations.model import model_height from common.transformations.camera import get_view_frame_from_road_frame @@ -63,7 +64,7 @@ class Calibrator(): # Read saved calibration params = Params() calibration_params = params.get("CalibrationParams") - + self.wide_camera = TICI and params.get_bool('EnableWideCamera') rpy_init = RPY_INIT valid_blocks = 0 @@ -80,7 +81,7 @@ class Calibrator(): rpy_init = list(msg.liveCalibration.rpyCalib) valid_blocks = msg.liveCalibration.validBlocks except (ValueError, capnp.lib.capnp.KjException): - # TODO: remove this when offroad can read capnp + # TODO: remove this after next release calibration_params = json.loads(calibration_params) rpy_init = calibration_params["calib_radians"] valid_blocks = calibration_params['valid_blocks'] @@ -134,10 +135,7 @@ class Calibrator(): write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: - # TODO: change to raw bytes when offroad can read capnp - cal_params = {"calib_radians": list(self.rpy), - "valid_blocks": int(self.valid_blocks)} - put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) + put_nonblocking("CalibrationParams", self.get_msg().to_bytes()) def handle_v_ego(self, v_ego): self.v_ego = v_ego @@ -152,9 +150,12 @@ class Calibrator(): self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) - certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or + if self.wide_camera: + angle_std_threshold = 4*MAX_VEL_ANGLE_STD + else: + angle_std_threshold = MAX_VEL_ANGLE_STD + certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or (self.valid_blocks < INPUTS_NEEDED)) - if not (straight_and_fast and certain_if_calib): return None diff --git a/selfdrive/locationd/generated/gps.cpp b/selfdrive/locationd/generated/gps.cpp new file mode 100644 index 000000000..9b020735b --- /dev/null +++ b/selfdrive/locationd/generated/gps.cpp @@ -0,0 +1,325 @@ +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "gps.h" +#include "kaitai/exceptions.h" + +gps_t::gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = this; + m_tlm = 0; + m_how = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::_read() { + m_tlm = new tlm_t(m__io, this, m__root); + m_how = new how_t(m__io, this, m__root); + n_body = true; + switch (how()->subframe_id()) { + case 1: { + n_body = false; + m_body = new subframe_1_t(m__io, this, m__root); + break; + } + case 2: { + n_body = false; + m_body = new subframe_2_t(m__io, this, m__root); + break; + } + case 3: { + n_body = false; + m_body = new subframe_3_t(m__io, this, m__root); + break; + } + case 4: { + n_body = false; + m_body = new subframe_4_t(m__io, this, m__root); + break; + } + } +} + +gps_t::~gps_t() { + _clean_up(); +} + +void gps_t::_clean_up() { + if (m_tlm) { + delete m_tlm; m_tlm = 0; + } + if (m_how) { + delete m_how; m_how = 0; + } + if (!n_body) { + if (m_body) { + delete m_body; m_body = 0; + } + } +} + +gps_t::subframe_1_t::subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_af_0 = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_1_t::_read() { + m_week_no = m__io->read_bits_int_be(10); + m_code = m__io->read_bits_int_be(2); + m_sv_accuracy = m__io->read_bits_int_be(4); + m_sv_health = m__io->read_bits_int_be(6); + m_iodc_msb = m__io->read_bits_int_be(2); + m_l2_p_data_flag = m__io->read_bits_int_be(1); + m_reserved1 = m__io->read_bits_int_be(23); + m_reserved2 = m__io->read_bits_int_be(24); + m_reserved3 = m__io->read_bits_int_be(24); + m_reserved4 = m__io->read_bits_int_be(16); + m__io->align_to_byte(); + m_t_gd = m__io->read_s1(); + m_iodc_lsb = m__io->read_u1(); + m_t_oc = m__io->read_u2be(); + m_af_2 = m__io->read_s1(); + m_af_1 = m__io->read_s2be(); + m_af_0_sign = m__io->read_bits_int_be(1); + m_af_0_value = m__io->read_bits_int_be(21); + m_reserved5 = m__io->read_bits_int_be(2); +} + +gps_t::subframe_1_t::~subframe_1_t() { + _clean_up(); +} + +void gps_t::subframe_1_t::_clean_up() { +} + +int32_t gps_t::subframe_1_t::af_0() { + if (f_af_0) + return m_af_0; + m_af_0 = ((af_0_sign()) ? ((af_0_value() - (1 << 21))) : (af_0_value())); + f_af_0 = true; + return m_af_0; +} + +gps_t::subframe_3_t::subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_omega_dot = false; + f_idot = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_3_t::_read() { + m_c_ic = m__io->read_s2be(); + m_omega_0 = m__io->read_s4be(); + m_c_is = m__io->read_s2be(); + m_i_0 = m__io->read_s4be(); + m_c_rc = m__io->read_s2be(); + m_omega = m__io->read_s4be(); + m_omega_dot_sign = m__io->read_bits_int_be(1); + m_omega_dot_value = m__io->read_bits_int_be(23); + m__io->align_to_byte(); + m_iode = m__io->read_u1(); + m_idot_sign = m__io->read_bits_int_be(1); + m_idot_value = m__io->read_bits_int_be(13); + m_reserved = m__io->read_bits_int_be(2); +} + +gps_t::subframe_3_t::~subframe_3_t() { + _clean_up(); +} + +void gps_t::subframe_3_t::_clean_up() { +} + +int32_t gps_t::subframe_3_t::omega_dot() { + if (f_omega_dot) + return m_omega_dot; + m_omega_dot = ((omega_dot_sign()) ? ((omega_dot_value() - (1 << 23))) : (omega_dot_value())); + f_omega_dot = true; + return m_omega_dot; +} + +int32_t gps_t::subframe_3_t::idot() { + if (f_idot) + return m_idot; + m_idot = ((idot_sign()) ? ((idot_value() - (1 << 13))) : (idot_value())); + f_idot = true; + return m_idot; +} + +gps_t::subframe_4_t::subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_4_t::_read() { + m_data_id = m__io->read_bits_int_be(2); + m_page_id = m__io->read_bits_int_be(6); + m__io->align_to_byte(); + n_body = true; + switch (page_id()) { + case 56: { + n_body = false; + m_body = new ionosphere_data_t(m__io, this, m__root); + break; + } + } +} + +gps_t::subframe_4_t::~subframe_4_t() { + _clean_up(); +} + +void gps_t::subframe_4_t::_clean_up() { + if (!n_body) { + if (m_body) { + delete m_body; m_body = 0; + } + } +} + +gps_t::subframe_4_t::ionosphere_data_t::ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_4_t::ionosphere_data_t::_read() { + m_a0 = m__io->read_s1(); + m_a1 = m__io->read_s1(); + m_a2 = m__io->read_s1(); + m_a3 = m__io->read_s1(); + m_b0 = m__io->read_s1(); + m_b1 = m__io->read_s1(); + m_b2 = m__io->read_s1(); + m_b3 = m__io->read_s1(); +} + +gps_t::subframe_4_t::ionosphere_data_t::~ionosphere_data_t() { + _clean_up(); +} + +void gps_t::subframe_4_t::ionosphere_data_t::_clean_up() { +} + +gps_t::how_t::how_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::how_t::_read() { + m_tow_count = m__io->read_bits_int_be(17); + m_alert = m__io->read_bits_int_be(1); + m_anti_spoof = m__io->read_bits_int_be(1); + m_subframe_id = m__io->read_bits_int_be(3); + m_reserved = m__io->read_bits_int_be(2); +} + +gps_t::how_t::~how_t() { + _clean_up(); +} + +void gps_t::how_t::_clean_up() { +} + +gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::tlm_t::_read() { + m_magic = m__io->read_bytes(1); + if (!(magic() == std::string("\x8B", 1))) { + throw kaitai::validation_not_equal_error(std::string("\x8B", 1), magic(), _io(), std::string("/types/tlm/seq/0")); + } + m_tlm = m__io->read_bits_int_be(14); + m_integrity_status = m__io->read_bits_int_be(1); + m_reserved = m__io->read_bits_int_be(1); +} + +gps_t::tlm_t::~tlm_t() { + _clean_up(); +} + +void gps_t::tlm_t::_clean_up() { +} + +gps_t::subframe_2_t::subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_2_t::_read() { + m_iode = m__io->read_u1(); + m_c_rs = m__io->read_s2be(); + m_delta_n = m__io->read_s2be(); + m_m_0 = m__io->read_s4be(); + m_c_uc = m__io->read_s2be(); + m_e = m__io->read_s4be(); + m_c_us = m__io->read_s2be(); + m_sqrt_a = m__io->read_u4be(); + m_t_oe = m__io->read_u2be(); + m_fit_interval_flag = m__io->read_bits_int_be(1); + m_aoda = m__io->read_bits_int_be(5); + m_reserved = m__io->read_bits_int_be(2); +} + +gps_t::subframe_2_t::~subframe_2_t() { + _clean_up(); +} + +void gps_t::subframe_2_t::_clean_up() { +} diff --git a/selfdrive/locationd/generated/gps.h b/selfdrive/locationd/generated/gps.h new file mode 100644 index 000000000..293e2e4a0 --- /dev/null +++ b/selfdrive/locationd/generated/gps.h @@ -0,0 +1,359 @@ +#ifndef GPS_H_ +#define GPS_H_ + +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "kaitai/kaitaistruct.h" +#include + +#if KAITAI_STRUCT_VERSION < 9000L +#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" +#endif + +class gps_t : public kaitai::kstruct { + +public: + class subframe_1_t; + class subframe_3_t; + class subframe_4_t; + class how_t; + class tlm_t; + class subframe_2_t; + + gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, gps_t* p__root = 0); + +private: + void _read(); + void _clean_up(); + +public: + ~gps_t(); + + class subframe_1_t : public kaitai::kstruct { + + public: + + subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_1_t(); + + private: + bool f_af_0; + int32_t m_af_0; + + public: + int32_t af_0(); + + private: + uint64_t m_week_no; + uint64_t m_code; + uint64_t m_sv_accuracy; + uint64_t m_sv_health; + uint64_t m_iodc_msb; + bool m_l2_p_data_flag; + uint64_t m_reserved1; + uint64_t m_reserved2; + uint64_t m_reserved3; + uint64_t m_reserved4; + int8_t m_t_gd; + uint8_t m_iodc_lsb; + uint16_t m_t_oc; + int8_t m_af_2; + int16_t m_af_1; + bool m_af_0_sign; + uint64_t m_af_0_value; + uint64_t m_reserved5; + gps_t* m__root; + gps_t* m__parent; + + public: + uint64_t week_no() const { return m_week_no; } + uint64_t code() const { return m_code; } + uint64_t sv_accuracy() const { return m_sv_accuracy; } + uint64_t sv_health() const { return m_sv_health; } + uint64_t iodc_msb() const { return m_iodc_msb; } + bool l2_p_data_flag() const { return m_l2_p_data_flag; } + uint64_t reserved1() const { return m_reserved1; } + uint64_t reserved2() const { return m_reserved2; } + uint64_t reserved3() const { return m_reserved3; } + uint64_t reserved4() const { return m_reserved4; } + int8_t t_gd() const { return m_t_gd; } + uint8_t iodc_lsb() const { return m_iodc_lsb; } + uint16_t t_oc() const { return m_t_oc; } + int8_t af_2() const { return m_af_2; } + int16_t af_1() const { return m_af_1; } + bool af_0_sign() const { return m_af_0_sign; } + uint64_t af_0_value() const { return m_af_0_value; } + uint64_t reserved5() const { return m_reserved5; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class subframe_3_t : public kaitai::kstruct { + + public: + + subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_3_t(); + + private: + bool f_omega_dot; + int32_t m_omega_dot; + + public: + int32_t omega_dot(); + + private: + bool f_idot; + int32_t m_idot; + + public: + int32_t idot(); + + private: + int16_t m_c_ic; + int32_t m_omega_0; + int16_t m_c_is; + int32_t m_i_0; + int16_t m_c_rc; + int32_t m_omega; + bool m_omega_dot_sign; + uint64_t m_omega_dot_value; + uint8_t m_iode; + bool m_idot_sign; + uint64_t m_idot_value; + uint64_t m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + int16_t c_ic() const { return m_c_ic; } + int32_t omega_0() const { return m_omega_0; } + int16_t c_is() const { return m_c_is; } + int32_t i_0() const { return m_i_0; } + int16_t c_rc() const { return m_c_rc; } + int32_t omega() const { return m_omega; } + bool omega_dot_sign() const { return m_omega_dot_sign; } + uint64_t omega_dot_value() const { return m_omega_dot_value; } + uint8_t iode() const { return m_iode; } + bool idot_sign() const { return m_idot_sign; } + uint64_t idot_value() const { return m_idot_value; } + uint64_t reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class subframe_4_t : public kaitai::kstruct { + + public: + class ionosphere_data_t; + + subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_4_t(); + + class ionosphere_data_t : public kaitai::kstruct { + + public: + + ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~ionosphere_data_t(); + + private: + int8_t m_a0; + int8_t m_a1; + int8_t m_a2; + int8_t m_a3; + int8_t m_b0; + int8_t m_b1; + int8_t m_b2; + int8_t m_b3; + gps_t* m__root; + gps_t::subframe_4_t* m__parent; + + public: + int8_t a0() const { return m_a0; } + int8_t a1() const { return m_a1; } + int8_t a2() const { return m_a2; } + int8_t a3() const { return m_a3; } + int8_t b0() const { return m_b0; } + int8_t b1() const { return m_b1; } + int8_t b2() const { return m_b2; } + int8_t b3() const { return m_b3; } + gps_t* _root() const { return m__root; } + gps_t::subframe_4_t* _parent() const { return m__parent; } + }; + + private: + uint64_t m_data_id; + uint64_t m_page_id; + ionosphere_data_t* m_body; + bool n_body; + + public: + bool _is_null_body() { body(); return n_body; }; + + private: + gps_t* m__root; + gps_t* m__parent; + + public: + uint64_t data_id() const { return m_data_id; } + uint64_t page_id() const { return m_page_id; } + ionosphere_data_t* body() const { return m_body; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class how_t : public kaitai::kstruct { + + public: + + how_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~how_t(); + + private: + uint64_t m_tow_count; + bool m_alert; + bool m_anti_spoof; + uint64_t m_subframe_id; + uint64_t m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + uint64_t tow_count() const { return m_tow_count; } + bool alert() const { return m_alert; } + bool anti_spoof() const { return m_anti_spoof; } + uint64_t subframe_id() const { return m_subframe_id; } + uint64_t reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class tlm_t : public kaitai::kstruct { + + public: + + tlm_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~tlm_t(); + + private: + std::string m_magic; + uint64_t m_tlm; + bool m_integrity_status; + bool m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + std::string magic() const { return m_magic; } + uint64_t tlm() const { return m_tlm; } + bool integrity_status() const { return m_integrity_status; } + bool reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class subframe_2_t : public kaitai::kstruct { + + public: + + subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_2_t(); + + private: + uint8_t m_iode; + int16_t m_c_rs; + int16_t m_delta_n; + int32_t m_m_0; + int16_t m_c_uc; + int32_t m_e; + int16_t m_c_us; + uint32_t m_sqrt_a; + uint16_t m_t_oe; + bool m_fit_interval_flag; + uint64_t m_aoda; + uint64_t m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + uint8_t iode() const { return m_iode; } + int16_t c_rs() const { return m_c_rs; } + int16_t delta_n() const { return m_delta_n; } + int32_t m_0() const { return m_m_0; } + int16_t c_uc() const { return m_c_uc; } + int32_t e() const { return m_e; } + int16_t c_us() const { return m_c_us; } + uint32_t sqrt_a() const { return m_sqrt_a; } + uint16_t t_oe() const { return m_t_oe; } + bool fit_interval_flag() const { return m_fit_interval_flag; } + uint64_t aoda() const { return m_aoda; } + uint64_t reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + +private: + tlm_t* m_tlm; + how_t* m_how; + kaitai::kstruct* m_body; + bool n_body; + +public: + bool _is_null_body() { body(); return n_body; }; + +private: + gps_t* m__root; + kaitai::kstruct* m__parent; + +public: + tlm_t* tlm() const { return m_tlm; } + how_t* how() const { return m_how; } + kaitai::kstruct* body() const { return m_body; } + gps_t* _root() const { return m__root; } + kaitai::kstruct* _parent() const { return m__parent; } +}; + +#endif // GPS_H_ diff --git a/selfdrive/locationd/generated/ubx.cpp b/selfdrive/locationd/generated/ubx.cpp new file mode 100644 index 000000000..5e743e1ee --- /dev/null +++ b/selfdrive/locationd/generated/ubx.cpp @@ -0,0 +1,340 @@ +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "ubx.h" +#include "kaitai/exceptions.h" + +ubx_t::ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = this; + f_checksum = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::_read() { + m_magic = m__io->read_bytes(2); + if (!(magic() == std::string("\xB5\x62", 2))) { + throw kaitai::validation_not_equal_error(std::string("\xB5\x62", 2), magic(), _io(), std::string("/seq/0")); + } + m_msg_type = m__io->read_u2be(); + m_length = m__io->read_u2le(); + n_body = true; + switch (msg_type()) { + case 2569: { + n_body = false; + m_body = new mon_hw_t(m__io, this, m__root); + break; + } + case 533: { + n_body = false; + m_body = new rxm_rawx_t(m__io, this, m__root); + break; + } + case 531: { + n_body = false; + m_body = new rxm_sfrbx_t(m__io, this, m__root); + break; + } + case 2571: { + n_body = false; + m_body = new mon_hw2_t(m__io, this, m__root); + break; + } + case 263: { + n_body = false; + m_body = new nav_pvt_t(m__io, this, m__root); + break; + } + } +} + +ubx_t::~ubx_t() { + _clean_up(); +} + +void ubx_t::_clean_up() { + if (!n_body) { + if (m_body) { + delete m_body; m_body = 0; + } + } + if (f_checksum) { + } +} + +ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + m_measurements = 0; + m__raw_measurements = 0; + m__io__raw_measurements = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::rxm_rawx_t::_read() { + m_rcv_tow = m__io->read_f8le(); + m_week = m__io->read_u2le(); + m_leap_s = m__io->read_s1(); + m_num_meas = m__io->read_u1(); + m_rec_stat = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(3); + int l_measurements = num_meas(); + m__raw_measurements = new std::vector(); + m__raw_measurements->reserve(l_measurements); + m__io__raw_measurements = new std::vector(); + m__io__raw_measurements->reserve(l_measurements); + m_measurements = new std::vector(); + m_measurements->reserve(l_measurements); + for (int i = 0; i < l_measurements; i++) { + m__raw_measurements->push_back(m__io->read_bytes(32)); + kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1)); + m__io__raw_measurements->push_back(io__raw_measurements); + m_measurements->push_back(new meas_t(io__raw_measurements, this, m__root)); + } +} + +ubx_t::rxm_rawx_t::~rxm_rawx_t() { + _clean_up(); +} + +void ubx_t::rxm_rawx_t::_clean_up() { + if (m__raw_measurements) { + delete m__raw_measurements; m__raw_measurements = 0; + } + if (m__io__raw_measurements) { + for (std::vector::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) { + delete *it; + } + delete m__io__raw_measurements; m__io__raw_measurements = 0; + } + if (m_measurements) { + for (std::vector::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) { + delete *it; + } + delete m_measurements; m_measurements = 0; + } +} + +ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::rxm_rawx_t::meas_t::_read() { + m_pr_mes = m__io->read_f8le(); + m_cp_mes = m__io->read_f8le(); + m_do_mes = m__io->read_f4le(); + m_gnss_id = static_cast(m__io->read_u1()); + m_sv_id = m__io->read_u1(); + m_reserved2 = m__io->read_bytes(1); + m_freq_id = m__io->read_u1(); + m_lock_time = m__io->read_u2le(); + m_cno = m__io->read_u1(); + m_pr_stdev = m__io->read_u1(); + m_cp_stdev = m__io->read_u1(); + m_do_stdev = m__io->read_u1(); + m_trk_stat = m__io->read_u1(); + m_reserved3 = m__io->read_bytes(1); +} + +ubx_t::rxm_rawx_t::meas_t::~meas_t() { + _clean_up(); +} + +void ubx_t::rxm_rawx_t::meas_t::_clean_up() { +} + +ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + m_body = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::rxm_sfrbx_t::_read() { + m_gnss_id = static_cast(m__io->read_u1()); + m_sv_id = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(1); + m_freq_id = m__io->read_u1(); + m_num_words = m__io->read_u1(); + m_reserved2 = m__io->read_bytes(1); + m_version = m__io->read_u1(); + m_reserved3 = m__io->read_bytes(1); + int l_body = num_words(); + m_body = new std::vector(); + m_body->reserve(l_body); + for (int i = 0; i < l_body; i++) { + m_body->push_back(m__io->read_u4le()); + } +} + +ubx_t::rxm_sfrbx_t::~rxm_sfrbx_t() { + _clean_up(); +} + +void ubx_t::rxm_sfrbx_t::_clean_up() { + if (m_body) { + delete m_body; m_body = 0; + } +} + +ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::nav_pvt_t::_read() { + m_i_tow = m__io->read_u4le(); + m_year = m__io->read_u2le(); + m_month = m__io->read_u1(); + m_day = m__io->read_u1(); + m_hour = m__io->read_u1(); + m_min = m__io->read_u1(); + m_sec = m__io->read_u1(); + m_valid = m__io->read_u1(); + m_t_acc = m__io->read_u4le(); + m_nano = m__io->read_s4le(); + m_fix_type = m__io->read_u1(); + m_flags = m__io->read_u1(); + m_flags2 = m__io->read_u1(); + m_num_sv = m__io->read_u1(); + m_lon = m__io->read_s4le(); + m_lat = m__io->read_s4le(); + m_height = m__io->read_s4le(); + m_h_msl = m__io->read_s4le(); + m_h_acc = m__io->read_u4le(); + m_v_acc = m__io->read_u4le(); + m_vel_n = m__io->read_s4le(); + m_vel_e = m__io->read_s4le(); + m_vel_d = m__io->read_s4le(); + m_g_speed = m__io->read_s4le(); + m_head_mot = m__io->read_s4le(); + m_s_acc = m__io->read_s4le(); + m_head_acc = m__io->read_u4le(); + m_p_dop = m__io->read_u2le(); + m_flags3 = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(5); + m_head_veh = m__io->read_s4le(); + m_mag_dec = m__io->read_s2le(); + m_mag_acc = m__io->read_u2le(); +} + +ubx_t::nav_pvt_t::~nav_pvt_t() { + _clean_up(); +} + +void ubx_t::nav_pvt_t::_clean_up() { +} + +ubx_t::mon_hw2_t::mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::mon_hw2_t::_read() { + m_ofs_i = m__io->read_s1(); + m_mag_i = m__io->read_u1(); + m_ofs_q = m__io->read_s1(); + m_mag_q = m__io->read_u1(); + m_cfg_source = static_cast(m__io->read_u1()); + m_reserved1 = m__io->read_bytes(3); + m_low_lev_cfg = m__io->read_u4le(); + m_reserved2 = m__io->read_bytes(8); + m_post_status = m__io->read_u4le(); + m_reserved3 = m__io->read_bytes(4); +} + +ubx_t::mon_hw2_t::~mon_hw2_t() { + _clean_up(); +} + +void ubx_t::mon_hw2_t::_clean_up() { +} + +ubx_t::mon_hw_t::mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::mon_hw_t::_read() { + m_pin_sel = m__io->read_u4le(); + m_pin_bank = m__io->read_u4le(); + m_pin_dir = m__io->read_u4le(); + m_pin_val = m__io->read_u4le(); + m_noise_per_ms = m__io->read_u2le(); + m_agc_cnt = m__io->read_u2le(); + m_a_status = static_cast(m__io->read_u1()); + m_a_power = static_cast(m__io->read_u1()); + m_flags = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(1); + m_used_mask = m__io->read_u4le(); + m_vp = m__io->read_bytes(17); + m_jam_ind = m__io->read_u1(); + m_reserved2 = m__io->read_bytes(2); + m_pin_irq = m__io->read_u4le(); + m_pull_h = m__io->read_u4le(); + m_pull_l = m__io->read_u4le(); +} + +ubx_t::mon_hw_t::~mon_hw_t() { + _clean_up(); +} + +void ubx_t::mon_hw_t::_clean_up() { +} + +uint16_t ubx_t::checksum() { + if (f_checksum) + return m_checksum; + std::streampos _pos = m__io->pos(); + m__io->seek((length() + 6)); + m_checksum = m__io->read_u2le(); + m__io->seek(_pos); + f_checksum = true; + return m_checksum; +} diff --git a/selfdrive/locationd/generated/ubx.h b/selfdrive/locationd/generated/ubx.h new file mode 100644 index 000000000..6be4ce8c4 --- /dev/null +++ b/selfdrive/locationd/generated/ubx.h @@ -0,0 +1,410 @@ +#ifndef UBX_H_ +#define UBX_H_ + +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "kaitai/kaitaistruct.h" +#include +#include + +#if KAITAI_STRUCT_VERSION < 9000L +#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" +#endif + +class ubx_t : public kaitai::kstruct { + +public: + class rxm_rawx_t; + class rxm_sfrbx_t; + class nav_pvt_t; + class mon_hw2_t; + class mon_hw_t; + + enum gnss_type_t { + GNSS_TYPE_GPS = 0, + GNSS_TYPE_SBAS = 1, + GNSS_TYPE_GALILEO = 2, + GNSS_TYPE_BEIDOU = 3, + GNSS_TYPE_IMES = 4, + GNSS_TYPE_QZSS = 5, + GNSS_TYPE_GLONASS = 6 + }; + + ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, ubx_t* p__root = 0); + +private: + void _read(); + void _clean_up(); + +public: + ~ubx_t(); + + class rxm_rawx_t : public kaitai::kstruct { + + public: + class meas_t; + + rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~rxm_rawx_t(); + + class meas_t : public kaitai::kstruct { + + public: + + meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~meas_t(); + + private: + double m_pr_mes; + double m_cp_mes; + float m_do_mes; + gnss_type_t m_gnss_id; + uint8_t m_sv_id; + std::string m_reserved2; + uint8_t m_freq_id; + uint16_t m_lock_time; + uint8_t m_cno; + uint8_t m_pr_stdev; + uint8_t m_cp_stdev; + uint8_t m_do_stdev; + uint8_t m_trk_stat; + std::string m_reserved3; + ubx_t* m__root; + ubx_t::rxm_rawx_t* m__parent; + + public: + double pr_mes() const { return m_pr_mes; } + double cp_mes() const { return m_cp_mes; } + float do_mes() const { return m_do_mes; } + gnss_type_t gnss_id() const { return m_gnss_id; } + uint8_t sv_id() const { return m_sv_id; } + std::string reserved2() const { return m_reserved2; } + uint8_t freq_id() const { return m_freq_id; } + uint16_t lock_time() const { return m_lock_time; } + uint8_t cno() const { return m_cno; } + uint8_t pr_stdev() const { return m_pr_stdev; } + uint8_t cp_stdev() const { return m_cp_stdev; } + uint8_t do_stdev() const { return m_do_stdev; } + uint8_t trk_stat() const { return m_trk_stat; } + std::string reserved3() const { return m_reserved3; } + ubx_t* _root() const { return m__root; } + ubx_t::rxm_rawx_t* _parent() const { return m__parent; } + }; + + private: + double m_rcv_tow; + uint16_t m_week; + int8_t m_leap_s; + uint8_t m_num_meas; + uint8_t m_rec_stat; + std::string m_reserved1; + std::vector* m_measurements; + ubx_t* m__root; + ubx_t* m__parent; + std::vector* m__raw_measurements; + std::vector* m__io__raw_measurements; + + public: + double rcv_tow() const { return m_rcv_tow; } + uint16_t week() const { return m_week; } + int8_t leap_s() const { return m_leap_s; } + uint8_t num_meas() const { return m_num_meas; } + uint8_t rec_stat() const { return m_rec_stat; } + std::string reserved1() const { return m_reserved1; } + std::vector* measurements() const { return m_measurements; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + std::vector* _raw_measurements() const { return m__raw_measurements; } + std::vector* _io__raw_measurements() const { return m__io__raw_measurements; } + }; + + class rxm_sfrbx_t : public kaitai::kstruct { + + public: + + rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~rxm_sfrbx_t(); + + private: + gnss_type_t m_gnss_id; + uint8_t m_sv_id; + std::string m_reserved1; + uint8_t m_freq_id; + uint8_t m_num_words; + std::string m_reserved2; + uint8_t m_version; + std::string m_reserved3; + std::vector* m_body; + ubx_t* m__root; + ubx_t* m__parent; + + public: + gnss_type_t gnss_id() const { return m_gnss_id; } + uint8_t sv_id() const { return m_sv_id; } + std::string reserved1() const { return m_reserved1; } + uint8_t freq_id() const { return m_freq_id; } + uint8_t num_words() const { return m_num_words; } + std::string reserved2() const { return m_reserved2; } + uint8_t version() const { return m_version; } + std::string reserved3() const { return m_reserved3; } + std::vector* body() const { return m_body; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + + class nav_pvt_t : public kaitai::kstruct { + + public: + + nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~nav_pvt_t(); + + private: + uint32_t m_i_tow; + uint16_t m_year; + uint8_t m_month; + uint8_t m_day; + uint8_t m_hour; + uint8_t m_min; + uint8_t m_sec; + uint8_t m_valid; + uint32_t m_t_acc; + int32_t m_nano; + uint8_t m_fix_type; + uint8_t m_flags; + uint8_t m_flags2; + uint8_t m_num_sv; + int32_t m_lon; + int32_t m_lat; + int32_t m_height; + int32_t m_h_msl; + uint32_t m_h_acc; + uint32_t m_v_acc; + int32_t m_vel_n; + int32_t m_vel_e; + int32_t m_vel_d; + int32_t m_g_speed; + int32_t m_head_mot; + int32_t m_s_acc; + uint32_t m_head_acc; + uint16_t m_p_dop; + uint8_t m_flags3; + std::string m_reserved1; + int32_t m_head_veh; + int16_t m_mag_dec; + uint16_t m_mag_acc; + ubx_t* m__root; + ubx_t* m__parent; + + public: + uint32_t i_tow() const { return m_i_tow; } + uint16_t year() const { return m_year; } + uint8_t month() const { return m_month; } + uint8_t day() const { return m_day; } + uint8_t hour() const { return m_hour; } + uint8_t min() const { return m_min; } + uint8_t sec() const { return m_sec; } + uint8_t valid() const { return m_valid; } + uint32_t t_acc() const { return m_t_acc; } + int32_t nano() const { return m_nano; } + uint8_t fix_type() const { return m_fix_type; } + uint8_t flags() const { return m_flags; } + uint8_t flags2() const { return m_flags2; } + uint8_t num_sv() const { return m_num_sv; } + int32_t lon() const { return m_lon; } + int32_t lat() const { return m_lat; } + int32_t height() const { return m_height; } + int32_t h_msl() const { return m_h_msl; } + uint32_t h_acc() const { return m_h_acc; } + uint32_t v_acc() const { return m_v_acc; } + int32_t vel_n() const { return m_vel_n; } + int32_t vel_e() const { return m_vel_e; } + int32_t vel_d() const { return m_vel_d; } + int32_t g_speed() const { return m_g_speed; } + int32_t head_mot() const { return m_head_mot; } + int32_t s_acc() const { return m_s_acc; } + uint32_t head_acc() const { return m_head_acc; } + uint16_t p_dop() const { return m_p_dop; } + uint8_t flags3() const { return m_flags3; } + std::string reserved1() const { return m_reserved1; } + int32_t head_veh() const { return m_head_veh; } + int16_t mag_dec() const { return m_mag_dec; } + uint16_t mag_acc() const { return m_mag_acc; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + + class mon_hw2_t : public kaitai::kstruct { + + public: + + enum config_source_t { + CONFIG_SOURCE_FLASH = 102, + CONFIG_SOURCE_OTP = 111, + CONFIG_SOURCE_CONFIG_PINS = 112, + CONFIG_SOURCE_ROM = 113 + }; + + mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~mon_hw2_t(); + + private: + int8_t m_ofs_i; + uint8_t m_mag_i; + int8_t m_ofs_q; + uint8_t m_mag_q; + config_source_t m_cfg_source; + std::string m_reserved1; + uint32_t m_low_lev_cfg; + std::string m_reserved2; + uint32_t m_post_status; + std::string m_reserved3; + ubx_t* m__root; + ubx_t* m__parent; + + public: + int8_t ofs_i() const { return m_ofs_i; } + uint8_t mag_i() const { return m_mag_i; } + int8_t ofs_q() const { return m_ofs_q; } + uint8_t mag_q() const { return m_mag_q; } + config_source_t cfg_source() const { return m_cfg_source; } + std::string reserved1() const { return m_reserved1; } + uint32_t low_lev_cfg() const { return m_low_lev_cfg; } + std::string reserved2() const { return m_reserved2; } + uint32_t post_status() const { return m_post_status; } + std::string reserved3() const { return m_reserved3; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + + class mon_hw_t : public kaitai::kstruct { + + public: + + enum antenna_status_t { + ANTENNA_STATUS_INIT = 0, + ANTENNA_STATUS_DONTKNOW = 1, + ANTENNA_STATUS_OK = 2, + ANTENNA_STATUS_SHORT = 3, + ANTENNA_STATUS_OPEN = 4 + }; + + enum antenna_power_t { + ANTENNA_POWER_FALSE = 0, + ANTENNA_POWER_TRUE = 1, + ANTENNA_POWER_DONTKNOW = 2 + }; + + mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~mon_hw_t(); + + private: + uint32_t m_pin_sel; + uint32_t m_pin_bank; + uint32_t m_pin_dir; + uint32_t m_pin_val; + uint16_t m_noise_per_ms; + uint16_t m_agc_cnt; + antenna_status_t m_a_status; + antenna_power_t m_a_power; + uint8_t m_flags; + std::string m_reserved1; + uint32_t m_used_mask; + std::string m_vp; + uint8_t m_jam_ind; + std::string m_reserved2; + uint32_t m_pin_irq; + uint32_t m_pull_h; + uint32_t m_pull_l; + ubx_t* m__root; + ubx_t* m__parent; + + public: + uint32_t pin_sel() const { return m_pin_sel; } + uint32_t pin_bank() const { return m_pin_bank; } + uint32_t pin_dir() const { return m_pin_dir; } + uint32_t pin_val() const { return m_pin_val; } + uint16_t noise_per_ms() const { return m_noise_per_ms; } + uint16_t agc_cnt() const { return m_agc_cnt; } + antenna_status_t a_status() const { return m_a_status; } + antenna_power_t a_power() const { return m_a_power; } + uint8_t flags() const { return m_flags; } + std::string reserved1() const { return m_reserved1; } + uint32_t used_mask() const { return m_used_mask; } + std::string vp() const { return m_vp; } + uint8_t jam_ind() const { return m_jam_ind; } + std::string reserved2() const { return m_reserved2; } + uint32_t pin_irq() const { return m_pin_irq; } + uint32_t pull_h() const { return m_pull_h; } + uint32_t pull_l() const { return m_pull_l; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + +private: + bool f_checksum; + uint16_t m_checksum; + +public: + uint16_t checksum(); + +private: + std::string m_magic; + uint16_t m_msg_type; + uint16_t m_length; + kaitai::kstruct* m_body; + bool n_body; + +public: + bool _is_null_body() { body(); return n_body; }; + +private: + ubx_t* m__root; + kaitai::kstruct* m__parent; + +public: + std::string magic() const { return m_magic; } + uint16_t msg_type() const { return m_msg_type; } + uint16_t length() const { return m_length; } + kaitai::kstruct* body() const { return m_body; } + ubx_t* _root() const { return m__root; } + kaitai::kstruct* _parent() const { return m__parent; } +}; + +#endif // UBX_H_ diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc new file mode 100755 index 000000000..b9749bb70 --- /dev/null +++ b/selfdrive/locationd/locationd.cc @@ -0,0 +1,457 @@ +#include +#include + +#include "locationd.h" + +using namespace EKFS; +using namespace Eigen; + +ExitHandler do_exit; +const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 +const double ROTATION_SANITY_CHECK = 10.0; // rad/s +const double TRANS_SANITY_CHECK = 200.0; // m/s +const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) +const double ALTITUDE_SANITY_CHECK = 10000; // m +const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad + +static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { + VectorXd res(floatlist.size()); + for (int i = 0; i < floatlist.size(); i++) { + res[i] = floatlist[i]; + } + return res; +} + +static Vector4d quat2vector(const Quaterniond& quat) { + return Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); +} + +static Quaterniond vector2quat(const VectorXd& vec) { + return Quaterniond(vec(0), vec(1), vec(2), vec(3)); +} + +static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { + meas.setValue(kj::arrayPtr(val.data(), val.size())); + meas.setStd(kj::arrayPtr(std.data(), std.size())); + meas.setValid(valid); +} + + +static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { + // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix + return ((rot_matrix * cov_in) * rot_matrix.transpose()); +} + +static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { + // Stds cannot be rotated like values, only covariances can be rotated + return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); +} + +Localizer::Localizer() { + this->kf = std::make_unique(); + this->reset_kalman(); + + this->calib = Vector3d(0.0, 0.0, 0.0); + this->device_from_calib = MatrixXdr::Identity(3, 3); + this->calib_from_device = MatrixXdr::Identity(3, 3); + + for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) { + this->posenet_stds.push_back(10.0); + } + + VectorXd ecef_pos = this->kf->get_x().segment(STATE_ECEF_POS_START); + this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); +} + +void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { + VectorXd predicted_state = this->kf->get_x(); + MatrixXdr predicted_cov = this->kf->get_P(); + VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); + + VectorXd fix_ecef = predicted_state.segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + VectorXd fix_ecef_std = predicted_std.segment(STATE_ECEF_POS_ERR_START); + VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); + VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); + VectorXd fix_pos_geo_vec = this->get_position_geodetic(); + //fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo) + VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr device_from_ecef = quat2rot(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); + VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef); + + VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); + MatrixXdr acc_calib_cov = predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START); + VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt(); + VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + + MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); + VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt(); + + VectorXd vel_device = device_from_ecef * vel_ecef; + VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); + MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); + condensed_cov.topLeftCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + condensed_cov.topRightCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomRightCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomLeftCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size()); + H_input << device_from_ecef_eul, vel_ecef; + MatrixXdr HH = this->kf->H(H_input); + MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose(); + VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); + + VectorXd vel_calib = this->calib_from_device * vel_device; + VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); + + VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); + //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned + VectorXd nextfix_ecef = fix_ecef + vel_ecef; + VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); + //ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef) + + VectorXd accDevice = predicted_state.segment(STATE_ACCELERATION_START); + VectorXd accDeviceErr = predicted_std.segment(STATE_ACCELERATION_ERR_START); + + VectorXd angVelocityDevice = predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + VectorXd angVelocityDeviceErr = predicted_std.segment(STATE_ANGULAR_VELOCITY_ERR_START); + + Vector3d nans = Vector3d(NAN, NAN, NAN); + + // write measurements to msg + init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, true); + init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, true); + init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, true); + init_measurement(fix.initVelocityNED(), ned_vel, nans, true); + init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); + init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); + init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true); + init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated); + init_measurement(fix.initOrientationNED(), orientation_ned, nans, true); + init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); + init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); + init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); + init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated); + + double old_mean = 0.0, new_mean = 0.0; + int i = 0; + for (double x : this->posenet_stds) { + if (i < POSENET_STD_HIST_HALF) { + old_mean += x; + } else { + new_mean += x; + } + i++; + } + old_mean /= POSENET_STD_HIST_HALF; + new_mean /= POSENET_STD_HIST_HALF; + // experimentally found these values, no false positives in 20k minutes of driving + bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); + + fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); + fix.setDeviceStable(!this->device_fell); + this->device_fell = false; + + //fix.setGpsWeek(this->time.week); + //fix.setGpsTimeOfWeek(this->time.tow); + fix.setUnixTimestampMillis(this->unix_timestamp_millis); + + if (fix_ecef_std.norm() < 50.0 && this->calibrated) { + fix.setStatus(cereal::LiveLocationKalman::Status::VALID); + } else if (fix_ecef_std.norm() < 50.0) { + fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED); + } else { + fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED); + } +} + +VectorXd Localizer::get_position_geodetic() { + VectorXd fix_ecef = this->kf->get_x().segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef); + return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt); +} + +void Localizer::handle_sensors(double current_time, const capnp::List::Reader& log) { + // TODO does not yet account for double sensor readings in the log + for (int i = 0; i < log.size(); i++) { + const cereal::SensorEventData::Reader& sensor_reading = log[i]; + + // Ignore empty readings (e.g. in case the magnetometer had no data ready) + if (sensor_reading.getTimestamp() == 0){ + continue; + } + + double sensor_time = 1e-9 * sensor_reading.getTimestamp(); + + // sensor time and log time should be close + if (abs(current_time - sensor_time) > 0.1) { + LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); + return; + } + + // TODO: handle messages from two IMUs at the same time + if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::LSM6DS3) { + continue; + } + + // Gyro Uncalibrated + if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { + auto v = sensor_reading.getGyroUncalibrated().getV(); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ROTATION_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); + } + } + + // Accelerometer + if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) { + auto v = sensor_reading.getAcceleration().getV(); + + // check if device fell, estimate 10 for g + // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving + this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; + + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ACCEL_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); + } + } + } +} + +void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) { + // ignore the message if the fix is invalid + if (log.getFlags() % 2 == 0) { + return; + } + + // Sanity checks + if ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)) { + return; + } + + if ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) { + return; + } + + if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK){ + return; + } + + // Process message + this->last_gps_fix = current_time; + Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; + this->converter = std::make_unique(geodetic); + + VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); + VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; + MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(3.0 * log.getVerticalAccuracy(), 2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy(), 2)).asDiagonal(); + + this->unix_timestamp_millis = log.getTimestamp(); + double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { + LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + } else if (gps_est_error > 50.0) { + LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos); + } + + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { + this->car_speed = std::abs(log.getVEgo()); + if (log.getStandstill()) { + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); + } +} + +void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { + VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); + VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + + if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + return; + } + + VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); + VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); + + if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)){ + return; + } + + if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + return; + } + + this->posenet_stds.pop_front(); + this->posenet_stds.push_back(trans_calib_std[0]); + + // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise + trans_calib_std *= 10.0; + rot_calib_std *= 10.0; + VectorXd rot_device_std = rotate_std(this->device_from_calib, rot_calib_std); + VectorXd trans_device_std = rotate_std(this->device_from_calib, trans_calib_std); + + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, + { (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() }); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, + { (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() }); +} + +void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { + if (log.getRpyCalib().size() > 0) { + auto calib = floatlist2vector(log.getRpyCalib()); + if ((calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)){ + return; + } + + this->calib = calib; + this->device_from_calib = euler2rot(this->calib); + this->calib_from_device = this->device_from_calib.transpose(); + this->calibrated = log.getCalStatus() == 1; + } +} + +void Localizer::reset_kalman(double current_time) { + VectorXd init_x = this->kf->get_initial_x(); + this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3)); +} + +void Localizer::finite_check(double current_time) { + bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all(); + if (!all_finite){ + LOGE("Non-finite values detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::time_check(double current_time) { + double filter_time = this->kf->get_filter_time(); + bool big_time_gap = !isnan(filter_time) && (current_time - filter_time > 10); + if (big_time_gap){ + LOGE("Time gap of over 10s detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) { + // too nonlinear to init on completely wrong + VectorXd init_x = this->kf->get_initial_x(); + MatrixXdr init_P = this->kf->get_initial_P(); + init_x.segment<4>(3) = init_orient; + init_x.head(3) = init_pos; + + this->kf->init_state(init_x, init_P, current_time); +} + +void Localizer::handle_msg_bytes(const char *data, const size_t size) { + AlignedBuffer aligned_buf; + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size)); + cereal::Event::Reader event = cmsg.getRoot(); + + this->handle_msg(event); +} + +void Localizer::handle_msg(const cereal::Event::Reader& log) { + double t = log.getLogMonoTime() * 1e-9; + this->time_check(t); + if (log.isSensorEvents()) { + this->handle_sensors(t, log.getSensorEvents()); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal()); + } else if (log.isCarState()) { + this->handle_car_state(t, log.getCarState()); + } else if (log.isCameraOdometry()) { + this->handle_cam_odo(t, log.getCameraOdometry()); + } else if (log.isLiveCalibration()) { + this->handle_live_calib(t, log.getLiveCalibration()); + } + this->finite_check(); +} + +kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, + bool inputsOK, bool sensorsOK, bool gpsOK) +{ + cereal::Event::Builder evt = msg_builder.initEvent(); + evt.setLogMonoTime(logMonoTime); + cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); + this->build_live_location(liveLoc); + liveLoc.setInputsOK(inputsOK); + liveLoc.setSensorsOK(sensorsOK); + liveLoc.setGpsOK(gpsOK); + return msg_builder.toBytes(); +} + +int Localizer::locationd_thread() { + const std::initializer_list service_list = + { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" }; + PubMaster pm({ "liveLocationKalman" }); + SubMaster sm(service_list, nullptr, { "gpsLocationExternal" }); + + Params params; + + while (!do_exit) { + sm.update(); + for (const char* service : service_list) { + if (sm.updated(service) && sm.valid(service)) { + const cereal::Event::Reader log = sm[service]; + this->handle_msg(log); + } + } + + if (sm.updated("cameraOdometry")) { + uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime(); + bool inputsOK = sm.allAliveAndValid(); + bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents"); + bool gpsOK = (logMonoTime / 1e9) - this->last_gps_fix < 1.0; + + MessageBuilder msg_builder; + kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK); + pm.send("liveLocationKalman", bytes.begin(), bytes.size()); + + if (sm.frame % 1200 == 0 && gpsOK) { // once a minute + VectorXd posGeo = this->get_position_geodetic(); + std::string lastGPSPosJSON = util::string_format( + "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); + + std::thread([¶ms] (const std::string gpsjson) { + params.put("LastGPSPosition", gpsjson); + }, lastGPSPosJSON).detach(); + } + } + } + return 0; +} + +int main() { + setpriority(PRIO_PROCESS, 0, -20); + + Localizer localizer; + return localizer.locationd_thread(); +} diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h new file mode 100755 index 000000000..58030cd92 --- /dev/null +++ b/selfdrive/locationd/locationd.h @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/transformations/coordinates.hpp" +#include "common/transformations/orientation.hpp" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" +#include + +#include "selfdrive/sensord/sensors/constants.h" +#define VISION_DECIMATION 2 +#define SENSOR_DECIMATION 10 +#include "selfdrive/locationd/models/live_kf.h" + +#define POSENET_STD_HIST_HALF 20 + +class Localizer { +public: + Localizer(); + + int locationd_thread(); + + void reset_kalman(double current_time = NAN); + void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos); + void finite_check(double current_time = NAN); + void time_check(double current_time = NAN); + + kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, + bool inputsOK, bool sensorsOK, bool gpsOK); + void build_live_location(cereal::LiveLocationKalman::Builder& fix); + + Eigen::VectorXd get_position_geodetic(); + + void handle_msg_bytes(const char *data, const size_t size); + void handle_msg(const cereal::Event::Reader& log); + void handle_sensors(double current_time, const capnp::List::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log); + void handle_car_state(double current_time, const cereal::CarState::Reader& log); + void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); + void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); + +private: + std::unique_ptr kf; + + Eigen::VectorXd calib; + MatrixXdr device_from_calib; + MatrixXdr calib_from_device; + bool calibrated = false; + + double car_speed = 0.0; + std::deque posenet_stds; + + std::unique_ptr converter; + + int64_t unix_timestamp_millis = 0; + double last_gps_fix = 0; + bool device_fell = false; +}; diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py deleted file mode 100755 index f7267f0cc..000000000 --- a/selfdrive/locationd/locationd.py +++ /dev/null @@ -1,350 +0,0 @@ -#!/usr/bin/env python3 -import json -import numpy as np -import sympy as sp -import cereal.messaging as messaging -from cereal import log -from common.params import Params -import common.transformations.coordinates as coord -from common.transformations.orientation import ecef_euler_from_ned, \ - euler_from_quat, \ - ned_euler_from_ecef, \ - quat_from_euler, euler_from_rot, \ - rot_from_quat, rot_from_euler -from rednose.helpers import KalmanError -from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind -from selfdrive.locationd.models.constants import GENERATED_DIR -from selfdrive.swaglog import cloudlog - -#from datetime import datetime -#from laika.gps_time import GPSTime - -from sympy.utilities.lambdify import lambdify -from rednose.helpers.sympy_helpers import euler_rotate - -SensorSource = log.SensorEventData.SensorSource - - -VISION_DECIMATION = 2 -SENSOR_DECIMATION = 10 -POSENET_STD_HIST = 40 - - -def to_float(arr): - return [float(arr[0]), float(arr[1]), float(arr[2])] - - -def get_H(): - # this returns a function to eval the jacobian - # of the observation function of the local vel - roll = sp.Symbol('roll') - pitch = sp.Symbol('pitch') - yaw = sp.Symbol('yaw') - vx = sp.Symbol('vx') - vy = sp.Symbol('vy') - vz = sp.Symbol('vz') - - h = euler_rotate(roll, pitch, yaw).T*(sp.Matrix([vx, vy, vz])) - H = h.jacobian(sp.Matrix([roll, pitch, yaw, vx, vy, vz])) - H_f = lambdify([roll, pitch, yaw, vx, vy, vz], H) - return H_f - - -class Localizer(): - def __init__(self, disabled_logs=None, dog=None): - if disabled_logs is None: - disabled_logs = [] - - self.kf = LiveKalman(GENERATED_DIR) - self.reset_kalman() - self.max_age = .1 # seconds - self.disabled_logs = disabled_logs - self.calib = np.zeros(3) - self.device_from_calib = np.eye(3) - self.calib_from_device = np.eye(3) - self.calibrated = False - self.H = get_H() - - self.posenet_invalid_count = 0 - self.posenet_speed = 0 - self.car_speed = 0 - self.posenet_stds = 10*np.ones((POSENET_STD_HIST)) - - self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS]) - - self.unix_timestamp_millis = 0 - self.last_gps_fix = 0 - self.device_fell = False - - @staticmethod - def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov, calibrated): - predicted_std = np.sqrt(np.diagonal(predicted_cov)) - - fix_ecef = predicted_state[States.ECEF_POS] - fix_ecef_std = predicted_std[States.ECEF_POS_ERR] - vel_ecef = predicted_state[States.ECEF_VELOCITY] - vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR] - fix_pos_geo = coord.ecef2geodetic(fix_ecef) - #fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo) - orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]) - orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR] - device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T - calibrated_orientation_ecef = euler_from_rot(calib_from_device.dot(device_from_ecef)) - - acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION]) - acc_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( - predicted_cov[States.ACCELERATION_ERR, States.ACCELERATION_ERR]).dot( - calib_from_device.T))) - ang_vel_calib = calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) - ang_vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( - predicted_cov[States.ANGULAR_VELOCITY_ERR, States.ANGULAR_VELOCITY_ERR]).dot( - calib_from_device.T))) - - vel_device = device_from_ecef.dot(vel_ecef) - device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T - idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \ - list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop)) - condensed_cov = predicted_cov[idxs][:, idxs] - HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef]))) - vel_device_cov = HH.dot(condensed_cov).dot(HH.T) - vel_device_std = np.sqrt(np.diagonal(vel_device_cov)) - - vel_calib = calib_from_device.dot(vel_device) - vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( - vel_device_cov).dot(calib_from_device.T))) - - orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) - #orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned - ned_vel = converter.ecef2ned(fix_ecef + vel_ecef) - converter.ecef2ned(fix_ecef) - #ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) - - fix = messaging.log.LiveLocationKalman.new_message() - - # write measurements to msg - measurements = [ - # measurement field, value, std, valid - (fix.positionGeodetic, fix_pos_geo, np.nan*np.zeros(3), True), - (fix.positionECEF, fix_ecef, fix_ecef_std, True), - (fix.velocityECEF, vel_ecef, vel_ecef_std, True), - (fix.velocityNED, ned_vel, np.nan*np.zeros(3), True), - (fix.velocityDevice, vel_device, vel_device_std, True), - (fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True), - (fix.orientationECEF, orientation_ecef, orientation_ecef_std, True), - (fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), calibrated), - (fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True), - (fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True), - (fix.velocityCalibrated, vel_calib, vel_calib_std, calibrated), - (fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, calibrated), - (fix.accelerationCalibrated, acc_calib, acc_calib_std, calibrated), - ] - - for field, value, std, valid in measurements: - # TODO: can we write the lists faster? - field.value = to_float(value) - field.std = to_float(std) - field.valid = valid - - return fix - - def liveLocationMsg(self): - fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P, self.calibrated) - # experimentally found these values, no false positives in 20k minutes of driving - old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:]) - std_spike = new_mean/old_mean > 4 and new_mean > 7 - - fix.posenetOK = not (std_spike and self.car_speed > 5) - fix.deviceStable = not self.device_fell - self.device_fell = False - - #fix.gpsWeek = self.time.week - #fix.gpsTimeOfWeek = self.time.tow - fix.unixTimestampMillis = self.unix_timestamp_millis - - if np.linalg.norm(fix.positionECEF.std) < 50 and self.calibrated: - fix.status = 'valid' - elif np.linalg.norm(fix.positionECEF.std) < 50: - fix.status = 'uncalibrated' - else: - fix.status = 'uninitialized' - return fix - - def update_kalman(self, time, kind, meas, R=None): - try: - self.kf.predict_and_observe(time, kind, meas, R) - except KalmanError: - cloudlog.error("Error in predict and observe, kalman reset") - self.reset_kalman() - - def handle_gps(self, current_time, log): - # ignore the message if the fix is invalid - if log.flags % 2 == 0: - return - - self.last_gps_fix = current_time - - self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) - ecef_pos = self.converter.ned2ecef([0, 0, 0]) - ecef_vel = self.converter.ned2ecef(np.array(log.vNED)) - ecef_pos - ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3) - ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3) - - #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3)) - self.unix_timestamp_millis = log.timestamp - gps_est_error = np.sqrt((self.kf.x[0] - ecef_pos[0])**2 + - (self.kf.x[1] - ecef_pos[1])**2 + - (self.kf.x[2] - ecef_pos[2])**2) - - orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION]) - orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef) - orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)]) - orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi - initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps)) - if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1: - cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset") - self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat) - self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat) - elif gps_est_error > 50: - cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset") - self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat) - - self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R) - self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R) - - def handle_car_state(self, current_time, log): - self.speed_counter += 1 - - if self.speed_counter % SENSOR_DECIMATION == 0: - self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo]) - self.car_speed = abs(log.vEgo) - if log.vEgo == 0: - self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0]) - - def handle_cam_odo(self, current_time, log): - self.cam_counter += 1 - - if self.cam_counter % VISION_DECIMATION == 0: - rot_device = self.device_from_calib.dot(log.rot) - rot_device_std = self.device_from_calib.dot(log.rotStd) - self.update_kalman(current_time, - ObservationKind.CAMERA_ODO_ROTATION, - np.concatenate([rot_device, 10*rot_device_std])) - trans_device = self.device_from_calib.dot(log.trans) - trans_device_std = self.device_from_calib.dot(log.transStd) - self.posenet_speed = np.linalg.norm(trans_device) - self.posenet_stds[:-1] = self.posenet_stds[1:] - self.posenet_stds[-1] = trans_device_std[0] - self.update_kalman(current_time, - ObservationKind.CAMERA_ODO_TRANSLATION, - np.concatenate([trans_device, 10*trans_device_std])) - - def handle_sensors(self, current_time, log): - # TODO does not yet account for double sensor readings in the log - for sensor_reading in log: - sensor_time = 1e-9 * sensor_reading.timestamp - # 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 - if self.gyro_counter % SENSOR_DECIMATION == 0: - v = sensor_reading.gyroUncalibrated.v - self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) - - # Accelerometer - if sensor_reading.sensor == 1 and sensor_reading.type == 1: - # check if device fell, estimate 10 for g - # 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving - self.device_fell = self.device_fell or (np.linalg.norm(np.array(sensor_reading.acceleration.v) - np.array([10, 0, 0])) > 40) - - self.acc_counter += 1 - if self.acc_counter % SENSOR_DECIMATION == 0: - v = sensor_reading.acceleration.v - self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) - - def handle_live_calib(self, current_time, log): - if len(log.rpyCalib): - self.calib = log.rpyCalib - self.device_from_calib = rot_from_euler(self.calib) - self.calib_from_device = self.device_from_calib.T - self.calibrated = log.calStatus == 1 - - def reset_kalman(self, current_time=None, init_orient=None, init_pos=None): - self.filter_time = current_time - init_x = LiveKalman.initial_x.copy() - # too nonlinear to init on completely wrong - if init_orient is not None: - init_x[3:7] = init_orient - if init_pos is not None: - init_x[:3] = init_pos - self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time) - - self.observation_buffer = [] - - self.gyro_counter = 0 - self.acc_counter = 0 - self.speed_counter = 0 - self.cam_counter = 0 - - -def locationd_thread(sm, pm, disabled_logs=None): - if disabled_logs is None: - disabled_logs = [] - - if sm is None: - socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState'] - sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal']) - if pm is None: - pm = messaging.PubMaster(['liveLocationKalman']) - - params = Params() - localizer = Localizer(disabled_logs=disabled_logs) - - while True: - sm.update() - - for sock, updated in sm.updated.items(): - if updated and sm.valid[sock]: - t = sm.logMonoTime[sock] * 1e-9 - if sock == "sensorEvents": - localizer.handle_sensors(t, sm[sock]) - elif sock == "gpsLocationExternal": - localizer.handle_gps(t, sm[sock]) - elif sock == "carState": - localizer.handle_car_state(t, sm[sock]) - elif sock == "cameraOdometry": - localizer.handle_cam_odo(t, sm[sock]) - elif sock == "liveCalibration": - localizer.handle_live_calib(t, sm[sock]) - - if sm.updated['cameraOdometry']: - t = sm.logMonoTime['cameraOdometry'] - msg = messaging.new_message('liveLocationKalman') - msg.logMonoTime = t - - msg.liveLocationKalman = localizer.liveLocationMsg() - msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid() - msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents'] - - gps_age = (t / 1e9) - localizer.last_gps_fix - msg.liveLocationKalman.gpsOK = gps_age < 1.0 - pm.send('liveLocationKalman', msg) - - if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK: # once a minute - location = { - 'latitude': msg.liveLocationKalman.positionGeodetic.value[0], - 'longitude': msg.liveLocationKalman.positionGeodetic.value[1], - 'altitude': msg.liveLocationKalman.positionGeodetic.value[2], - } - params.put("LastGPSPosition", json.dumps(location)) - - -def main(sm=None, pm=None): - locationd_thread(sm, pm) - - -if __name__ == "__main__": - import os - os.environ["OMP_NUM_THREADS"] = "1" - main() diff --git a/selfdrive/locationd/models/SConscript b/selfdrive/locationd/models/SConscript deleted file mode 100644 index 0a197ffda..000000000 --- a/selfdrive/locationd/models/SConscript +++ /dev/null @@ -1,37 +0,0 @@ -Import('env', 'arch') - -templates = Glob('#rednose/templates/*') - -sympy_helpers = "#rednose/helpers/sympy_helpers.py" -ekf_sym = "#rednose/helpers/ekf_sym.py" - -to_build = { - 'live': ('live_kf.py', 'generated'), - 'car': ('car_kf.py', 'generated'), -} - -if arch != "aarch64": - to_build.update({ - 'gnss': ('gnss_kf.py', 'generated'), - 'loc_4': ('loc_kf.py', 'generated'), - 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', 'generated'), - 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', 'generated'), - 'feature_handler_5': ('#rednose/helpers/feature_handler.py', 'generated'), - 'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', 'generated'), - }) - -found = {} - -for target, (command, generated_folder) in to_build.items(): - if File(command).exists(): - found[target] = (command, generated_folder) - -for target, (command, generated_folder) in found.items(): - target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h']) - command_file = File(command) - - env.Command(target_files, - [templates, command_file, sympy_helpers, ekf_sym], - command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath()) - - env.SharedLibrary(f'{generated_folder}/' + target, target_files[0]) diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 6c5357f2d..b440cc9bb 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -4,15 +4,20 @@ import sys from typing import Any, Dict import numpy as np -import sympy as sp -from rednose import KalmanFilter -from rednose.helpers.ekf_sym import EKF_sym, gen_code from selfdrive.locationd.models.constants import ObservationKind from selfdrive.swaglog import cloudlog -i = 0 +from rednose.helpers.kalmanfilter import KalmanFilter +if __name__ == '__main__': # Generating sympy + import sympy as sp + from rednose.helpers.ekf_sym import gen_code +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module, import-error + + +i = 0 def _slice(n): global i @@ -70,12 +75,12 @@ class CarKalman(KalmanFilter): } global_vars = [ - sp.Symbol('mass'), - sp.Symbol('rotational_inertia'), - sp.Symbol('center_to_front'), - sp.Symbol('center_to_rear'), - sp.Symbol('stiffness_front'), - sp.Symbol('stiffness_rear'), + 'mass', + 'rotational_inertia', + 'center_to_front', + 'center_to_rear', + 'stiffness_front', + 'stiffness_rear', ] @staticmethod @@ -84,7 +89,8 @@ class CarKalman(KalmanFilter): name = CarKalman.name # globals - m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars + global_vars = [sp.Symbol(name) for name in CarKalman.global_vars] + m, j, aF, aR, cF_orig, cR_orig = global_vars # make functions and jacobians with sympy # state variables @@ -138,7 +144,7 @@ class CarKalman(KalmanFilter): [sp.Matrix([x]), ObservationKind.STIFFNESS, None], ] - gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=CarKalman.global_vars) + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars) def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0): # pylint: disable=super-init-not-called dim_state = self.initial_x.shape[0] diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc new file mode 100755 index 000000000..00080ab83 --- /dev/null +++ b/selfdrive/locationd/models/live_kf.cc @@ -0,0 +1,144 @@ +#include "live_kf.h" + +using namespace EKFS; +using namespace Eigen; + +Eigen::Map get_mapvec(Eigen::VectorXd& vec) { + return Eigen::Map(vec.data(), vec.rows(), vec.cols()); +} + +Eigen::Map get_mapmat(MatrixXdr& mat) { + return Eigen::Map(mat.data(), mat.rows(), mat.cols()); +} + +std::vector> get_vec_mapvec(std::vector& vec_vec) { + std::vector> res; + for (Eigen::VectorXd& vec : vec_vec) { + res.push_back(get_mapvec(vec)); + } + return res; +} + +std::vector> get_vec_mapmat(std::vector& mat_vec) { + std::vector> res; + for (MatrixXdr& mat : mat_vec) { + res.push_back(get_mapmat(mat)); + } + return res; +} + +LiveKalman::LiveKalman() { + this->dim_state = 23; + this->dim_state_err = 22; + + this->initial_x = live_initial_x; + this->initial_P = live_initial_P_diag.asDiagonal(); + this->Q = live_Q_diag.asDiagonal(); + for (auto& pair : live_obs_noise_diag) { + this->obs_noise[pair.first] = pair.second.asDiagonal(); + } + + // init filter + this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), + get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), + std::vector{3}, std::vector(), 0.2); +} + +void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) { + MatrixXdr covs = covs_diag.asDiagonal(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(VectorXd& state, MatrixXdr& covs, double filter_time) { + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(VectorXd& state, double filter_time) { + MatrixXdr covs = this->filter->covs(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +VectorXd LiveKalman::get_x() { + return this->filter->state(); +} + +MatrixXdr LiveKalman::get_P() { + return this->filter->covs(); +} + +double LiveKalman::get_filter_time() { + return this->filter->get_filter_time(); +} + +std::vector LiveKalman::get_R(int kind, int n) { + std::vector R; + for (int i = 0; i < n; i++) { + R.push_back(this->obs_noise[kind]); + } + return R; +} + +std::optional LiveKalman::predict_and_observe(double t, int kind, std::vector meas, std::vector R) { + std::optional r; + switch (kind) { + case OBSERVATION_CAMERA_ODO_TRANSLATION: + r = this->predict_and_update_odo_trans(meas, t, kind); + break; + case OBSERVATION_CAMERA_ODO_ROTATION: + r = this->predict_and_update_odo_rot(meas, t, kind); + break; + case OBSERVATION_ODOMETRIC_SPEED: + r = this->predict_and_update_odo_speed(meas, t, kind); + break; + default: + if (R.size() == 0) { + R = this->get_R(kind, meas.size()); + } + r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); + break; + } + return r; +} + +std::optional LiveKalman::predict_and_update_odo_speed(std::vector speed, double t, int kind) { + std::vector R; + R.assign(speed.size(), (MatrixXdr(1, 1) << std::pow(0.2, 2)).finished().asDiagonal()); + return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(speed), get_vec_mapmat(R)); +} + +std::optional LiveKalman::predict_and_update_odo_trans(std::vector trans, double t, int kind) { + std::vector z; + std::vector R; + for (VectorXd& trns : trans) { + assert(trns.size() == 6); // TODO remove + z.push_back(trns.head(3)); + R.push_back(trns.segment<3>(3).array().square().matrix().asDiagonal()); + } + return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R)); +} + +std::optional LiveKalman::predict_and_update_odo_rot(std::vector rot, double t, int kind) { + std::vector z; + std::vector R; + for (VectorXd& rt : rot) { + assert(rt.size() == 6); // TODO remove + z.push_back(rt.head(3)); + R.push_back(rt.segment<3>(3).array().square().matrix().asDiagonal()); + } + return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R)); +} + +Eigen::VectorXd LiveKalman::get_initial_x() { + return this->initial_x; +} + +MatrixXdr LiveKalman::get_initial_P() { + return this->initial_P; +} + +MatrixXdr LiveKalman::H(VectorXd in) { + assert(in.size() == 6); + Matrix res; + this->filter->get_extra_routine("H")(in.data(), res.data()); + return res; +} diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h new file mode 100755 index 000000000..4cd4756c9 --- /dev/null +++ b/selfdrive/locationd/models/live_kf.h @@ -0,0 +1,57 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include "generated/live_kf_constants.h" +#include "rednose/helpers/ekf_sym.h" + +#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth) + +using namespace EKFS; + +Eigen::Map get_mapvec(Eigen::VectorXd& vec); +Eigen::Map get_mapmat(MatrixXdr& mat); +std::vector> get_vec_mapvec(std::vector& vec_vec); +std::vector> get_vec_mapmat(std::vector& mat_vec); + +class LiveKalman { +public: + LiveKalman(); + + void init_state(Eigen::VectorXd& state, Eigen::VectorXd& covs_diag, double filter_time); + void init_state(Eigen::VectorXd& state, MatrixXdr& covs, double filter_time); + void init_state(Eigen::VectorXd& state, double filter_time); + + Eigen::VectorXd get_x(); + MatrixXdr get_P(); + double get_filter_time(); + std::vector get_R(int kind, int n); + + std::optional predict_and_observe(double t, int kind, std::vector meas, std::vector R = {}); + std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); + std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); + std::optional predict_and_update_odo_rot(std::vector rot, double t, int kind); + + Eigen::VectorXd get_initial_x(); + MatrixXdr get_initial_P(); + + MatrixXdr H(Eigen::VectorXd in); + +private: + std::string name = "live"; + + std::shared_ptr filter; + + int dim_state; + int dim_state_err; + + Eigen::VectorXd initial_x; + MatrixXdr initial_P; + MatrixXdr Q; // process noise + std::unordered_map obs_noise; +}; diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 3d8a1f65d..3df1416ae 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -1,18 +1,25 @@ #!/usr/bin/env python3 import sys - +import os import numpy as np -import sympy as sp -from selfdrive.swaglog import cloudlog from selfdrive.locationd.models.constants import ObservationKind -from rednose.helpers.ekf_sym import EKF_sym, gen_code + +import sympy as sp +import inspect from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate +from rednose.helpers.ekf_sym import gen_code EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) +def numpy2eigenstring(arr): + assert(len(arr.shape) == 1) + arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '') + return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()" + + class States(): ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef @@ -48,8 +55,8 @@ class LiveKalman(): # state covariance initial_P_diag = np.array([1e16, 1e16, 1e16, - 1e6, 1e6, 1e6, - 1e4, 1e4, 1e4, + 10**2, 10**2, 10**2, + 10**2, 10**2, 10**2, 1**2, 1**2, 1**2, 0.05**2, 0.05**2, 0.05**2, 0.02**2, @@ -57,14 +64,24 @@ class LiveKalman(): (0.01)**2, (0.01)**2, (0.01)**2]) # process noise - Q = np.diag([0.03**2, 0.03**2, 0.03**2, - 0.001**2, 0.001**2, 0.001**2, - 0.01**2, 0.01**2, 0.01**2, - 0.1**2, 0.1**2, 0.1**2, - (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, - (0.02 / 100)**2, - 3**2, 3**2, 3**2, - (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) + Q_diag = np.array([0.03**2, 0.03**2, 0.03**2, + 0.001**2, 0.001**2, 0.001**2, + 0.01**2, 0.01**2, 0.01**2, + 0.1**2, 0.1**2, 0.1**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.02 / 100)**2, + 3**2, 3**2, 3**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) + + obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]), + ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), + ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.IMU_FRAME: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), + ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]), + ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])} @staticmethod def generate_code(generated_dir): @@ -190,99 +207,37 @@ class LiveKalman(): [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]] - gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params) + # this returns a sympy routine for the jacobian of the observation function of the local vel + in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz + h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T*(sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) + extra_routines = [('H', h.jacobian(in_vec), [in_vec])] - def __init__(self, generated_dir): - self.dim_state = self.initial_x.shape[0] - self.dim_state_err = self.initial_P_diag.shape[0] + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) - self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), - ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), - ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), - ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.NO_ROT: np.diag([0.005**2, 0.005**2, 0.005**2]), - ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]), - ObservationKind.ECEF_VEL: np.diag([.5**2, .5**2, .5**2]), - ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])} + # write constants to extra header file for use in cpp + live_kf_header = "#pragma once\n\n" + live_kf_header += "#include \n" + live_kf_header += "#include \n\n" + for state, slc in inspect.getmembers(States, lambda x: type(x) == slice): + assert(slc.step is None) # unsupported + live_kf_header += f'#define STATE_{state}_START {slc.start}\n' + live_kf_header += f'#define STATE_{state}_END {slc.stop}\n' + live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n' + live_kf_header += "\n" - # init filter - self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2, logger=cloudlog) + for kind, val in inspect.getmembers(ObservationKind, lambda x: type(x) == int): + live_kf_header += f'#define OBSERVATION_{kind} {val}\n' + live_kf_header += "\n" - @property - def x(self): - return self.filter.state() + live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n" + live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n" + live_kf_header += "static const std::unordered_map> live_obs_noise_diag = {\n" + for kind, noise in LiveKalman.obs_noise_diag.items(): + live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n" + live_kf_header += "};\n\n" - @property - def t(self): - return self.filter.filter_time - - @property - def P(self): - return self.filter.covs() - - def rts_smooth(self, estimates): - return self.filter.rts_smooth(estimates, norm_quats=True) - - def init_state(self, state, covs_diag=None, covs=None, filter_time=None): - if covs_diag is not None: - P = np.diag(covs_diag) - elif covs is not None: - P = covs - else: - P = self.filter.covs() - self.filter.init_state(state, P, filter_time) - - def predict_and_observe(self, t, kind, meas, R=None): - if len(meas) > 0: - meas = np.atleast_2d(meas) - if kind == ObservationKind.CAMERA_ODO_TRANSLATION: - r = self.predict_and_update_odo_trans(meas, t, kind) - elif kind == ObservationKind.CAMERA_ODO_ROTATION: - r = self.predict_and_update_odo_rot(meas, t, kind) - elif kind == ObservationKind.ODOMETRIC_SPEED: - r = self.predict_and_update_odo_speed(meas, t, kind) - else: - if R is None: - R = self.get_R(kind, len(meas)) - elif len(R.shape) == 2: - R = R[None] - r = self.filter.predict_and_update_batch(t, kind, meas, R) - - # Normalize quats - quat_norm = np.linalg.norm(self.filter.x[3:7, 0]) - self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm - - return r - - def get_R(self, kind, n): - obs_noise = self.obs_noise[kind] - dim = obs_noise.shape[0] - R = np.zeros((n, dim, dim)) - for i in range(n): - R[i, :, :] = obs_noise - return R - - def predict_and_update_odo_speed(self, speed, t, kind): - z = np.array(speed) - R = np.zeros((len(speed), 1, 1)) - for i, _ in enumerate(z): - R[i, :, :] = np.diag([0.2**2]) - return self.filter.predict_and_update_batch(t, kind, z, R) - - def predict_and_update_odo_trans(self, trans, t, kind): - z = trans[:, :3] - R = np.zeros((len(trans), 3, 3)) - for i, _ in enumerate(z): - R[i, :, :] = np.diag(trans[i, 3:]**2) - return self.filter.predict_and_update_batch(t, kind, z, R) - - def predict_and_update_odo_rot(self, rot, t, kind): - z = rot[:, :3] - R = np.zeros((len(rot), 3, 3)) - for i, _ in enumerate(z): - R[i, :, :] = np.diag(rot[i, 3:]**2) - return self.filter.predict_and_update_batch(t, kind, z, R) + open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header) if __name__ == "__main__": diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 96b3c26dc..119d22fb8 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import gc import math import json @@ -16,12 +17,12 @@ class ParamsLearner: def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset): self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset) - self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member - self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member - self.kf.filter.set_center_to_front(CP.centerToFront) # pylint: disable=no-member - self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront) # pylint: disable=no-member - self.kf.filter.set_stiffness_front(CP.tireStiffnessFront) # pylint: disable=no-member - self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear) # pylint: disable=no-member + self.kf.filter.set_global("mass", CP.mass) + self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia) + self.kf.filter.set_global("center_to_front", CP.centerToFront) + self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront) + self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront) + self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear) self.active = False @@ -33,18 +34,20 @@ class ParamsLearner: def handle_log(self, t, which, msg): if which == 'liveLocationKalman': - yaw_rate = msg.angularVelocityCalibrated.value[2] yaw_rate_std = msg.angularVelocityCalibrated.std[2] + yaw_rate_valid = msg.angularVelocityCalibrated.valid + yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s + yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s if self.active: if msg.inputsOK and msg.posenetOK and yaw_rate_valid: self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, - np.array([[[-yaw_rate]]]), + np.array([[-yaw_rate]]), np.array([np.atleast_2d(yaw_rate_std**2)])) - self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]])) + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) elif which == 'carState': self.steering_angle = msg.steeringAngleDeg @@ -55,16 +58,18 @@ class ParamsLearner: self.active = self.speed > 5 and in_linear_region if self.active: - self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngleDeg)]]])) - self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]])) + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]])) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]])) if not self.active: # Reset time when stopped so uncertainty doesn't grow - self.kf.filter.filter_time = t + self.kf.filter.set_filter_time(t) self.kf.filter.reset_rewind() def main(sm=None, pm=None): + gc.disable() + if sm is None: sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman']) if pm is None: @@ -87,16 +92,18 @@ def main(sm=None, pm=None): cloudlog.info("Parameter learner found parameters for wrong car.") params = None - try: - angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0 - steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr - params_sane = angle_offset_sane and steer_ratio_sane - if params is not None and not params_sane: - cloudlog.info(f"Invalid starting values found {params}") + # Check if starting values are sane + if params is not None: + try: + angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0 + steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr + params_sane = angle_offset_sane and steer_ratio_sane + if not params_sane: + cloudlog.info(f"Invalid starting values found {params}") + params = None + except Exception as e: + cloudlog.info(f"Error reading params {params}: {str(e)}") params = None - except Exception as e: - cloudlog.info(f"Error reading params {params}: {str(e)}") - params = None # TODO: cache the params with the capnp struct if params is None: @@ -123,13 +130,17 @@ def main(sm=None, pm=None): learner.handle_log(t, which, sm[which]) if sm.updated['liveLocationKalman']: + x = learner.kf.x + if not all(map(math.isfinite, x)): + cloudlog.error("NaN in liveParameters estimate. Resetting to default values") + learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) + x = learner.kf.x + msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState'] msg.liveParameters.posenetValid = True msg.liveParameters.sensorValid = True - - x = learner.kf.x msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverageDeg = math.degrees(x[States.ANGLE_OFFSET]) diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 154200c08..064d4f90d 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -1,144 +1,30 @@ -#include -#include -#include -#include -#include -#include - -#include "common/swaglog.h" - #include "ublox_msg.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "selfdrive/common/swaglog.h" + +const double gpsPi = 3.1415926535898; #define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4]) -#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1)) -namespace ublox { - -inline int twos_complement(uint32_t v, uint32_t nb) { - int sign = v >> (nb - 1); - int value = v; - if(sign != 0) - value = value - (1 << nb); - return value; -} - -inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) { - int v = GET_FIELD_U(w, nb, pos); - return twos_complement(v, nb); -} - -class EphemerisData { - public: - EphemerisData(uint8_t svId, subframes_map &subframes) { - this->svId = svId; - int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20); - int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6); - int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U( - subframes[1][2+5], 8, 22); - - int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6); - int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22); - int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6); - int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8); - - int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6); - int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14); - int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U( - subframes[2][2+2], 24, 6); - int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14); - int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6); - int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14); - uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U( - subframes[2][2+6], 24, 6); - int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14); - - int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14); - int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U( - subframes[3][2+1], 24, 6); - int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14); - int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U( - subframes[3][2+3], 24, 6); - int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14); - int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6); - int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6); - int idot = GET_FIELD_S(subframes[3][2+7], 14, 8); - - this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6); - this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6); - this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6); - this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14); - this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8); - - double gpsPi = 3.1415926535898; - - // now form variables in radians, meters and seconds etc - this->Tgd = t_gd * pow(2, -31); - this->A = pow(a_powhalf * pow(2, -19), 2.0); - this->cic = c_ic * pow(2, -29); - this->cis = c_is * pow(2, -29); - this->crc = c_rc * pow(2, -5); - this->crs = c_rs * pow(2, -5); - this->cuc = c_uc * pow(2, -29); - this->cus = c_us * pow(2, -29); - this->deltaN = delta_n * pow(2, -43) * gpsPi; - this->ecc = e * pow(2, -33); - this->i0 = i_0 * pow(2, -31) * gpsPi; - this->idot = idot * pow(2, -43) * gpsPi; - this->M0 = m_0 * pow(2, -31) * gpsPi; - this->omega = w * pow(2, -31) * gpsPi; - this->omega_dot = omega_dot * pow(2, -43) * gpsPi; - this->omega0 = omega_0 * pow(2, -31) * gpsPi; - this->toe = t_oe * pow(2, 4); - - this->toc = t_oc * pow(2, 4); - this->gpsWeek = week_no; - this->af0 = a_f0 * pow(2, -31); - this->af1 = a_f1 * pow(2, -43); - this->af2 = a_f2 * pow(2, -55); - - uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22); - uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22); - this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff)); - this->iode = iode1; - - if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 && - GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 && - GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) { - double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30); - double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27); - double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24); - double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24); - double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11); - double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14); - double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16); - double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16); - this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3; - this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3; - this->ionoCoeffsValid = true; - } else { - this->ionoCoeffsValid = false; - } - } - uint16_t svId; - double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc; - uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo; - double af0, af1, af2; - bool valid; - double ionoAlpha[4], ionoBeta[4]; - bool ionoCoeffsValid; -}; - -UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) { - nav_frame_buffer[0U] = std::map(); - for(int i = 1;i < 33;i++) - nav_frame_buffer[0U][i] = subframes_map(); +inline static bool bit_to_bool(uint8_t val, int shifts) { + return (bool)(val & (1 << shifts)); } inline int UbloxMsgParser::needed_bytes() { // Msg header incomplete? - if(bytes_in_parse_buf < UBLOX_HEADER_SIZE) - return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; - uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE; + if(bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE) + return ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; + uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE; // too much data if(needed < (uint16_t)bytes_in_parse_buf) return -1; @@ -147,7 +33,7 @@ inline int UbloxMsgParser::needed_bytes() { inline bool UbloxMsgParser::valid_cheksum() { uint8_t ck_a = 0, ck_b = 0; - for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) { + for(int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE;i++) { ck_a = (ck_a + msg_parse_buf[i]) & 0xFF; ck_b = (ck_b + ck_a) & 0xFF; } @@ -163,17 +49,15 @@ inline bool UbloxMsgParser::valid_cheksum() { } inline bool UbloxMsgParser::valid() { - return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE && + return bytes_in_parse_buf >= ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE && needed_bytes() == 0 && valid_cheksum(); } inline bool UbloxMsgParser::valid_so_far() { - if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) { - //LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]); + if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) { return false; } - if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) { - //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]); + if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) { return false; } if(needed_bytes() == 0 && !valid()) { @@ -182,192 +66,6 @@ inline bool UbloxMsgParser::valid_so_far() { return true; } -kj::Array UbloxMsgParser::gen_solution() { - nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - MessageBuilder msg_builder; - auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal(); - gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); - gpsLoc.setFlags(msg->flags); - gpsLoc.setLatitude(msg->lat * 1e-07); - gpsLoc.setLongitude(msg->lon * 1e-07); - gpsLoc.setAltitude(msg->height * 1e-03); - gpsLoc.setSpeed(msg->gSpeed * 1e-03); - gpsLoc.setBearingDeg(msg->headMot * 1e-5); - gpsLoc.setAccuracy(msg->hAcc * 1e-03); - std::tm timeinfo = std::tm(); - timeinfo.tm_year = msg->year - 1900; - timeinfo.tm_mon = msg->month - 1; - timeinfo.tm_mday = msg->day; - timeinfo.tm_hour = msg->hour; - timeinfo.tm_min = msg->min; - timeinfo.tm_sec = msg->sec; - std::time_t utc_tt = timegm(&timeinfo); - gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06); - float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f }; - gpsLoc.setVNED(f); - gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); - gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); - gpsLoc.setBearingAccuracyDeg(msg->headAcc * 1e-05); - return capnp::messageToFlatArray(msg_builder); -} - -inline bool bit_to_bool(uint8_t val, int shifts) { - return (bool)(val & (1 << shifts)); -} - -kj::Array UbloxMsgParser::gen_raw() { - rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - if(bytes_in_parse_buf != ( - UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE - )) { - LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); - return kj::Array(); - } - rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)]; - MessageBuilder msg_builder; - auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); - mr.setRcvTow(msg->rcvTow); - mr.setGpsWeek(msg->week); - mr.setLeapSeconds(msg->leapS); - mr.setGpsWeek(msg->week); - auto mb = mr.initMeasurements(msg->numMeas); - for(int8_t i = 0; i < msg->numMeas; i++) { - mb[i].setSvId(measurements[i].svId); - mb[i].setSigId(measurements[i].sigId); - mb[i].setPseudorange(measurements[i].prMes); - mb[i].setCarrierCycles(measurements[i].cpMes); - mb[i].setDoppler(measurements[i].doMes); - mb[i].setGnssId(measurements[i].gnssId); - mb[i].setGlonassFrequencyIndex(measurements[i].freqId); - mb[i].setLocktime(measurements[i].locktime); - mb[i].setCno(measurements[i].cno); - mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong - mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15)); - mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong - auto ts = mb[i].initTrackingStatus(); - ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0)); - ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1)); - ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2)); - ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3)); - } - - mr.setNumMeas(msg->numMeas); - auto rs = mr.initReceiverStatus(); - rs.setLeapSecValid(bit_to_bool(msg->recStat, 0)); - rs.setClkReset(bit_to_bool(msg->recStat, 2)); - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::gen_nav_data() { - rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - if(bytes_in_parse_buf != ( - UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE - )) { - LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); - return kj::Array(); - } - rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)]; - if(msg->gnssId == 0) { - uint8_t subframeId = GET_FIELD_U(measurements[1].dwrd, 3, 8); - std::vector words; - for(int i = 0; i < msg->numWords;i++) - words.push_back(measurements[i].dwrd); - - subframes_map &map = nav_frame_buffer[msg->gnssId][msg->svid]; - if (subframeId == 1) { - map = subframes_map(); - map[subframeId] = words; - } else if (map.find(subframeId-1) != map.end()) { - map[subframeId] = words; - } - if(map.size() == 5) { - EphemerisData ephem_data(msg->svid, map); - MessageBuilder msg_builder; - auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); - eph.setSvId(ephem_data.svId); - eph.setToc(ephem_data.toc); - eph.setGpsWeek(ephem_data.gpsWeek); - eph.setAf0(ephem_data.af0); - eph.setAf1(ephem_data.af1); - eph.setAf2(ephem_data.af2); - eph.setIode(ephem_data.iode); - eph.setCrs(ephem_data.crs); - eph.setDeltaN(ephem_data.deltaN); - eph.setM0(ephem_data.M0); - eph.setCuc(ephem_data.cuc); - eph.setEcc(ephem_data.ecc); - eph.setCus(ephem_data.cus); - eph.setA(ephem_data.A); - eph.setToe(ephem_data.toe); - eph.setCic(ephem_data.cic); - eph.setOmega0(ephem_data.omega0); - eph.setCis(ephem_data.cis); - eph.setI0(ephem_data.i0); - eph.setCrc(ephem_data.crc); - eph.setOmega(ephem_data.omega); - eph.setOmegaDot(ephem_data.omega_dot); - eph.setIDot(ephem_data.idot); - eph.setTgd(ephem_data.Tgd); - eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid); - if(ephem_data.ionoCoeffsValid) { - eph.setIonoAlpha(ephem_data.ionoAlpha); - eph.setIonoBeta(ephem_data.ionoBeta); - } else { - eph.setIonoAlpha(kj::ArrayPtr()); - eph.setIonoBeta(kj::ArrayPtr()); - } - return capnp::messageToFlatArray(msg_builder); - } - } - return kj::Array(); -} - -kj::Array UbloxMsgParser::gen_mon_hw() { - mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - - MessageBuilder msg_builder; - auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus(); - hwStatus.setNoisePerMS(msg->noisePerMS); - hwStatus.setAgcCnt(msg->agcCnt); - hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus); - hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->aPower); - hwStatus.setJamInd(msg->jamInd); - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::gen_mon_hw2() { - mon_hw2_msg *msg = (mon_hw2_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - - MessageBuilder msg_builder; - auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2(); - hwStatus.setOfsI(msg->ofsI); - hwStatus.setMagI(msg->magI); - hwStatus.setOfsQ(msg->ofsQ); - hwStatus.setMagQ(msg->magQ); - - switch (msg->cfgSource) { - case 114: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM); - break; - case 111: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP); - break; - case 112: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS); - break; - case 102: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH); - break; - default: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED); - break; - } - - hwStatus.setLowLevCfg(msg->lowLevCfg); - hwStatus.setPostStatus(msg->postStatus); - - return capnp::messageToFlatArray(msg_builder); -} bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { int needed = needed_bytes(); @@ -379,18 +77,268 @@ bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_da } else { bytes_consumed = incoming_data_len; } + // Validate msg format, detect invalid header and invalid checksum. while(!valid_so_far() && bytes_in_parse_buf != 0) { - //LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf); // Corrupted msg, drop a byte. bytes_in_parse_buf -= 1; if(bytes_in_parse_buf > 0) memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf); } + // There is redundant data at the end of buffer, reset the buffer. - if(needed_bytes() == -1) + if(needed_bytes() == -1) { bytes_in_parse_buf = 0; + } return valid(); } + +std::pair> UbloxMsgParser::gen_msg() { + std::string dat = data(); + kaitai::kstream stream(dat); + + ubx_t ubx_message(&stream); + auto body = ubx_message.body(); + + switch (ubx_message.msg_type()) { + case 0x0107: + return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; + break; + case 0x0213: + return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; + break; + case 0x0215: + return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; + break; + case 0x0a09: + return {"ubloxGnss", gen_mon_hw(static_cast(body))}; + break; + case 0x0a0b: + return {"ubloxGnss", gen_mon_hw2(static_cast(body))}; + break; + default: + LOGE("Unkown message type %x", ubx_message.msg_type()); + return {"ubloxGnss", kj::Array()}; + break; + } +} + + +kj::Array UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) { + MessageBuilder msg_builder; + auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal(); + gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); + gpsLoc.setFlags(msg->flags()); + gpsLoc.setLatitude(msg->lat() * 1e-07); + gpsLoc.setLongitude(msg->lon() * 1e-07); + gpsLoc.setAltitude(msg->height() * 1e-03); + gpsLoc.setSpeed(msg->g_speed() * 1e-03); + gpsLoc.setBearingDeg(msg->head_mot() * 1e-5); + gpsLoc.setAccuracy(msg->h_acc() * 1e-03); + std::tm timeinfo = std::tm(); + timeinfo.tm_year = msg->year() - 1900; + timeinfo.tm_mon = msg->month() - 1; + timeinfo.tm_mday = msg->day(); + timeinfo.tm_hour = msg->hour(); + timeinfo.tm_min = msg->min(); + timeinfo.tm_sec = msg->sec(); + + std::time_t utc_tt = timegm(&timeinfo); + gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano() * 1e-06); + float f[] = { msg->vel_n() * 1e-03f, msg->vel_e() * 1e-03f, msg->vel_d() * 1e-03f }; + gpsLoc.setVNED(f); + gpsLoc.setVerticalAccuracy(msg->v_acc() * 1e-03); + gpsLoc.setSpeedAccuracy(msg->s_acc() * 1e-03); + gpsLoc.setBearingAccuracyDeg(msg->head_acc() * 1e-05); + return capnp::messageToFlatArray(msg_builder); +} + + +kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { + auto body = *msg->body(); + + if (msg->gnss_id() == ubx_t::gnss_type_t::GNSS_TYPE_GPS) { + // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes + // We will first need to separate the data from the padding and parity + assert(body.size() == 10); + + std::string subframe_data; + subframe_data.reserve(30); + for (uint32_t word : body) { + word = word >> 6; // TODO: Verify parity + subframe_data.push_back(word >> 16); + subframe_data.push_back(word >> 8); + subframe_data.push_back(word >> 0); + } + + // Collect subframes in map and parse when we have all the parts + kaitai::kstream stream(subframe_data); + gps_t subframe(&stream); + int subframe_id = subframe.how()->subframe_id(); + + if (subframe_id == 1) gps_subframes[msg->sv_id()].clear(); + gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + + if (gps_subframes[msg->sv_id()].size() == 5) { + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); + eph.setSvId(msg->sv_id()); + + // Subframe 1 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); + gps_t subframe(&stream); + gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); + + eph.setGpsWeek(subframe_1->week_no()); + eph.setTgd(subframe_1->t_gd() * pow(2, -31)); + eph.setToc(subframe_1->t_oc() * pow(2, 4)); + eph.setAf2(subframe_1->af_2() * pow(2, -55)); + eph.setAf1(subframe_1->af_1() * pow(2, -43)); + eph.setAf0(subframe_1->af_0() * pow(2, -31)); + } + + // Subframe 2 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); + gps_t subframe(&stream); + gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); + + eph.setCrs(subframe_2->c_rs() * pow(2, -5)); + eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); + eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); + eph.setCuc(subframe_2->c_uc() * pow(2, -29)); + eph.setEcc(subframe_2->e() * pow(2, -33)); + eph.setCus(subframe_2->c_us() * pow(2, -29)); + eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); + eph.setToe(subframe_2->t_oe() * pow(2, 4)); + } + + // Subframe 3 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); + gps_t subframe(&stream); + gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); + + eph.setCic(subframe_3->c_ic() * pow(2, -29)); + eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); + eph.setCis(subframe_3->c_is() * pow(2, -29)); + eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); + eph.setCrc(subframe_3->c_rc() * pow(2, -5)); + eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); + eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); + eph.setIode(subframe_3->iode()); + eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); + } + + // Subframe 4 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][4]); + gps_t subframe(&stream); + gps_t::subframe_4_t* subframe_4 = static_cast(subframe.body()); + + // This is page 18, why is the page id 56? + if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) { + auto iono = static_cast(subframe_4->body()); + double a0 = iono->a0() * pow(2, -30); + double a1 = iono->a1() * pow(2, -27); + double a2 = iono->a2() * pow(2, -24); + double a3 = iono->a3() * pow(2, -24); + eph.setIonoAlpha({a0, a1, a2, a3}); + + double b0 = iono->b0() * pow(2, 11); + double b1 = iono->b1() * pow(2, 14); + double b2 = iono->b2() * pow(2, 16); + double b3 = iono->b3() * pow(2, 16); + eph.setIonoBeta({b0, b1, b2, b3}); + } + } + + return capnp::messageToFlatArray(msg_builder); + } + } + return kj::Array(); +} + +kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { + MessageBuilder msg_builder; + auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); + mr.setRcvTow(msg->rcv_tow()); + mr.setGpsWeek(msg->week()); + mr.setLeapSeconds(msg->leap_s()); + mr.setGpsWeek(msg->week()); + + auto mb = mr.initMeasurements(msg->num_meas()); + auto measurements = *msg->measurements(); + for(int8_t i = 0; i < msg->num_meas(); i++) { + mb[i].setSvId(measurements[i]->sv_id()); + mb[i].setPseudorange(measurements[i]->pr_mes()); + mb[i].setCarrierCycles(measurements[i]->cp_mes()); + mb[i].setDoppler(measurements[i]->do_mes()); + mb[i].setGnssId(measurements[i]->gnss_id()); + mb[i].setGlonassFrequencyIndex(measurements[i]->freq_id()); + mb[i].setLocktime(measurements[i]->lock_time()); + mb[i].setCno(measurements[i]->cno()); + mb[i].setPseudorangeStdev(0.01 * (pow(2, (measurements[i]->pr_stdev() & 15)))); // weird scaling, might be wrong + mb[i].setCarrierPhaseStdev(0.004 * (measurements[i]->cp_stdev() & 15)); + mb[i].setDopplerStdev(0.002 * (pow(2, (measurements[i]->do_stdev() & 15)))); // weird scaling, might be wrong + + auto ts = mb[i].initTrackingStatus(); + auto trk_stat = measurements[i]->trk_stat(); + ts.setPseudorangeValid(bit_to_bool(trk_stat, 0)); + ts.setCarrierPhaseValid(bit_to_bool(trk_stat, 1)); + ts.setHalfCycleValid(bit_to_bool(trk_stat, 2)); + ts.setHalfCycleSubtracted(bit_to_bool(trk_stat, 3)); + } + + mr.setNumMeas(msg->num_meas()); + auto rs = mr.initReceiverStatus(); + rs.setLeapSecValid(bit_to_bool(msg->rec_stat(), 0)); + rs.setClkReset(bit_to_bool(msg->rec_stat(), 2)); + return capnp::messageToFlatArray(msg_builder); +} + +kj::Array UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) { + MessageBuilder msg_builder; + auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus(); + hwStatus.setNoisePerMS(msg->noise_per_ms()); + hwStatus.setFlags(msg->flags()); + hwStatus.setAgcCnt(msg->agc_cnt()); + hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status()); + hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->a_power()); + hwStatus.setJamInd(msg->jam_ind()); + return capnp::messageToFlatArray(msg_builder); +} + +kj::Array UbloxMsgParser::gen_mon_hw2(ubx_t::mon_hw2_t *msg) { + MessageBuilder msg_builder; + auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2(); + hwStatus.setOfsI(msg->ofs_i()); + hwStatus.setMagI(msg->mag_i()); + hwStatus.setOfsQ(msg->ofs_q()); + hwStatus.setMagQ(msg->mag_q()); + + switch (msg->cfg_source()) { + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_ROM: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM); + break; + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_OTP: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP); + break; + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_CONFIG_PINS: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS); + break; + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_FLASH: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH); + break; + default: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED); + break; + } + + hwStatus.setLowLevCfg(msg->low_lev_cfg()); + hwStatus.setPostStatus(msg->post_status()); + + return capnp::messageToFlatArray(msg_builder); } diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 2e99e94e1..1f8ed9747 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -1,191 +1,52 @@ #pragma once -#include -#include "messaging.hpp" +#include +#include +#include +#include -// NAV_PVT -typedef struct __attribute__((packed)) { - uint32_t iTOW; - uint16_t year; - int8_t month; - int8_t day; - int8_t hour; - int8_t min; - int8_t sec; - int8_t valid; - uint32_t tAcc; - int32_t nano; - int8_t fixType; - int8_t flags; - int8_t flags2; - int8_t numSV; - int32_t lon; - int32_t lat; - int32_t height; - int32_t hMSL; - uint32_t hAcc; - uint32_t vAcc; - int32_t velN; - int32_t velE; - int32_t velD; - int32_t gSpeed; - int32_t headMot; - uint32_t sAcc; - uint32_t headAcc; - uint16_t pDOP; - int8_t reserverd1[6]; - int32_t headVeh; - int16_t magDec; - uint16_t magAcc; -} nav_pvt_msg; - -// RXM_RAW -typedef struct __attribute__((packed)) { - double rcvTow; - uint16_t week; - int8_t leapS; - int8_t numMeas; - int8_t recStat; - int8_t reserved1[3]; -} rxm_raw_msg; - -// Extra data count is in numMeas -typedef struct __attribute__((packed)) { - double prMes; - double cpMes; - float doMes; - int8_t gnssId; - int8_t svId; - int8_t sigId; - int8_t freqId; - uint16_t locktime; - int8_t cno; - int8_t prStdev; - int8_t cpStdev; - int8_t doStdev; - int8_t trkStat; - int8_t reserved3; -} rxm_raw_msg_extra; - -// RXM_SFRBX -typedef struct __attribute__((packed)) { - int8_t gnssId; - int8_t svid; - int8_t reserved1; - int8_t freqId; - int8_t numWords; - int8_t reserved2; - int8_t version; - int8_t reserved3; -} rxm_sfrbx_msg; - -// Extra data count is in numWords -typedef struct __attribute__((packed)) { - uint32_t dwrd; -} rxm_sfrbx_msg_extra; - -// MON_HW -typedef struct __attribute__((packed)) { - uint32_t pinSel; - uint32_t pinBank; - uint32_t pinDir; - uint32_t pinVal; - uint16_t noisePerMS; - uint16_t agcCnt; - uint8_t aStatus; - uint8_t aPower; - uint8_t flags; - uint8_t reserved1; - uint32_t usedMask; - uint8_t VP[17]; - uint8_t jamInd; - uint8_t reserved2[2]; - uint32_t pinIrq; - uint32_t pullH; - uint32_t pullL; -} mon_hw_msg; - -// MON_HW2 -typedef struct __attribute__((packed)) { - int8_t ofsI; - uint8_t magI; - int8_t ofsQ; - uint8_t magQ; - uint8_t cfgSource; - uint8_t reserved1[3]; - uint32_t lowLevCfg; - uint8_t reserved2[8]; - uint32_t postStatus; - uint8_t reserved3[4]; -} mon_hw2_msg; +#include "cereal/messaging/messaging.h" +#include "selfdrive/locationd/generated/gps.h" +#include "selfdrive/locationd/generated/ubx.h" +// protocol constants namespace ublox { - // protocol constants const uint8_t PREAMBLE1 = 0xb5; const uint8_t PREAMBLE2 = 0x62; - // message classes - const uint8_t CLASS_NAV = 0x01; - const uint8_t CLASS_RXM = 0x02; - const uint8_t CLASS_MON = 0x0A; - - // NAV messages - const uint8_t MSG_NAV_PVT = 0x7; - - // RXM messages - const uint8_t MSG_RXM_RAW = 0x15; - const uint8_t MSG_RXM_SFRBX = 0x13; - - // MON messages - const uint8_t MSG_MON_HW = 0x09; - const uint8_t MSG_MON_HW2 = 0x0B; - const int UBLOX_HEADER_SIZE = 6; const int UBLOX_CHECKSUM_SIZE = 2; const int UBLOX_MAX_MSG_SIZE = 65536; - typedef std::map> subframes_map; - - class UbloxMsgParser { - public: - - UbloxMsgParser(); - kj::Array gen_solution(); - kj::Array gen_raw(); - kj::Array gen_mon_hw(); - kj::Array gen_mon_hw2(); - - kj::Array gen_nav_data(); - bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); - inline void reset() {bytes_in_parse_buf = 0;} - inline uint8_t msg_class() { - return msg_parse_buf[2]; - } - - inline uint8_t msg_id() { - return msg_parse_buf[3]; - } - inline int needed_bytes(); - - void hexdump(uint8_t *d, int l) { - for (int i = 0; i < l; i++) { - if (i%0x10 == 0 && i != 0) printf("\n"); - printf("%02X ", d[i]); - } - printf("\n"); - } - private: - inline bool valid_cheksum(); - inline bool valid(); - inline bool valid_so_far(); - - uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE]; - int bytes_in_parse_buf; - std::map> nav_frame_buffer; - }; - + // Boardd still uses these: + const uint8_t CLASS_NAV = 0x01; + const uint8_t CLASS_RXM = 0x02; + const uint8_t CLASS_MON = 0x0A; } -typedef Message * (*poll_ubloxraw_msg_func)(Poller *poller); -typedef int (*send_gps_event_func)(PubSocket *s, const void *buf, size_t len); -int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func); +class UbloxMsgParser { + public: + bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); + inline void reset() {bytes_in_parse_buf = 0;} + inline int needed_bytes(); + inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);} + + std::pair> gen_msg(); + kj::Array gen_nav_pvt(ubx_t::nav_pvt_t *msg); + kj::Array gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg); + kj::Array gen_rxm_rawx(ubx_t::rxm_rawx_t *msg); + kj::Array gen_mon_hw(ubx_t::mon_hw_t *msg); + kj::Array gen_mon_hw2(ubx_t::mon_hw2_t *msg); + + private: + inline bool valid_cheksum(); + inline bool valid(); + inline bool valid_so_far(); + + std::unordered_map> gps_subframes; + + size_t bytes_in_parse_buf = 0; + uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE]; + +}; + diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc index 5fdcebba0..445495a86 100644 --- a/selfdrive/locationd/ubloxd.cc +++ b/selfdrive/locationd/ubloxd.cc @@ -1,23 +1,66 @@ -#include +#include -#include "messaging.hpp" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/locationd/ublox_msg.h" -#include "ublox_msg.h" - -Message * poll_ubloxraw_msg(Poller * poller) { - auto p = poller->poll(1000); - - if (p.size()) { - return p[0]->receive(); - } else { - return NULL; - } -} - -int send_gps_event(PubSocket *s, const void *buf, size_t len) { - return s->send((char*)buf, len); -} +ExitHandler do_exit; +using namespace ublox; int main() { - return ubloxd_main(poll_ubloxraw_msg, send_gps_event); -} \ No newline at end of file + LOGW("starting ubloxd"); + AlignedBuffer aligned_buf; + UbloxMsgParser parser; + + PubMaster pm({"ubloxGnss", "gpsLocationExternal"}); + + Context * context = Context::create(); + SubSocket * subscriber = SubSocket::create(context, "ubloxRaw"); + assert(subscriber != NULL); + subscriber->setTimeout(100); + + + while (!do_exit) { + Message * msg = subscriber->receive(); + if (!msg){ + if (errno == EINTR) { + do_exit = true; + } + continue; + } + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg)); + cereal::Event::Reader event = cmsg.getRoot(); + auto ubloxRaw = event.getUbloxRaw(); + + const uint8_t *data = ubloxRaw.begin(); + size_t len = ubloxRaw.size(); + size_t bytes_consumed = 0; + + while(bytes_consumed < len && !do_exit) { + size_t bytes_consumed_this_time = 0U; + if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { + + try { + auto msg = parser.gen_msg(); + if (msg.second.size() > 0) { + auto bytes = msg.second.asBytes(); + pm.send(msg.first.c_str(), bytes.begin(), bytes.size()); + } + } catch (const std::exception& e) { + LOGE("Error parsing ublox message %s", e.what()); + } + + parser.reset(); + } + bytes_consumed += bytes_consumed_this_time; + } + delete msg; + } + + delete subscriber; + delete context; + + return 0; +} diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc deleted file mode 100644 index 7fd395434..000000000 --- a/selfdrive/locationd/ubloxd_main.cc +++ /dev/null @@ -1,114 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "messaging.hpp" -#include "common/util.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/timing.h" - -#include "ublox_msg.h" - -ExitHandler do_exit; -using namespace ublox; -int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) { - LOGW("starting ubloxd"); - AlignedBuffer aligned_buf; - UbloxMsgParser parser; - - Context * context = Context::create(); - SubSocket * subscriber = SubSocket::create(context, "ubloxRaw"); - assert(subscriber != NULL); - subscriber->setTimeout(100); - - PubMaster pm({"ubloxGnss", "gpsLocationExternal"}); - - while (!do_exit) { - Message * msg = subscriber->receive(); - if (!msg){ - if (errno == EINTR) { - do_exit = true; - } - continue; - } - - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg)); - cereal::Event::Reader event = cmsg.getRoot(); - auto ubloxRaw = event.getUbloxRaw(); - - const uint8_t *data = ubloxRaw.begin(); - size_t len = ubloxRaw.size(); - size_t bytes_consumed = 0; - while(bytes_consumed < len && !do_exit) { - size_t bytes_consumed_this_time = 0U; - if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { - // New message available - if(parser.msg_class() == CLASS_NAV) { - if(parser.msg_id() == MSG_NAV_PVT) { - //LOGD("MSG_NAV_PVT"); - auto words = parser.gen_solution(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("gpsLocationExternal", bytes.begin(), bytes.size()); - } - } else - LOGW("Unknown nav msg id: 0x%02X", parser.msg_id()); - } else if(parser.msg_class() == CLASS_RXM) { - if(parser.msg_id() == MSG_RXM_RAW) { - //LOGD("MSG_RXM_RAW"); - auto words = parser.gen_raw(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else if(parser.msg_id() == MSG_RXM_SFRBX) { - //LOGD("MSG_RXM_SFRBX"); - auto words = parser.gen_nav_data(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else - LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id()); - } else if(parser.msg_class() == CLASS_MON) { - if(parser.msg_id() == MSG_MON_HW) { - //LOGD("MSG_MON_HW"); - auto words = parser.gen_mon_hw(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else if(parser.msg_id() == MSG_MON_HW2) { - //LOGD("MSG_MON_HW2"); - auto words = parser.gen_mon_hw2(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else { - LOGW("Unknown mon msg id: 0x%02X", parser.msg_id()); - } - } else - LOGW("Unknown msg class: 0x%02X", parser.msg_class()); - parser.reset(); - } - bytes_consumed += bytes_consumed_this_time; - } - delete msg; - } - - delete subscriber; - delete context; - - return 0; -} diff --git a/selfdrive/logcatd/logcatd_android.cc b/selfdrive/logcatd/logcatd_android.cc index 95a0c7b70..1a982a0fe 100644 --- a/selfdrive/logcatd/logcatd_android.cc +++ b/selfdrive/logcatd/logcatd_android.cc @@ -1,13 +1,17 @@ +#include +#include + #include #include #include -#include "common/util.h" -#include "messaging.hpp" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/util.h" int main() { - ExitHandler do_exit; + setpriority(PRIO_PROCESS, 0, -15); + ExitHandler do_exit; PubMaster pm({"androidLog"}); log_time last_log_time = {}; diff --git a/selfdrive/logcatd/logcatd_systemd.cc b/selfdrive/logcatd/logcatd_systemd.cc index da9b3bb88..61d6cb9b0 100644 --- a/selfdrive/logcatd/logcatd_systemd.cc +++ b/selfdrive/logcatd/logcatd_systemd.cc @@ -1,15 +1,16 @@ -#include -#include -#include -#include -#include - -#include "json11.hpp" #include -#include "common/timing.h" -#include "common/util.h" -#include "messaging.hpp" +#include +#include +#include +#include +#include + +#include "json11.hpp" + +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" ExitHandler do_exit; int main(int argc, char *argv[]) { diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 1492d92f3..18325c7a4 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -2,9 +2,10 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'gpucommon') logger_lib = env.Library('logger', ["logger.cc"]) -libs = [logger_lib, 'zmq', 'capnp', 'kj', 'z', +libs = [logger_lib, common, cereal, messaging, visionipc, + 'zmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', - 'yuv', 'bz2', 'OpenCL', common, cereal, messaging, visionipc] + 'yuv', 'bz2', 'OpenCL'] src = ['loggerd.cc'] if arch in ["aarch64", "larch64"]: diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc index 16ca95b27..e36012bd2 100644 --- a/selfdrive/loggerd/bootlog.cc +++ b/selfdrive/loggerd/bootlog.cc @@ -1,8 +1,10 @@ #include + #include -#include "common/swaglog.h" -#include "logger.h" -#include "messaging.hpp" + +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/loggerd/logger.h" static kj::Array build_boot_log() { MessageBuilder msg; @@ -10,11 +12,25 @@ static kj::Array build_boot_log() { boot.setWallTimeNanos(nanos_since_epoch()); - std::string lastKmsg = util::read_file("/sys/fs/pstore/console-ramoops"); - boot.setLastKmsg(capnp::Data::Reader((const kj::byte*)lastKmsg.data(), lastKmsg.size())); + std::string pstore = "/sys/fs/pstore"; + std::map pstore_map; + util::read_files_in_dir(pstore, &pstore_map); - std::string lastPmsg = util::read_file("/sys/fs/pstore/pmsg-ramoops-0"); - boot.setLastPmsg(capnp::Data::Reader((const kj::byte*)lastPmsg.data(), lastPmsg.size())); + const std::vector log_keywords = {"Kernel panic"}; + auto lpstore = boot.initPstore().initEntries(pstore_map.size()); + int i = 0; + for (auto& kv : pstore_map) { + auto lentry = lpstore[i]; + lentry.setKey(kv.first); + lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size())); + i++; + + for (auto &k : log_keywords) { + if (kv.second.find(k) != std::string::npos) { + LOGE("%s: found '%s'", kv.first.c_str(), k.c_str()); + } + } + } std::string launchLog = util::read_file("/tmp/launch_log"); boot.setLaunchLog(capnp::Text::Reader(launchLog.data(), launchLog.size())); diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc index ca621810e..72bb4cb62 100644 --- a/selfdrive/loggerd/logger.cc +++ b/selfdrive/loggerd/logger.cc @@ -1,27 +1,27 @@ +#include "selfdrive/loggerd/logger.h" + +#include +#include +#include +#include #include #include -#include -#include #include -#include -#include -#include -#include #include +#include +#include -#include #include +#include #include #ifdef QCOM #include #endif -#include "common/swaglog.h" -#include "common/params.h" -#include "common/version.h" -#include "messaging.hpp" -#include "logger.h" - +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/version.h" // ***** logging helpers ***** @@ -53,9 +53,9 @@ kj::Array logger_build_init_data() { MessageBuilder msg; auto init = msg.initEvent().initInitData(); - if (util::file_exists("/EON")) { + if (Hardware::EON()) { init.setDeviceType(cereal::InitData::DeviceType::NEO); - } else if (util::file_exists("/TICI")) { + } else if (Hardware::TICI()) { init.setDeviceType(cereal::InitData::DeviceType::TICI); } else { init.setDeviceType(cereal::InitData::DeviceType::PC); @@ -91,10 +91,6 @@ kj::Array logger_build_init_data() { } #endif - const char* dongle_id = getenv("DONGLE_ID"); - if (dongle_id) { - init.setDongleId(std::string(dongle_id)); - } init.setDirty(!getenv("CLEAN")); // log params @@ -102,10 +98,11 @@ kj::Array logger_build_init_data() { init.setGitCommit(params.get("GitCommit")); init.setGitBranch(params.get("GitBranch")); init.setGitRemote(params.get("GitRemote")); - init.setPassive(params.read_db_bool("Passive")); + init.setPassive(params.getBool("Passive")); + init.setDongleId(params.get("DongleId")); { std::map params_map; - params.read_db_all(¶ms_map); + params.readAll(¶ms_map); auto lparams = init.initParams().initEntries(params_map.size()); int i = 0; for (auto& kv : params_map) { diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h index 7e26a2246..f530a2b04 100644 --- a/selfdrive/loggerd/logger.h +++ b/selfdrive/loggerd/logger.h @@ -1,20 +1,22 @@ #pragma once -#include -#include #include +#include +#include + #include + #include -#include #include -#include "common/util.h" +#include -#if defined(QCOM) || defined(QCOM2) -const std::string LOG_ROOT = "/data/media/0/realdata"; -#else -const std::string LOG_ROOT = util::getenv_default("HOME", "/.comma/media/0/realdata", "/data/media/0/realdata"); -#endif +#include "selfdrive/common/util.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/hardware/hw.h" +const std::string LOG_ROOT = + Hardware::PC() ? util::getenv_default("HOME", "/.comma/media/0/realdata", "/data/media/0/realdata") + : "/data/media/0/realdata"; #define LOGGER_MAX_HANDLES 16 class BZFile { diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index c28657c6f..c55115402 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -1,57 +1,49 @@ -#include -#include -#include -#include -#include #include -#include +#include #include #include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include +#include "cereal/messaging/messaging.h" +#include "cereal/services.h" +#include "cereal/visionipc/visionipc.h" +#include "cereal/visionipc/visionipc_client.h" +#include "selfdrive/camerad/cameras/camera_common.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" -#include "common/timing.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "camerad/cameras/camera_common.h" -#include "logger.h" -#include "messaging.hpp" -#include "services.h" - -#include "visionipc.h" -#include "visionipc_client.h" - -#include "encoder.h" +#include "selfdrive/loggerd/encoder.h" +#include "selfdrive/loggerd/logger.h" #if defined(QCOM) || defined(QCOM2) -#include "omx_encoder.h" +#include "selfdrive/loggerd/omx_encoder.h" #define Encoder OmxEncoder #else -#include "raw_logger.h" +#include "selfdrive/loggerd/raw_logger.h" #define Encoder RawLogger #endif namespace { constexpr int MAIN_FPS = 20; - -#ifndef QCOM2 -constexpr int MAIN_BITRATE = 5000000; -constexpr int MAX_CAM_IDX = LOG_CAMERA_ID_DCAMERA; -constexpr int DCAM_BITRATE = 2500000; -#else -constexpr int MAIN_BITRATE = 10000000; -constexpr int MAX_CAM_IDX = LOG_CAMERA_ID_ECAMERA; -constexpr int DCAM_BITRATE = MAIN_BITRATE; -#endif +const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000; +const int MAX_CAM_IDX = Hardware::TICI() ? LOG_CAMERA_ID_ECAMERA : LOG_CAMERA_ID_DCAMERA; +const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000; #define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead @@ -96,11 +88,8 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { .bitrate = 256000, .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 + .frame_width = Hardware::TICI() ? 526 : 480, + .frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same? }, }; @@ -279,17 +268,18 @@ void encoder_thread(int cam_idx) { eidx.setFrameId(extra.frame_id); eidx.setTimestampSof(extra.timestamp_sof); eidx.setTimestampEof(extra.timestamp_eof); - #ifdef QCOM2 - eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C); - #else - eidx.setType(cam_idx == LOG_CAMERA_ID_DCAMERA ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); - #endif + if (Hardware::TICI()) { + eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C); + } else { + eidx.setType(cam_idx == LOG_CAMERA_ID_DCAMERA ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); + } eidx.setEncodeId(cnt); eidx.setSegmentNum(rotate_state.cur_seg); eidx.setSegmentId(out_id); if (lh) { + // TODO: this should read cereal/services.h for qlog decimation auto bytes = msg.toBytes(); - lh_log(lh, bytes.begin(), bytes.size(), false); + lh_log(lh, bytes.begin(), bytes.size(), true); } } } @@ -325,8 +315,7 @@ void clear_locks() { } // namespace int main(int argc, char** argv) { - - setpriority(PRIO_PROCESS, 0, -12); + setpriority(PRIO_PROCESS, 0, -20); clear_locks(); @@ -368,18 +357,14 @@ int main(int argc, char** argv) { encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_FCAMERA)); s.rotate_state[LOG_CAMERA_ID_FCAMERA].enabled = true; -#if defined(QCOM) || defined(QCOM2) - bool record_front = Params().read_db_bool("RecordFront"); - if (record_front) { + if (!Hardware::PC() && Params().getBool("RecordFront")) { encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_DCAMERA)); s.rotate_state[LOG_CAMERA_ID_DCAMERA].enabled = true; } - -#ifdef QCOM2 - encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_ECAMERA)); - s.rotate_state[LOG_CAMERA_ID_ECAMERA].enabled = true; -#endif -#endif + if (Hardware::TICI()) { + encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_ECAMERA)); + s.rotate_state[LOG_CAMERA_ID_ECAMERA].enabled = true; + } uint64_t msg_count = 0; uint64_t bytes_count = 0; @@ -453,9 +438,7 @@ int main(int argc, char** argv) { new_segment &= (((r.stream_frame_id >= r.last_rotate_frame_id + SEGMENT_LENGTH * MAIN_FPS) && (!r.should_rotate) && (r.initialized)) || (!r.enabled)); -#ifndef QCOM2 - break; // only look at fcamera frame id if not QCOM2 -#endif + if (!Hardware::TICI()) break; // only look at fcamera frame id if not QCOM2 } } else { if (tms - last_rotate_tms > SEGMENT_LENGTH * 1000) { diff --git a/selfdrive/loggerd/omx_encoder.cc b/selfdrive/loggerd/omx_encoder.cc index b629490f5..6891d47d5 100644 --- a/selfdrive/loggerd/omx_encoder.cc +++ b/selfdrive/loggerd/omx_encoder.cc @@ -1,25 +1,24 @@ #pragma clang diagnostic ignored "-Wdeprecated-declarations" +#include "selfdrive/loggerd/omx_encoder.h" + +#include +#include +#include #include #include -#include -#include -#include #include -#include +#include #include #include -#include #include +#include +#include "libyuv.h" -#include -#include - -#include "common/util.h" -#include "common/swaglog.h" - -#include "omx_encoder.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/loggerd/include/msm_media_info.h" // Check the OMX error code and assert if an error occurred. #define OMX_CHECK(_expr) \ diff --git a/selfdrive/loggerd/omx_encoder.h b/selfdrive/loggerd/omx_encoder.h index 08bb30ec3..271ef113e 100644 --- a/selfdrive/loggerd/omx_encoder.h +++ b/selfdrive/loggerd/omx_encoder.h @@ -1,17 +1,18 @@ #pragma once -#include -#include #include -#include -#include +#include +#include +#include + +#include extern "C" { - #include +#include } -#include "encoder.h" -#include "common/queue.h" +#include "selfdrive/common/queue.h" +#include "selfdrive/loggerd/encoder.h" // OmxEncoder, lossey codec using hardware hevc class OmxEncoder : public VideoEncoder { diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc index 2e8fd20a7..cd2799773 100644 --- a/selfdrive/loggerd/raw_logger.cc +++ b/selfdrive/loggerd/raw_logger.cc @@ -1,24 +1,24 @@ #pragma clang diagnostic ignored "-Wdeprecated-declarations" -#include -#include -#include +#include "selfdrive/loggerd/raw_logger.h" #include #include +#include +#include +#include + #define __STDC_CONSTANT_MACROS extern "C" { -#include #include #include +#include } -#include "common/swaglog.h" -#include "common/util.h" - -#include "raw_logger.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" RawLogger::RawLogger(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h index 5df6eabed..7955ef5e1 100644 --- a/selfdrive/loggerd/raw_logger.h +++ b/selfdrive/loggerd/raw_logger.h @@ -2,20 +2,19 @@ #include #include - #include #include extern "C" { -#include #include #include +#include } -#include "encoder.h" +#include "selfdrive/loggerd/encoder.h" class RawLogger : public VideoEncoder { -public: + public: RawLogger(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale); ~RawLogger(); diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 4a8539dd3..86fdfaee9 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -192,14 +192,14 @@ class Uploader(): def uploader_fn(exit_event): params = Params() - dongle_id = params.get("DongleId").decode('utf8') + dongle_id = params.get("DongleId", encoding='utf8') if dongle_id is None: cloudlog.info("uploader missing dongle_id") raise Exception("uploader can't start without dongle id") if TICI and not Path("/data/media").is_mount(): - cloudlog.debug("NVME not mounted") + cloudlog.warning("NVME not mounted") sm = messaging.SubMaster(['deviceState']) uploader = Uploader(dongle_id, ROOT) @@ -207,7 +207,7 @@ def uploader_fn(exit_event): backoff = 0.1 while not exit_event.is_set(): sm.update(0) - offroad = params.get("IsOffroad") == b'1' + offroad = params.get_bool("IsOffroad") network_type = sm['deviceState'].networkType if not force_wifi else NetworkType.wifi if network_type == NetworkType.none: if allow_sleep: @@ -215,7 +215,7 @@ def uploader_fn(exit_event): continue on_wifi = network_type == NetworkType.wifi - allow_raw_upload = params.get("IsUploadRawEnabled") != b"0" + allow_raw_upload = params.get_bool("IsUploadRawEnabled") d = uploader.next_file_to_upload(with_raw=allow_raw_upload and on_wifi and offroad) if d is None: # Nothing to upload diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 393b51393..b568fee63 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -9,40 +9,39 @@ import traceback import cereal.messaging as messaging import selfdrive.crash as crash from common.basedir import BASEDIR -from common.params import Params +from common.params import Params, ParamKeyType from common.text_window import TextWindow -from selfdrive.hardware import HARDWARE +from selfdrive.boardd.set_time import set_time +from selfdrive.hardware import HARDWARE, PC, TICI from selfdrive.manager.helpers import unblock_stdout from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes -from selfdrive.registration import register +from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID from selfdrive.swaglog import cloudlog, add_file_handler -from selfdrive.version import dirty, version - +from selfdrive.version import dirty, get_git_commit, version, origin, branch, commit, \ + terms_version, training_version, comma_remote, \ + get_git_branch, get_git_remote def manager_init(): + + # update system time from panda + set_time(cloudlog) + params = Params() - params.manager_start() + params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) default_params = [ - ("CommunityFeaturesToggle", "0"), - ("EndToEndToggle", "0"), ("CompletedTrainingVersion", "0"), - ("IsRHD", "0"), - ("IsMetric", "0"), - ("RecordFront", "0"), ("HasAcceptedTerms", "0"), - ("HasCompletedSetup", "0"), - ("IsUploadRawEnabled", "1"), - ("IsLdwEnabled", "0"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), - ("VisionRadarToggle", "0"), - ("IsDriverViewEnabled", "0"), ] - if params.get("RecordFrontLock", encoding='utf-8') == "1": - params.put("RecordFront", "1") + if TICI: + default_params.append(("IsUploadRawEnabled", "1")) + + if params.get_bool("RecordFrontLock"): + params.put_bool("RecordFront", True) # set unset params for k, v in default_params: @@ -51,7 +50,7 @@ def manager_init(): # is this dashcam? if os.getenv("PASSIVE") is not None: - params.put("Passive", str(int(os.getenv("PASSIVE")))) + params.put_bool("Passive", bool(int(os.getenv("PASSIVE")))) if params.get("Passive") is None: raise Exception("Passive must be set to continue") @@ -66,21 +65,34 @@ def manager_init(): except PermissionError: print("WARNING: failed to make /dev/shm") + # set version params + params.put("Version", version) + params.put("TermsVersion", terms_version) + params.put("TrainingVersion", training_version) + params.put("GitCommit", get_git_commit(default="")) + params.put("GitBranch", get_git_branch(default="")) + params.put("GitRemote", get_git_remote(default="")) + # set dongle id reg_res = register(show_spinner=True) if reg_res: dongle_id = reg_res else: - raise Exception("server registration failed") - os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog and loggerd + serial = params.get("HardwareSerial") + raise Exception(f"Registration failed for device {serial}") + os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog if not dirty: os.environ['CLEAN'] = '1' cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, device=HARDWARE.get_device_type()) + + if comma_remote and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC): + crash.init() crash.bind_user(id=dongle_id) - crash.bind_extra(version=version, dirty=dirty, device=HARDWARE.get_device_type()) + crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit, + device=HARDWARE.get_device_type()) def manager_prepare(): @@ -102,7 +114,11 @@ def manager_thread(): # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) + params = Params() + ignore = [] + if params.get("DongleId") == UNREGISTERED_DONGLE_ID: + ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: @@ -111,7 +127,6 @@ def manager_thread(): ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False - params = Params() sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) @@ -123,7 +138,7 @@ def manager_thread(): not_run.append("loggerd") started = sm['deviceState'].started - driverview = params.get("IsDriverViewEnabled") == b"1" + driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad @@ -142,8 +157,9 @@ def manager_thread(): msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()] pm.send('managerState', msg) + # TODO: let UI handle this # Exit main loop when uninstall is needed - if params.get("DoUninstall", encoding='utf8') == "1": + if params.get_bool("DoUninstall"): break @@ -152,7 +168,7 @@ def main(): manager_init() - # Start ui early so prepare can happen in the background + # Start UI early so prepare can happen in the background if not prepare_only: managed_processes['ui'].start() @@ -172,7 +188,7 @@ def main(): finally: manager_cleanup() - if Params().get("DoUninstall", encoding='utf8') == "1": + if Params().get_bool("DoUninstall"): cloudlog.warning("uninstalling") HARDWARE.uninstall() diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index f0ae7da2b..b58f4dc2c 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -18,11 +18,11 @@ procs = [ NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=EON, sigkill=EON), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), NativeProcess("ui", "selfdrive/ui", ["./ui"], persistent=True, watchdog_max_dt=(10 if TICI else None)), + NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"), PythonProcess("controlsd", "selfdrive.controls.controlsd"), PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True), - PythonProcess("locationd", "selfdrive.locationd.locationd"), PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True), PythonProcess("pandad", "selfdrive.pandad", persistent=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index e19311f0b..fd8934850 100644 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -13,7 +13,7 @@ os.environ['FAKEUPLOAD'] = "1" # TODO: make eon fast MAX_STARTUP_TIME = 30 if EON else 15 -ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and (p.name not in ['updated', 'pandad'])] +ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['updated', 'pandad'])] class TestManager(unittest.TestCase): @@ -45,9 +45,12 @@ class TestManager(unittest.TestCase): time.sleep(30) for p in reversed(ALL_PROCESSES): + state = managed_processes[p].get_process_state_msg() + self.assertTrue(state.running, f"{p} not running") + exit_code = managed_processes[p].stop(retry=False) - if (not EON and p == 'ui') or (EON and p == 'logcatd'): - # TODO: make Qt UI exit gracefully and fix OMX encoder exiting + if (p == 'ui') or (EON and p == 'logcatd'): + # TODO: make Qt UI exit gracefully continue # Make sure the process is actually dead diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 44a2fc7f2..19c57f1dd 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -1,7 +1,8 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') lenv = env.Clone() -libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc] +libs = [cereal, messaging, common, visionipc, gpucommon, + 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'zmq', 'kj', 'yuv'] def get_dlsym_offset(): """Returns the offset between dlopen and dlsym in libdl.so""" diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index b21bbd784..85b412609 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -1,16 +1,12 @@ #include #include #include +#include -#include "visionipc_client.h" -#include "common/swaglog.h" -#include "common/util.h" - -#include "models/dmonitoring.h" - -#ifndef PATH_MAX -#include -#endif +#include "cereal/visionipc/visionipc_client.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/modeld/models/dmonitoring.h" ExitHandler do_exit; diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 4eb64cfd4..0cfed4acd 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -1,15 +1,18 @@ #include #include + #include + #include -#include "visionipc_client.h" -#include "common/swaglog.h" -#include "common/clutil.h" -#include "common/util.h" - -#include "models/driving.h" -#include "messaging.hpp" +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc_client.h" +#include "selfdrive/common/clutil.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/modeld/models/driving.h" ExitHandler do_exit; // globals @@ -17,7 +20,7 @@ bool live_calib_seen; mat3 cur_transform; std::mutex transform_lock; -void calibration_thread() { +void calibration_thread(bool wide_camera) { set_thread_name("calibration"); set_realtime_priority(50); @@ -35,19 +38,19 @@ void calibration_thread() { -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; - Eigen::Matrix fcam_intrinsics = Eigen::Matrix(fcam_intrinsic_matrix.v); + Eigen::Matrix cam_intrinsics = Eigen::Matrix(wide_camera ? ecam_intrinsic_matrix.v : fcam_intrinsic_matrix.v); const mat3 yuv_transform = get_model_yuv_transform(); while (!do_exit) { - if (sm.update(100) > 0){ - + sm.update(100); + if(sm.updated("liveCalibration")){ auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix(); Eigen::Matrix extrinsic_matrix_eigen; for (int i = 0; i < 4*3; i++){ extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; } - auto camera_frame_from_road_frame = fcam_intrinsics * extrinsic_matrix_eigen; + auto camera_frame_from_road_frame = cam_intrinsics * extrinsic_matrix_eigen; Eigen::Matrix camera_frame_from_ground; camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); @@ -72,10 +75,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) { SubMaster sm({"lateralPlan", "roadCameraState"}); // setup filter to track dropped frames - const float dt = 1. / MODEL_FREQ; - const float ts = 10.0; // filter time constant (s) - const float frame_filter_k = (dt / ts) / (1. + dt / ts); - float frames_dropped = 0; + FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ); uint32_t frame_id = 0, last_vipc_frame_id = 0; double last = 0; @@ -92,11 +92,10 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) { const bool run_model_this_iter = live_calib_seen; transform_lock.unlock(); - if (sm.update(0) > 0) { - // TODO: path planner timeout? - desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); - frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); - } + // TODO: path planner timeout? + sm.update(0); + desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); + frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); if (run_model_this_iter) { run_count++; @@ -114,8 +113,12 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) { // tracked dropped frames uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1; - frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U); - if (run_count < 10) frames_dropped = 0; // let frame drops warm up + float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U)); + if (run_count < 10) { // let frame drops warm up + frame_dropped_filter.reset(0); + frames_dropped = 0.; + } + float frame_drop_ratio = frames_dropped / (1 + frames_dropped); model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time, @@ -132,15 +135,15 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) { int main(int argc, char **argv) { set_realtime_priority(54); -#ifdef QCOM - set_core_affinity(2); -#elif QCOM2 - // CPU usage is much lower when pinned to a single big core - set_core_affinity(4); -#endif + if (Hardware::EON()) { + set_core_affinity(2); + } else if (Hardware::TICI()) { + set_core_affinity(7); + } + bool wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false; // start calibration thread - std::thread thread = std::thread(calibration_thread); + std::thread thread = std::thread(calibration_thread, wide_camera); // cl init cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); @@ -151,7 +154,7 @@ int main(int argc, char **argv) { model_init(&model, device_id, context); LOGW("models loaded, modeld starting"); - VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context); + VisionIpcClient vipc_client = VisionIpcClient("camerad", wide_camera ? VISION_STREAM_YUV_WIDE : VISION_STREAM_YUV_BACK, true, device_id, context); while (!do_exit && !vipc_client.connect(false)) { util::sleep_for(100); } diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 4ce255806..495bfe782 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -1,10 +1,13 @@ +#include "commonmodel.h" + #include #include + #include -#include "commonmodel.h" -#include "common/clutil.h" -#include "common/mat.h" -#include "common/timing.h" + +#include "selfdrive/common/clutil.h" +#include "selfdrive/common/mat.h" +#include "selfdrive/common/timing.h" void frame_init(ModelFrame* frame, int width, int height, cl_device_id device_id, cl_context context) { diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 05bb3e464..281930be6 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -1,4 +1,8 @@ #pragma once + +#include +#include + #define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ #include @@ -6,11 +10,9 @@ #include #endif -#include -#include -#include "common/mat.h" -#include "transforms/transform.h" -#include "transforms/loadyuv.h" +#include "selfdrive/common/mat.h" +#include "selfdrive/modeld/transforms/loadyuv.h" +#include "selfdrive/modeld/transforms/transform.h" const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 33900579f..9a1b8723a 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -1,10 +1,12 @@ #include -#include "dmonitoring.h" -#include "common/mat.h" -#include "common/timing.h" -#include "common/params.h" -#include +#include "libyuv.h" + +#include "selfdrive/common/mat.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/modeld/models/dmonitoring.h" #define MODEL_WIDTH 320 #define MODEL_HEIGHT 640 @@ -17,15 +19,10 @@ #endif void dmonitoring_init(DMonitoringModelState* s) { -#if defined(QCOM) || defined(QCOM2) - const char* model_path = "../../models/dmonitoring_model_q.dlc"; -#else - const char* model_path = "../../models/dmonitoring_model.dlc"; -#endif - + const char *model_path = "../../models/dmonitoring_model_q.dlc"; int runtime = USE_DSP_RUNTIME; s->m = new DefaultRunModel(model_path, &s->output[0], OUTPUT_SIZE, runtime); - s->is_rhd = Params().read_db_bool("IsRHD"); + s->is_rhd = Params().getBool("IsRHD"); } template @@ -41,70 +38,69 @@ static inline auto get_yuv_buf(std::vector &buf, const int width, int h return std::make_tuple(y, u, v); } +struct Rect {int x, y, w, h;}; +void crop_yuv(uint8_t *raw, int width, int height, uint8_t *y, uint8_t *u, uint8_t *v, const Rect &rect) { + uint8_t *raw_y = raw; + uint8_t *raw_u = raw_y + (width * height); + uint8_t *raw_v = raw_u + ((width / 2) * (height / 2)); + for (int r = 0; r < rect.h / 2; r++) { + memcpy(y + 2 * r * rect.w, raw_y + (2 * r + rect.y) * width + rect.x, rect.w); + memcpy(y + (2 * r + 1) * rect.w, raw_y + (2 * r + rect.y + 1) * width + rect.x, rect.w); + memcpy(u + r * (rect.w / 2), raw_u + (r + (rect.y / 2)) * width / 2 + (rect.x / 2), rect.w / 2); + memcpy(v + r * (rect.w / 2), raw_v + (r + (rect.y / 2)) * width / 2 + (rect.x / 2), rect.w / 2); + } +} + DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) { - uint8_t *raw_buf = (uint8_t*) stream_buf; - uint8_t *raw_y_buf = raw_buf; - uint8_t *raw_u_buf = raw_y_buf + (width * height); - uint8_t *raw_v_buf = raw_u_buf + ((width/2) * (height/2)); + Rect crop_rect; + if (Hardware::TICI()) { + const int full_width_tici = 1928; + const int full_height_tici = 1208; + const int adapt_width_tici = 668; + const int cropped_height = adapt_width_tici / 1.33; + crop_rect = {full_width_tici / 2 - adapt_width_tici / 2, + full_height_tici / 2 - cropped_height / 2 - 196, + cropped_height / 2, + cropped_height}; + if (!s->is_rhd) { + crop_rect.x += adapt_width_tici - crop_rect.w + 32; + } -#ifndef QCOM2 - const int cropped_width = height/2; - const int cropped_height = height; - const int global_x_offset = 0; - const int global_y_offset = 0; - const int crop_x_offset = width - cropped_width; - const int crop_y_offset = 0; -#else - const int full_width_tici = 1928; - const int full_height_tici = 1208; - const int adapt_width_tici = 668; - - const int cropped_height = adapt_width_tici / 1.33; - const int cropped_width = cropped_height / 2; - const int global_x_offset = full_width_tici / 2 - adapt_width_tici / 2; - const int global_y_offset = full_height_tici / 2 - cropped_height / 2; - const int crop_x_offset = adapt_width_tici - cropped_width + 32; - const int crop_y_offset = -196; -#endif + } else { + crop_rect = {0, 0, height / 2, height}; + if (!s->is_rhd) { + crop_rect.x += width - crop_rect.w; + } + } int resized_width = MODEL_WIDTH; int resized_height = MODEL_HEIGHT; - auto [cropped_y_buf, cropped_u_buf, cropped_v_buf] = get_yuv_buf(s->cropped_buf, cropped_width, cropped_height); + auto [cropped_y, cropped_u, cropped_v] = get_yuv_buf(s->cropped_buf, crop_rect.w, crop_rect.h); if (!s->is_rhd) { - for (int r = 0; r < cropped_height/2; r++) { - memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset)*width + global_x_offset + crop_x_offset, cropped_width); - memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset + 1)*width + global_x_offset + crop_x_offset, cropped_width); - memcpy(cropped_u_buf + r*(cropped_width/2), raw_u_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + (global_x_offset + crop_x_offset)/2, cropped_width/2); - memcpy(cropped_v_buf + r*(cropped_width/2), raw_v_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + (global_x_offset + crop_x_offset)/2, cropped_width/2); - } + crop_yuv((uint8_t *)stream_buf, width, height, cropped_y, cropped_u, cropped_v, crop_rect); } else { - auto [premirror_cropped_y_buf, premirror_cropped_u_buf, premirror_cropped_v_buf] = get_yuv_buf(s->premirror_cropped_buf, cropped_width, cropped_height); - for (int r = 0; r < cropped_height/2; r++) { - memcpy(premirror_cropped_y_buf + (2*r)*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset)*width + global_x_offset, cropped_width); - memcpy(premirror_cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset + 1)*width + global_x_offset, cropped_width); - memcpy(premirror_cropped_u_buf + r*(cropped_width/2), raw_u_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + global_x_offset/2, cropped_width/2); - memcpy(premirror_cropped_v_buf + r*(cropped_width/2), raw_v_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + global_x_offset/2, cropped_width/2); - } - libyuv::I420Mirror(premirror_cropped_y_buf, cropped_width, - premirror_cropped_u_buf, cropped_width/2, - premirror_cropped_v_buf, cropped_width/2, - cropped_y_buf, cropped_width, - cropped_u_buf, cropped_width/2, - cropped_v_buf, cropped_width/2, - cropped_width, cropped_height); + auto [mirror_y, mirror_u, mirror_v] = get_yuv_buf(s->premirror_cropped_buf, crop_rect.w, crop_rect.h); + crop_yuv((uint8_t *)stream_buf, width, height, mirror_y, mirror_u, mirror_v, crop_rect); + libyuv::I420Mirror(mirror_y, crop_rect.w, + mirror_u, crop_rect.w / 2, + mirror_v, crop_rect.w / 2, + cropped_y, crop_rect.w, + cropped_u, crop_rect.w / 2, + cropped_v, crop_rect.w / 2, + crop_rect.w, crop_rect.h); } - auto [resized_buf, resized_u_buf, resized_v_buf] = get_yuv_buf(s->resized_buf, resized_width, resized_height); - uint8_t *resized_y_buf = resized_buf; + auto [resized_buf, resized_u, resized_v] = get_yuv_buf(s->resized_buf, resized_width, resized_height); + uint8_t *resized_y = resized_buf; libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear; - libyuv::I420Scale(cropped_y_buf, cropped_width, - cropped_u_buf, cropped_width/2, - cropped_v_buf, cropped_width/2, - cropped_width, cropped_height, - resized_y_buf, resized_width, - resized_u_buf, resized_width/2, - resized_v_buf, resized_width/2, + libyuv::I420Scale(cropped_y, crop_rect.w, + cropped_u, crop_rect.w / 2, + cropped_v, crop_rect.w / 2, + crop_rect.w, crop_rect.h, + resized_y, resized_width, + resized_u, resized_width / 2, + resized_v, resized_width / 2, resized_width, resized_height, mode); diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index dd015d409..2a134b7b6 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -1,9 +1,11 @@ #pragma once + #include -#include "common/util.h" -#include "commonmodel.h" -#include "runners/run.h" -#include "messaging.hpp" + +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/util.h" +#include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/runners/run.h" #define OUTPUT_SIZE 38 diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 767bc2fe1..a54fd4788 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -1,14 +1,16 @@ -#include +#include "driving.h" + #include #include +#include #include + #include -#include "common/timing.h" -#include "common/params.h" -#include "driving.h" -#include "clutil.h" +#include "selfdrive/common/clutil.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/timing.h" constexpr int DESIRE_PRED_SIZE = 32; constexpr int OTHER_META_SIZE = 4; @@ -70,7 +72,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { #endif #ifdef TRAFFIC_CONVENTION - const int idx = Params().read_db_bool("IsRHD") ? 1 : 0; + const int idx = Params().getBool("IsRHD") ? 1 : 0; s->traffic_convention[idx] = 1.0; s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif @@ -180,40 +182,39 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da } void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, - int columns, int column_offset, float * plan_t_arr) { + int columns, int column_offset, float * plan_t_arr, bool fill_std) { float x_arr[TRAJECTORY_SIZE] = {}; float y_arr[TRAJECTORY_SIZE] = {}; float z_arr[TRAJECTORY_SIZE] = {}; - //float x_std_arr[TRAJECTORY_SIZE]; - //float y_std_arr[TRAJECTORY_SIZE]; - //float z_std_arr[TRAJECTORY_SIZE]; + float x_std_arr[TRAJECTORY_SIZE]; + float y_std_arr[TRAJECTORY_SIZE]; + float z_std_arr[TRAJECTORY_SIZE]; float t_arr[TRAJECTORY_SIZE]; for (int i=0; i= 0) { t_arr[i] = T_IDXS[i]; x_arr[i] = data[i*columns + 0 + column_offset]; - //x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset]; + x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset]; } else { t_arr[i] = plan_t_arr[i]; x_arr[i] = X_IDXS[i]; - //x_std_arr[i] = NAN; + x_std_arr[i] = NAN; } y_arr[i] = data[i*columns + 1 + column_offset]; - //y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset]; + y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset]; z_arr[i] = data[i*columns + 2 + column_offset]; - //z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; + z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; } - //kj::ArrayPtr x_std(x_std_arr, TRAJECTORY_SIZE); - //kj::ArrayPtr y_std(y_std_arr, TRAJECTORY_SIZE); - //kj::ArrayPtr z_std(z_std_arr, TRAJECTORY_SIZE); xyzt.setX(x_arr); xyzt.setY(y_arr); xyzt.setZ(z_arr); - //xyzt.setXStd(x_std); - //xyzt.setYStd(y_std); - //xyzt.setZStd(z_std); xyzt.setT(t_arr); + if (fill_std) { + xyzt.setXStd(x_std_arr); + xyzt.setYStd(y_std_arr); + xyzt.setZStd(z_std_arr); + } } void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) { @@ -224,17 +225,17 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15]; } - fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr); - fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr); - fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr); - fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr); + fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr, true); + fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr, false); + fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr, false); + fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr, false); // lane lines auto lane_lines = framed.initLaneLines(4); float lane_line_probs_arr[4]; float lane_line_stds_arr[4]; for (int i = 0; i < 4; i++) { - fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr); + fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false); lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]); lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]); } @@ -245,7 +246,7 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou auto road_edges = framed.initRoadEdges(2); float road_edge_stds_arr[2]; for (int i = 0; i < 2; i++) { - fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr); + fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false); road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]); } framed.setRoadEdgeStds(road_edge_stds_arr); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 7b0c21390..d523f47c7 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -5,15 +5,14 @@ #define DESIRE #define TRAFFIC_CONVENTION -#include "common/mat.h" -#include "common/util.h" -#include "common/modeldata.h" - -#include "commonmodel.h" -#include "runners/run.h" - #include -#include "messaging.hpp" + +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/mat.h" +#include "selfdrive/common/modeldata.h" +#include "selfdrive/common/util.h" +#include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/runners/run.h" constexpr int DESIRE_LEN = 8; constexpr int TRAFFIC_CONVENTION_LEN = 2; diff --git a/selfdrive/modeld/runners/run.h b/selfdrive/modeld/runners/run.h index 98a3fb8b6..3051dfb03 100644 --- a/selfdrive/modeld/runners/run.h +++ b/selfdrive/modeld/runners/run.h @@ -4,13 +4,13 @@ #include "snpemodel.h" #if defined(QCOM) || defined(QCOM2) - #include "thneedmodel.h" - #define DefaultRunModel SNPEModel +#include "thneedmodel.h" +#define DefaultRunModel SNPEModel #else - #ifdef USE_ONNX_MODEL - #include "onnxmodel.h" - #define DefaultRunModel ONNXModel - #else - #define DefaultRunModel SNPEModel - #endif +#ifdef USE_ONNX_MODEL +#include "onnxmodel.h" +#define DefaultRunModel ONNXModel +#else +#define DefaultRunModel SNPEModel +#endif #endif diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index d3889aaaa..9eb7e1ea6 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -1,6 +1,4 @@ -#ifndef RUNMODEL_H -#define RUNMODEL_H - +#pragma once class RunModel { public: virtual void addRecurrent(float *state, int state_size) {} @@ -9,5 +7,3 @@ public: virtual void execute(float *net_input_buf, int buf_size) {} }; -#endif - diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 538c24656..9f26ad27f 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -1,10 +1,13 @@ #pragma clang diagnostic ignored "-Wexceptions" -#include -#include +#include "selfdrive/modeld/runners/snpemodel.h" + #include -#include "common/util.h" -#include "snpemodel.h" +#include + +#include + +#include "selfdrive/common/util.h" void PrintErrorStringAndExit() { std::cerr << zdl::DlSystem::getLastErrorString() << std::endl; @@ -24,14 +27,13 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int } assert(zdl::SNPE::SNPEFactory::isRuntimeAvailable(Runtime)); #endif - size_t model_size; - model_data = (uint8_t *)read_file(path, &model_size); - assert(model_data); + model_data = util::read_file(path); + assert(model_data.size() > 0); // load model - std::unique_ptr container = zdl::DlContainer::IDlContainer::open(model_data, model_size); + std::unique_ptr container = zdl::DlContainer::IDlContainer::open((uint8_t*)model_data.data(), model_data.size()); if (!container) { PrintErrorStringAndExit(); } - printf("loaded model with size: %lu\n", model_size); + printf("loaded model with size: %lu\n", model_data.size()); // create model runner zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index 76339642f..6d3a70414 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -1,15 +1,14 @@ -#ifndef SNPEMODEL_H -#define SNPEMODEL_H +#pragma once -#include -#include -#include #include #include #include #include #include #include +#include +#include +#include #include "runmodel.h" @@ -18,15 +17,12 @@ #define USE_DSP_RUNTIME 2 #ifdef USE_THNEED -#include "thneed/thneed.h" +#include "selfdrive/modeld/thneed/thneed.h" #endif class SNPEModel : public RunModel { public: SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime); - ~SNPEModel() { - if (model_data) free(model_data); - } void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); @@ -37,7 +33,7 @@ public: #endif private: - uint8_t *model_data = NULL; + std::string model_data; #if defined(QCOM) || defined(QCOM2) zdl::DlSystem::Runtime_t Runtime; @@ -66,6 +62,3 @@ private: float *desire; std::unique_ptr desireBuffer; }; - -#endif - diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc index 0ebe7226e..c83a58b02 100644 --- a/selfdrive/modeld/runners/thneedmodel.cc +++ b/selfdrive/modeld/runners/thneedmodel.cc @@ -1,4 +1,5 @@ #include "thneedmodel.h" + #include ThneedModel::ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime) { diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h index 05cb2438e..933c751d9 100644 --- a/selfdrive/modeld/runners/thneedmodel.h +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -1,7 +1,7 @@ #pragma once -#include "runmodel.h" -#include "thneed/thneed.h" +#include "selfdrive/modeld/runners/runmodel.h" +#include "selfdrive/modeld/thneed/thneed.h" class ThneedModel : public RunModel { public: diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc index 63955a6f9..cf15e9ceb 100644 --- a/selfdrive/modeld/thneed/compile.cc +++ b/selfdrive/modeld/thneed/compile.cc @@ -1,6 +1,7 @@ #include -#include "thneed.h" -#include "../runners/snpemodel.h" + +#include "selfdrive/modeld/runners/snpemodel.h" +#include "selfdrive/modeld/thneed/thneed.h" #define TEMPORAL_SIZE 512 #define DESIRE_LEN 8 diff --git a/selfdrive/modeld/thneed/serialize.cc b/selfdrive/modeld/thneed/serialize.cc index 7f22d631f..e3aa099d1 100644 --- a/selfdrive/modeld/thneed/serialize.cc +++ b/selfdrive/modeld/thneed/serialize.cc @@ -1,7 +1,9 @@ -#include #include -#include "thneed.h" + +#include + #include "json11.hpp" +#include "selfdrive/modeld/thneed/thneed.h" using namespace json11; extern map g_program_source; diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc index e11fcb8a9..3b0138a14 100644 --- a/selfdrive/modeld/thneed/thneed.cc +++ b/selfdrive/modeld/thneed/thneed.cc @@ -1,13 +1,16 @@ -#include -#include +#include "selfdrive/modeld/thneed/thneed.h" + #include +#include +#include +#include + +#include #include #include -#include -#include -#include "common/timing.h" -#include "common/clutil.h" -#include "thneed.h" + +#include "selfdrive/common/clutil.h" +#include "selfdrive/common/timing.h" //#define RUN_DISASSEMBLER //#define RUN_OPTIMIZER diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h index c36aaff70..eaea22e41 100644 --- a/selfdrive/modeld/thneed/thneed.h +++ b/selfdrive/modeld/thneed/thneed.h @@ -1,16 +1,18 @@ #pragma once #ifndef __user - #define __user __attribute__(()) +#define __user __attribute__(()) #endif -#include +#include #include -#include "include/msm_kgsl.h" -#include +#include + #include #include -#include +#include + +#include "selfdrive/modeld/thneed/include/msm_kgsl.h" #define THNEED_RECORD 1 #define THNEED_DEBUG 2 diff --git a/selfdrive/modeld/transforms/loadyuv.cc b/selfdrive/modeld/transforms/loadyuv.cc index fe84e6d27..d3686a667 100644 --- a/selfdrive/modeld/transforms/loadyuv.cc +++ b/selfdrive/modeld/transforms/loadyuv.cc @@ -1,7 +1,8 @@ -#include -#include #include "loadyuv.h" +#include +#include + void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) { memset(s, 0, sizeof(*s)); diff --git a/selfdrive/modeld/transforms/loadyuv.h b/selfdrive/modeld/transforms/loadyuv.h index dbef46149..0153815cc 100644 --- a/selfdrive/modeld/transforms/loadyuv.h +++ b/selfdrive/modeld/transforms/loadyuv.h @@ -2,7 +2,8 @@ #include #include -#include "clutil.h" + +#include "selfdrive/common/clutil.h" typedef struct { int width, height; diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc index 056267aca..6cf48220a 100644 --- a/selfdrive/modeld/transforms/transform.cc +++ b/selfdrive/modeld/transforms/transform.cc @@ -1,10 +1,10 @@ -#include -#include - -#include "clutil.h" - #include "transform.h" +#include +#include + +#include "selfdrive/common/clutil.h" + void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) { memset(s, 0, sizeof(*s)); diff --git a/selfdrive/modeld/transforms/transform.h b/selfdrive/modeld/transforms/transform.h index 3bce6f8a7..f46096bb0 100644 --- a/selfdrive/modeld/transforms/transform.h +++ b/selfdrive/modeld/transforms/transform.h @@ -10,7 +10,7 @@ #include #endif -#include "common/mat.h" +#include "selfdrive/common/mat.h" typedef struct { cl_kernel krnl; diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 83313b820..8321d3734 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -14,7 +14,7 @@ def dmonitoringd_thread(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) - driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1") + driver_status = DriverStatus(rhd=Params().get_bool("IsRHD")) sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 1e7c01203..ff7bdefb9 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -10,7 +10,7 @@ EventName = car.CarEvent.EventName # ****************************************************************************************** # NOTE: To fork maintainers. -# Disabling or nerfing safety features may get you and your users banned from our servers. +# Disabling or nerfing safety features will get you and your users banned from our servers. # We recommend that you do not change these numbers from the defaults. # ****************************************************************************************** @@ -30,6 +30,8 @@ _BLINK_THRESHOLD_SLACK = 0.65 _BLINK_THRESHOLD_STRICT = 0.5 _PITCH_WEIGHT = 1.35 # pitch matters a lot more _POSESTD_THRESHOLD = 0.14 +_E2E_POSE_THRESHOLD = 0.9 +_E2E_EYES_THRESHOLD = 0.75 _METRIC_THRESHOLD = 0.4 _METRIC_THRESHOLD_SLACK = 0.55 _METRIC_THRESHOLD_STRICT = 0.4 @@ -194,8 +196,10 @@ class DriverStatus(): self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sunglassesProb < _SG_THRESHOLD) self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD) * (driver_state.sunglassesProb < _SG_THRESHOLD) - self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \ - driver_state.faceProb > _FACE_THRESHOLD and self.pose.low_std + self.driver_distracted = (self._is_driver_distracted(self.pose, self.blink) > 0 and + driver_state.faceProb > _FACE_THRESHOLD and self.pose.low_std) or \ + ((driver_state.distractedPose > _E2E_POSE_THRESHOLD or driver_state.distractedEyes > _E2E_EYES_THRESHOLD) and + (self.face_detected and not self.face_partial)) self.driver_distraction_filter.update(self.driver_distracted) # update offseter @@ -209,7 +213,7 @@ class DriverStatus(): self.is_model_uncertain = self.hi_stds * DT_DMON > _HI_STD_FALLBACK_TIME self._set_timers(self.face_detected and not self.is_model_uncertain) - if self.face_detected and not self.pose.low_std: + if self.face_detected and not self.pose.low_std and not self.driver_distracted: self.hi_stds += 1 elif self.face_detected and self.pose.low_std: self.hi_stds = 0 diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index d4cc98474..fd9d89fdb 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -5,29 +5,11 @@ import time from panda import BASEDIR as PANDA_BASEDIR, Panda, PandaDFU from common.basedir import BASEDIR -from common.gpio import gpio_init, gpio_set -from selfdrive.hardware import TICI -from selfdrive.hardware.tici.pins import GPIO_HUB_RST_N, GPIO_STM_BOOT0, GPIO_STM_RST_N from selfdrive.swaglog import cloudlog PANDA_FW_FN = os.path.join(PANDA_BASEDIR, "board", "obj", "panda.bin.signed") -def set_panda_power(power=True): - if not TICI: - return - - gpio_init(GPIO_STM_RST_N, True) - gpio_init(GPIO_STM_BOOT0, True) - - gpio_set(GPIO_STM_RST_N, True) - gpio_set(GPIO_HUB_RST_N, True) - - time.sleep(0.1) - - gpio_set(GPIO_STM_RST_N, not power) - - def get_expected_signature(): try: return Panda.get_signature_from_firmware(PANDA_FW_FN) @@ -100,7 +82,6 @@ def update_panda(): def main(): - set_panda_power() update_panda() os.chdir(os.path.join(BASEDIR, "selfdrive/boardd")) diff --git a/selfdrive/proclogd/SConscript b/selfdrive/proclogd/SConscript index b80d17200..f95f2597e 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', 'capnp', 'kj']) +Import('env', 'cereal', 'messaging', 'common') +env.Program('proclogd.cc', LIBS=[cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj', 'common']) diff --git a/selfdrive/proclogd/proclogd.cc b/selfdrive/proclogd/proclogd.cc index 66f2f5025..af7a6e3f2 100644 --- a/selfdrive/proclogd/proclogd.cc +++ b/selfdrive/proclogd/proclogd.cc @@ -1,22 +1,22 @@ -#include #include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include #include -#include #include +#include +#include #include +#include -#include "messaging.hpp" - -#include "common/timing.h" -#include "common/util.h" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" ExitHandler do_exit; @@ -30,6 +30,8 @@ struct ProcCache { } int main() { + setpriority(PRIO_PROCESS, 0, -15); + PubMaster publisher({"procLog"}); double jiffy = sysconf(_SC_CLK_TCK); diff --git a/selfdrive/sensord/libdiag.h b/selfdrive/sensord/libdiag.h index ab3ee91b1..03a59464e 100644 --- a/selfdrive/sensord/libdiag.h +++ b/selfdrive/sensord/libdiag.h @@ -1,5 +1,4 @@ -#ifndef LIBDIAG_H -#define LIBDIAG_H +#pragma once #include #include @@ -36,5 +35,3 @@ int diag_send_dci_async_req(int client_id, unsigned char buf[], int bytes, unsig #ifdef __cplusplus } #endif - -#endif diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index 23e6d071e..590fa51e8 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -1,9 +1,9 @@ +#include "bmx055_accel.h" + #include -#include "common/swaglog.h" -#include "common/timing.h" - -#include "bmx055_accel.hpp" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} diff --git a/selfdrive/sensord/sensors/bmx055_accel.hpp b/selfdrive/sensord/sensors/bmx055_accel.h similarity index 95% rename from selfdrive/sensord/sensors/bmx055_accel.hpp rename to selfdrive/sensord/sensors/bmx055_accel.h index 2c9173f98..86ec419cd 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.hpp +++ b/selfdrive/sensord/sensors/bmx055_accel.h @@ -1,6 +1,6 @@ #pragma once -#include "sensors/i2c_sensor.hpp" +#include "selfdrive/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define BMX055_ACCEL_I2C_ADDR 0x18 diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index d7bf00401..c8e9e3565 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -1,8 +1,9 @@ +#include "bmx055_gyro.h" + #include #include -#include "common/swaglog.h" -#include "bmx055_gyro.hpp" +#include "selfdrive/common/swaglog.h" #define DEG2RAD(x) ((x) * M_PI / 180.0) diff --git a/selfdrive/sensord/sensors/bmx055_gyro.hpp b/selfdrive/sensord/sensors/bmx055_gyro.h similarity index 95% rename from selfdrive/sensord/sensors/bmx055_gyro.hpp rename to selfdrive/sensord/sensors/bmx055_gyro.h index f808139fd..ed0c16ff0 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.hpp +++ b/selfdrive/sensord/sensors/bmx055_gyro.h @@ -1,6 +1,6 @@ #pragma once -#include "sensors/i2c_sensor.hpp" +#include "selfdrive/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define BMX055_GYRO_I2C_ADDR 0x68 diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index 41b41467b..438e5b494 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -1,12 +1,13 @@ -#include -#include -#include +#include "bmx055_magn.h" + #include -#include "common/swaglog.h" -#include "common/util.h" +#include +#include +#include -#include "bmx055_magn.hpp" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) { uint16_t process_comp_x0 = data_rhall; diff --git a/selfdrive/sensord/sensors/bmx055_magn.hpp b/selfdrive/sensord/sensors/bmx055_magn.h similarity index 97% rename from selfdrive/sensord/sensors/bmx055_magn.hpp rename to selfdrive/sensord/sensors/bmx055_magn.h index 0e11934e6..d60fd5515 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.hpp +++ b/selfdrive/sensord/sensors/bmx055_magn.h @@ -1,7 +1,7 @@ #pragma once #include -#include "sensors/i2c_sensor.hpp" +#include "selfdrive/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define BMX055_MAGN_I2C_ADDR 0x10 diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/selfdrive/sensord/sensors/bmx055_temp.cc index 86f7d4f0c..b5cb893a0 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.cc +++ b/selfdrive/sensord/sensors/bmx055_temp.cc @@ -1,10 +1,10 @@ +#include "bmx055_temp.h" + #include -#include "common/swaglog.h" -#include "common/timing.h" - -#include "bmx055_temp.hpp" -#include "bmx055_accel.hpp" +#include "selfdrive/sensord/sensors/bmx055_accel.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {} diff --git a/selfdrive/sensord/sensors/bmx055_temp.hpp b/selfdrive/sensord/sensors/bmx055_temp.h similarity index 69% rename from selfdrive/sensord/sensors/bmx055_temp.hpp rename to selfdrive/sensord/sensors/bmx055_temp.h index 8b7119a60..5ffaa8fb6 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.hpp +++ b/selfdrive/sensord/sensors/bmx055_temp.h @@ -1,8 +1,7 @@ #pragma once -#include "sensors/i2c_sensor.hpp" -#include "sensors/bmx055_accel.hpp" - +#include "selfdrive/sensord/sensors/bmx055_accel.h" +#include "selfdrive/sensord/sensors/i2c_sensor.h" class BMX055_Temp : public I2CSensor { uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} diff --git a/selfdrive/sensord/sensors/constants.hpp b/selfdrive/sensord/sensors/constants.h similarity index 100% rename from selfdrive/sensord/sensors/constants.hpp rename to selfdrive/sensord/sensors/constants.h diff --git a/selfdrive/sensord/sensors/file_sensor.cc b/selfdrive/sensord/sensors/file_sensor.cc index 6d03ef1b1..a0ec1d71d 100644 --- a/selfdrive/sensord/sensors/file_sensor.cc +++ b/selfdrive/sensord/sensors/file_sensor.cc @@ -1,8 +1,8 @@ +#include "file_sensor.h" + #include #include -#include "file_sensor.hpp" - FileSensor::FileSensor(std::string filename) : file(filename) { } diff --git a/selfdrive/sensord/sensors/file_sensor.hpp b/selfdrive/sensord/sensors/file_sensor.h similarity index 87% rename from selfdrive/sensord/sensors/file_sensor.hpp rename to selfdrive/sensord/sensors/file_sensor.h index 25a6f203c..c5b4643e1 100644 --- a/selfdrive/sensord/sensors/file_sensor.hpp +++ b/selfdrive/sensord/sensors/file_sensor.h @@ -4,8 +4,7 @@ #include #include "cereal/gen/cpp/log.capnp.h" -#include "sensors/sensor.hpp" - +#include "selfdrive/sensord/sensors/sensor.h" class FileSensor : public Sensor { protected: diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc index e3000c8b0..64a970c59 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.cc +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -1,5 +1,6 @@ +#include "i2c_sensor.h" + #include -#include "i2c_sensor.hpp" int16_t read_12_bit(uint8_t lsb, uint8_t msb){ uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0); diff --git a/selfdrive/sensord/sensors/i2c_sensor.hpp b/selfdrive/sensord/sensors/i2c_sensor.h similarity index 80% rename from selfdrive/sensord/sensors/i2c_sensor.hpp rename to selfdrive/sensord/sensors/i2c_sensor.h index 39ef79cf8..98e2e2f85 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.hpp +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -1,10 +1,11 @@ #pragma once #include + #include "cereal/gen/cpp/log.capnp.h" -#include "common/i2c.h" -#include "sensors/sensor.hpp" -#include "sensors/constants.hpp" +#include "selfdrive/common/i2c.h" +#include "selfdrive/sensord/sensors/constants.h" +#include "selfdrive/sensord/sensors/sensor.h" int16_t read_12_bit(uint8_t lsb, uint8_t msb); int16_t read_16_bit(uint8_t lsb, uint8_t msb); diff --git a/selfdrive/sensord/sensors/light_sensor.cc b/selfdrive/sensord/sensors/light_sensor.cc index eb3eb05f1..80930e075 100644 --- a/selfdrive/sensord/sensors/light_sensor.cc +++ b/selfdrive/sensord/sensors/light_sensor.cc @@ -1,10 +1,10 @@ +#include "light_sensor.h" + #include #include -#include "common/timing.h" - -#include "light_sensor.hpp" -#include "constants.hpp" +#include "selfdrive/common/timing.h" +#include "selfdrive/sensord/sensors/constants.h" void LightSensor::get_event(cereal::SensorEventData::Builder &event){ uint64_t start_time = nanos_since_boot(); diff --git a/selfdrive/sensord/sensors/light_sensor.hpp b/selfdrive/sensord/sensors/light_sensor.h similarity index 87% rename from selfdrive/sensord/sensors/light_sensor.hpp rename to selfdrive/sensord/sensors/light_sensor.h index 7c98cb29c..faf901d41 100644 --- a/selfdrive/sensord/sensors/light_sensor.hpp +++ b/selfdrive/sensord/sensors/light_sensor.h @@ -1,5 +1,5 @@ #pragma once -#include "file_sensor.hpp" +#include "file_sensor.h" class LightSensor : public FileSensor { public: diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index 15a186725..27b882a4f 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -1,9 +1,9 @@ +#include "lsm6ds3_accel.h" + #include -#include "common/swaglog.h" -#include "common/timing.h" - -#include "lsm6ds3_accel.hpp" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus) : I2CSensor(bus) {} diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.hpp b/selfdrive/sensord/sensors/lsm6ds3_accel.h similarity index 92% rename from selfdrive/sensord/sensors/lsm6ds3_accel.hpp rename to selfdrive/sensord/sensors/lsm6ds3_accel.h index 314dcf8b5..e5a949d4f 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.hpp +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.h @@ -1,6 +1,6 @@ #pragma once -#include "sensors/i2c_sensor.hpp" +#include "selfdrive/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define LSM6DS3_ACCEL_I2C_ADDR 0x6A diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index 4c983e544..d38335eb6 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -1,9 +1,10 @@ +#include "lsm6ds3_gyro.h" + #include #include -#include "common/swaglog.h" -#include "common/timing.h" -#include "lsm6ds3_gyro.hpp" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" #define DEG2RAD(x) ((x) * M_PI / 180.0) diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.hpp b/selfdrive/sensord/sensors/lsm6ds3_gyro.h similarity index 91% rename from selfdrive/sensord/sensors/lsm6ds3_gyro.hpp rename to selfdrive/sensord/sensors/lsm6ds3_gyro.h index 13cec204e..cb8595e76 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.hpp +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.h @@ -1,6 +1,6 @@ #pragma once -#include "sensors/i2c_sensor.hpp" +#include "selfdrive/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define LSM6DS3_GYRO_I2C_ADDR 0x6A diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc index 04b0d3d4f..3b9e90f5e 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -1,9 +1,9 @@ +#include "lsm6ds3_temp.h" + #include -#include "common/swaglog.h" -#include "common/timing.h" - -#include "lsm6ds3_temp.hpp" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {} diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.hpp b/selfdrive/sensord/sensors/lsm6ds3_temp.h similarity index 90% rename from selfdrive/sensord/sensors/lsm6ds3_temp.hpp rename to selfdrive/sensord/sensors/lsm6ds3_temp.h index eea5ff4a6..c556631de 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.hpp +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.h @@ -1,6 +1,6 @@ #pragma once -#include "sensors/i2c_sensor.hpp" +#include "selfdrive/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus #define LSM6DS3_TEMP_I2C_ADDR 0x6A diff --git a/selfdrive/sensord/sensors/sensor.hpp b/selfdrive/sensord/sensors/sensor.h similarity index 100% rename from selfdrive/sensord/sensors/sensor.hpp rename to selfdrive/sensord/sensors/sensor.h diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc index 77e670d3f..81bb78e85 100644 --- a/selfdrive/sensord/sensors_qcom.cc +++ b/selfdrive/sensord/sensors_qcom.cc @@ -1,25 +1,24 @@ -#include +#include +#include +#include #include +#include #include #include -#include -#include -#include #include -#include #include +#include +#include +#include +#include #include #include -#include -#include -#include - -#include "messaging.hpp" -#include "common/timing.h" -#include "common/util.h" -#include "common/swaglog.h" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" // ACCELEROMETER_UNCALIBRATED is only in Android O // https://developer.android.com/reference/android/hardware/Sensor.html#STRING_TYPE_ACCELEROMETER_UNCALIBRATED @@ -197,7 +196,8 @@ void sensor_loop() { } // Check whether to go into low power mode at 5Hz - if (frame % 20 == 0 && sm.update(0) > 0) { + if (frame % 20 == 0) { + sm.update(0); bool offroad = !sm["deviceState"].getDeviceState().getStarted(); if (low_power_mode != offroad) { for (auto &s : sensors) { @@ -219,7 +219,7 @@ void sensor_loop() { }// Namespace end int main(int argc, char *argv[]) { - setpriority(PRIO_PROCESS, 0, -13); + setpriority(PRIO_PROCESS, 0, -18); signal(SIGPIPE, (sighandler_t)sigpipe_handler); sensor_loop(); diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 1cc685fe3..d0f8cf00e 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -1,27 +1,24 @@ -#include -#include -#include #include -#include "messaging.hpp" -#include "common/i2c.h" -#include "common/timing.h" -#include "common/util.h" -#include "common/swaglog.h" +#include +#include +#include -#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" +#include "cereal/messaging/messaging.h" +#include "selfdrive/common/i2c.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" +#include "selfdrive/sensord/sensors/bmx055_accel.h" +#include "selfdrive/sensord/sensors/bmx055_gyro.h" +#include "selfdrive/sensord/sensors/bmx055_magn.h" +#include "selfdrive/sensord/sensors/bmx055_temp.h" +#include "selfdrive/sensord/sensors/constants.h" +#include "selfdrive/sensord/sensors/light_sensor.h" +#include "selfdrive/sensord/sensors/lsm6ds3_accel.h" +#include "selfdrive/sensord/sensors/lsm6ds3_gyro.h" +#include "selfdrive/sensord/sensors/lsm6ds3_temp.h" +#include "selfdrive/sensord/sensors/sensor.h" #define I2C_BUS_IMU 1 @@ -93,6 +90,6 @@ int sensor_loop() { } int main(int argc, char *argv[]) { - setpriority(PRIO_PROCESS, 0, -13); + setpriority(PRIO_PROCESS, 0, -18); return sensor_loop(); } diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index fda1a3175..6a7c1a837 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -11,11 +11,10 @@ def set_params_enabled(): from common.params import Params params = Params() params.put("HasAcceptedTerms", terms_version) - params.put("HasCompletedSetup", "1") - params.put("OpenpilotEnabledToggle", "1") - params.put("CommunityFeaturesToggle", "1") - params.put("Passive", "0") params.put("CompletedTrainingVersion", training_version) + params.put_bool("OpenpilotEnabledToggle", True) + params.put_bool("CommunityFeaturesToggle", True) + params.put_bool("Passive", False) def phone_only(x): @@ -25,7 +24,9 @@ def phone_only(x): return x -def with_processes(processes, init_time=0): +def with_processes(processes, init_time=0, ignore_stopped=None): + ignore_stopped = [] if ignore_stopped is None else ignore_stopped + def wrapper(func): @wraps(func) def wrap(*args, **kwargs): @@ -40,7 +41,7 @@ def with_processes(processes, init_time=0): try: func(*args, **kwargs) # assert processes are still started - assert all(managed_processes[name].proc.exitcode is None for name in processes) + assert all(managed_processes[name].proc.exitcode is None for name in processes if name not in ignore_stopped) finally: for p in processes: managed_processes[p].stop() diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index 435a6af43..cc04504ad 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -26,8 +26,8 @@ if [ ! -d "$SOURCE_DIR" ]; then git clone --depth 1 https://github.com/commaai/openpilot.git "$SOURCE_DIR" fi -# clear scons cache dirs that haven't been written to in one day -#cd /tmp && find -name 'scons_cache_*' -type d -maxdepth 1 -mtime +1 -exec rm -rf '{}' \; +# clear stale build cache +find /tmp/scons_cache*/* -mtime +1 -exec rm -rf '{}' \; || true # this can get really big on the CI devices rm -rf /data/core diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py new file mode 100755 index 000000000..078b1cb4b --- /dev/null +++ b/selfdrive/test/test_onroad.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 +import json +import os +import subprocess +import time +import unittest +from collections import Counter +from pathlib import Path + +import cereal.messaging as messaging +from cereal.services import service_list +from common.basedir import BASEDIR +from common.timeout import Timeout +from selfdrive.hardware import TICI +from selfdrive.loggerd.config import ROOT +from selfdrive.test.helpers import set_params_enabled +from tools.lib.logreader import LogReader + +# Baseline CPU usage by process +PROCS = { + "selfdrive.controls.controlsd": 50.0, + "./loggerd": 45.0, + "./locationd": 9.1, + "selfdrive.controls.plannerd": 20.0, + "./_ui": 15.0, + "selfdrive.locationd.paramsd": 9.1, + "./camerad": 7.07, + "./_sensord": 6.17, + "selfdrive.controls.radard": 5.67, + "./_modeld": 4.48, + "./boardd": 3.63, + "./_dmonitoringmodeld": 2.67, + "selfdrive.thermald.thermald": 2.41, + "selfdrive.locationd.calibrationd": 2.0, + "selfdrive.monitoring.dmonitoringd": 1.90, + "./proclogd": 1.54, + "selfdrive.logmessaged": 0.2, + "./clocksd": 0.02, + "./ubloxd": 0.02, + "selfdrive.tombstoned": 0, + "./logcatd": 0, +} + +if TICI: + PROCS.update({ + "./loggerd": 60.0, + "selfdrive.controls.controlsd": 26.0, + "./camerad": 25.0, + "selfdrive.controls.plannerd": 12.0, + "selfdrive.locationd.paramsd": 5.0, + "./_dmonitoringmodeld": 10.0, + "selfdrive.thermald.thermald": 1.5, + }) + + +def cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + + +def check_cpu_usage(first_proc, last_proc): + result = "------------------------------------------------\n" + result += "------------------ CPU Usage -------------------\n" + result += "------------------------------------------------\n" + + r = True + dt = (last_proc.logMonoTime - first_proc.logMonoTime) / 1e9 + for proc_name, normal_cpu_usage in PROCS.items(): + first, last = None, None + try: + first = [p for p in first_proc.procLog.procs if proc_name in p.cmdline][0] + last = [p for p in last_proc.procLog.procs if proc_name in p.cmdline][0] + cpu_time = cputime_total(last) - cputime_total(first) + cpu_usage = cpu_time / dt * 100. + if cpu_usage > max(normal_cpu_usage * 1.1, normal_cpu_usage + 5.0): + # TODO: fix high CPU when playing sounds constantly in UI + if proc_name == "./_ui" and cpu_usage < 50.: + continue + result += f"Warning {proc_name} using more CPU than normal\n" + r = False + elif cpu_usage < min(normal_cpu_usage * 0.65, max(normal_cpu_usage - 1.0, 0.0)): + result += f"Warning {proc_name} using less CPU than normal\n" + r = False + result += f"{proc_name.ljust(35)} {cpu_usage:.2f}%\n" + except IndexError: + result += f"{proc_name.ljust(35)} NO METRICS FOUND {first=} {last=}\n" + r = False + result += "------------------------------------------------\n" + print(result) + return r + + +class TestOnroad(unittest.TestCase): + + @classmethod + def setUpClass(cls): + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" + set_params_enabled() + + logger_root = Path(ROOT) + initial_segments = set() + if logger_root.exists(): + initial_segments = set(Path(ROOT).iterdir()) + + # start manager and run openpilot for a minute + try: + manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") + proc = subprocess.Popen(["python", manager_path]) + + sm = messaging.SubMaster(['carState']) + with Timeout(150, "controls didn't start"): + while sm.rcv_frame['carState'] < 0: + sm.update(1000) + + # make sure we get at least two full segments + cls.segments = [] + with Timeout(300, "timed out waiting for logs"): + while len(cls.segments) < 3: + new_paths = set() + if logger_root.exists(): + new_paths = set(logger_root.iterdir()) - initial_segments + segs = [p for p in new_paths if "--" in str(p)] + cls.segments = sorted(segs, key=lambda s: int(str(s).rsplit('--')[-1])) + time.sleep(5) + + finally: + proc.terminate() + if proc.wait(60) is None: + proc.kill() + + cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog.bz2"))) + + def test_cloudlog_size(self): + msgs = [m for m in self.lr if m.which() == 'logMessage'] + + total_size = sum(len(m.as_builder().to_bytes()) for m in msgs) + self.assertLess(total_size, 2.5e5) + + cnt = Counter([json.loads(m.logMessage)['filename'] for m in msgs]) + big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.] + self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}") + + def test_cpu_usage(self): + proclogs = [m for m in self.lr if m.which() == 'procLog'] + self.assertGreater(len(proclogs), service_list['procLog'].frequency * 45, "insufficient samples") + cpu_ok = check_cpu_usage(proclogs[0], proclogs[-1]) + self.assertTrue(cpu_ok) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 485ebd2de..f8980eda3 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -16,7 +16,8 @@ CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) CAR_BATTERY_CAPACITY_uWh = 30e6 CAR_CHARGING_RATE_W = 45 -VBATT_PAUSE_CHARGING = 11.0 +VBATT_PAUSE_CHARGING = 11.0 # Lower limit on the LPF car battery voltage +VBATT_INSTANT_PAUSE_CHARGING = 7.0 # Lower limit on the instant car battery voltage measurements to avoid triggering on instant power loss MAX_TIME_OFFROAD_S = 30*3600 class PowerMonitoring: @@ -27,6 +28,7 @@ class PowerMonitoring: self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad self.next_pulsed_measurement_time = None self.car_voltage_mV = 12e3 # Low-passed version of pandaState voltage + self.car_voltage_instant_mV = 12e3 # Last value of pandaState voltage self.integration_lock = threading.Lock() car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") @@ -51,6 +53,7 @@ class PowerMonitoring: return # Low-pass battery voltage + self.car_voltage_instant_mV = pandaState.pandaState.voltage self.car_voltage_mV = ((pandaState.pandaState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) # Cap the car battery power and save it in a param every 10-ish seconds @@ -161,11 +164,11 @@ class PowerMonitoring: now = sec_since_boot() disable_charging = False disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S - disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) + disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) and (self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3)) disable_charging |= (self.car_battery_capacity_uWh <= 0) disable_charging &= (not pandaState.pandaState.ignitionLine and not pandaState.pandaState.ignitionCan) - disable_charging &= (self.params.get("DisablePowerDown") != b"1") - disable_charging |= (self.params.get("ForcePowerDown") == b"1") + disable_charging &= (not self.params.get_bool("DisablePowerDown")) + disable_charging |= self.params.get_bool("ForcePowerDown") return disable_charging # See if we need to shutdown diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 78a535083..b7480a9ce 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -12,7 +12,7 @@ import cereal.messaging as messaging from cereal import log from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp -from common.params import Params +from common.params import Params, ParamKeyType from common.realtime import DT_TRML, sec_since_boot from common.dict_helpers import strip_deprecated_keys from selfdrive.controls.lib.alertmanager import set_offroad_alert @@ -25,8 +25,6 @@ from selfdrive.version import get_git_branch, terms_version, training_version FW_SIGNATURE = get_expected_signature() -DISABLE_LTE_ONROAD = os.path.exists("/persist/disable_lte_onroad") or TICI - ThermalStatus = log.DeviceState.ThermalStatus NetworkType = log.DeviceState.NetworkType NetworkStrength = log.DeviceState.NetworkStrength @@ -176,6 +174,7 @@ def thermald_thread(): if EON: base_path = "/sys/kernel/debug/cpr3-regulator/" cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()] + cpr_files = ["/sys/kernel/debug/regulator/pm8994_s11/voltage"] + cpr_files cpr_data = {} for cf in cpr_files: with open(cf, "r") as f: @@ -223,7 +222,7 @@ def thermald_thread(): if pandaState_prev is not None: if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \ pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown: - params.panda_disconnect() + params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT) pandaState_prev = pandaState # get_network_type is an expensive call. update every 10s @@ -327,8 +326,8 @@ def thermald_thread(): set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) - startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get("DisableUpdates") == b"1" - startup_conditions["not_uninstalling"] = not params.get("DoUninstall") == b"1" + startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") + startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version panda_signature = params.get("PandaFirmware") @@ -339,8 +338,8 @@ def thermald_thread(): startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version 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" + startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled") + startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot") # 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 startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger @@ -348,12 +347,14 @@ def thermald_thread(): # Handle offroad/onroad transition should_start = all(startup_conditions.values()) - if should_start: - if not should_start_prev: - params.delete("IsOffroad") - if TICI and DISABLE_LTE_ONROAD: - os.system("sudo systemctl stop --no-block lte") + if should_start != should_start_prev or (count == 0): + params.put_bool("IsOffroad", not should_start) + HARDWARE.set_power_save(not should_start) + if TICI and not params.get_bool("EnableLteOnroad"): + fxn = "off" if should_start else "on" + os.system(f"nmcli radio wwan {fxn}") + if should_start: off_ts = None if started_ts is None: started_ts = sec_since_boot() @@ -362,11 +363,6 @@ def thermald_thread(): 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): - params.put("IsOffroad", "1") - if TICI and DISABLE_LTE_ONROAD: - os.system("sudo systemctl start --no-block lte") - started_ts = None if off_ts is None: off_ts = sec_since_boot() @@ -409,6 +405,9 @@ def thermald_thread(): # report to server once every 10 minutes if (count % int(600. / DT_TRML)) == 0: + if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: + cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent) + location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 404d71416..fb7a1a2c9 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -8,11 +8,11 @@ import subprocess import time import glob -from raven import Client -from raven.transport.http import HTTPTransport +import sentry_sdk +from common.params import Params from common.file_helpers import mkdirs_exists_ok -from selfdrive.hardware import TICI +from selfdrive.hardware import TICI, HARDWARE from selfdrive.loggerd.config import ROOT from selfdrive.swaglog import cloudlog from selfdrive.version import branch, commit, dirty, origin, version @@ -31,16 +31,15 @@ def safe_fn(s): return "".join(c for c in s if c.isalnum() or c in extra).rstrip() -def sentry_report(client, fn, message, contents): +def sentry_report(fn, message, contents): cloudlog.error({'tombstone': message}) - client.captureMessage( - message=message, - sdk={'name': 'tombstoned', 'version': '0'}, - extra={ - 'tombstone_fn': fn, - 'tombstone': contents - }, - ) + + with sentry_sdk.configure_scope() as scope: + scope.set_extra("tombstone_fn", fn) + scope.set_extra("tombstone", contents) + sentry_sdk.capture_message(message=message) + sentry_sdk.flush() + def clear_apport_folder(): for f in glob.glob(APPORT_DIR + '*'): @@ -77,7 +76,7 @@ def get_tombstones(): return files -def report_tombstone_android(fn, client): +def report_tombstone_android(fn): f_size = os.path.getsize(fn) if f_size > MAX_SIZE: cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") @@ -104,7 +103,7 @@ def report_tombstone_android(fn, client): if fault_idx >= 0: message = message[:fault_idx] - sentry_report(client, fn, message, contents) + sentry_report(fn, message, contents) # Copy crashlog to upload folder clean_path = executable.replace('./', '').replace('/', '_') @@ -118,7 +117,7 @@ def report_tombstone_android(fn, client): shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) -def report_tombstone_apport(fn, client): +def report_tombstone_apport(fn): f_size = os.path.getsize(fn) if f_size > MAX_SIZE: cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") @@ -178,7 +177,7 @@ def report_tombstone_apport(fn, client): contents = stacktrace + "\n\n" + contents message = message + " - " + crash_function - sentry_report(client, fn, message, contents) + sentry_report(fn, message, contents) # Copy crashlog to upload folder clean_path = path.replace('/', '_') @@ -202,15 +201,18 @@ def main(): clear_apport_folder() # Clear apport folder on start, otherwise duplicate crashes won't register initial_tombstones = set(get_tombstones()) - tags = { - 'dirty': dirty, - 'origin': origin, - 'branch': branch - } - client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', - install_sys_hook=False, transport=HTTPTransport, release=version, tags=tags, string_max_length=10000) + sentry_sdk.utils.MAX_STRING_LENGTH = 8192 + sentry_sdk.init("https://a40f22e13cbc4261873333c125fc9d38@o33823.ingest.sentry.io/157615", + default_integrations=False, release=version) + + dongle_id = Params().get("DongleId", encoding='utf-8') + sentry_sdk.set_user({"id": dongle_id}) + sentry_sdk.set_tag("dirty", dirty) + sentry_sdk.set_tag("origin", origin) + sentry_sdk.set_tag("branch", branch) + sentry_sdk.set_tag("commit", commit) + sentry_sdk.set_tag("device", HARDWARE.get_device_type()) - client.user_context({'id': os.environ.get('DONGLE_ID')}) while True: now_tombstones = set(get_tombstones()) @@ -218,9 +220,9 @@ def main(): try: cloudlog.info(f"reporting new tombstone {fn}") if fn.endswith(".crash"): - report_tombstone_apport(fn, client) + report_tombstone_apport(fn) else: - report_tombstone_android(fn, client) + report_tombstone_android(fn) except Exception: cloudlog.exception(f"Error reporting tombstone {fn}") diff --git a/selfdrive/ui/.gitignore b/selfdrive/ui/.gitignore index 63f85bac0..2dbc32523 100644 --- a/selfdrive/ui/.gitignore +++ b/selfdrive/ui/.gitignore @@ -1,6 +1,7 @@ moc_* *.moc +replay/replay qt/text qt/spinner qt/setup/setup diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index ebef246ab..325c50b73 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,37 +1,36 @@ import os -Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', +Import('qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal', 'transformations') -src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] -libs = [gpucommon, common, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', - cereal, messaging, visionipc, transformations] - +base_libs = [gpucommon, common, messaging, cereal, visionipc, transformations, 'zmq', + 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] if arch == 'aarch64': - libs += ['log', 'utils', 'gui', 'ui', 'CB', 'gsl', 'adreno_utils', - 'cutils', 'uuid'] + base_libs += ['log', 'utils', 'gui', 'ui', 'CB', 'gsl', 'adreno_utils', 'cutils', 'uuid'] -qt_base_libs = qt_env["LIBS"] + libs + ["pthread"] if arch == "Darwin": - del qt_base_libs[qt_base_libs.index('OpenCL')] + del base_libs[base_libs.index('OpenCL')] qt_env['FRAMEWORKS'] += ['OpenCL'] widgets_src = ["qt/widgets/input.cc", "qt/widgets/drive_stats.cc", - "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", "qt/sound.cc", + "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", "qt/widgets/offroad_alerts.cc", "qt/widgets/setup.cc", "qt/widgets/keyboard.cc", - "#phonelibs/qrcode/QrCode.cc"] + "qt/widgets/scrollview.cc", "#phonelibs/qrcode/QrCode.cc", "qt/api.cc", + "qt/request_repeater.cc"] if arch != 'aarch64': widgets_src += ["qt/offroad/networking.cc", "qt/offroad/wifiManager.cc"] -widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=qt_base_libs) -qt_libs = qt_base_libs + [widgets] +widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) +qt_libs = [widgets] + base_libs # spinner and text window -qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_base_libs) -qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_base_libs) +qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs) +qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=base_libs) # build main UI -qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/api.cc", "qt/offroad/settings.cc", - "qt/offroad/onboarding.cc"] + src +qt_src = ["main.cc", "ui.cc", "paint.cc", "qt/sidebar.cc", "qt/onroad.cc", + "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", + "qt/offroad/onboarding.cc", "#phonelibs/nanovg/nanovg.c"] + qt_env.Program("_ui", qt_src, LIBS=qt_libs) # setup, factory resetter, and installer @@ -42,10 +41,10 @@ if arch != 'aarch64' and "BUILD_SETUP" in os.environ: installers = [ ("openpilot", "master"), - #("openpilot_test", "release3-staging"), - #("openpilot_internal", "master"), - #("dashcam", "dashcam3-staging"), - #("dashcam_test", "dashcam3-staging"), + ("openpilot_test", "release3-staging"), + ("openpilot_internal", "master"), + ("dashcam", "dashcam3-staging"), + ("dashcam_test", "dashcam3-staging"), ] for name, branch in installers: d = {'BRANCH': f"'\"{branch}\"'"} @@ -56,4 +55,17 @@ if arch != 'aarch64' and "BUILD_SETUP" in os.environ: r = requests.get("https://github.com/commaci2.keys") r.raise_for_status() d['SSH_KEYS'] = f'\\"{r.text.strip()}\\"' - qt_env.Program(f"qt/setup/installer_{name}", ["qt/setup/installer.cc"], LIBS=qt_libs, CPPDEFINES=d) + obj = qt_env.Object(f"qt/setup/installer_{name}.o", ["qt/setup/installer.cc"], CPPDEFINES=d) + qt_env.Program(f"qt/setup/installer_{name}", obj, LIBS=qt_libs, CPPDEFINES=d) + +# build headless replay +if arch == 'x86_64' and os.path.exists(Dir("#tools/").get_abspath()): + qt_env['CPPPATH'] += ["#tools/clib"] + qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"] + + replay_lib_src = ["replay/replay.cc", "replay/unlogger.cc", + "replay/filereader.cc", "#tools/clib/framereader.cc"] + + replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs) + replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'swscale', 'bz2'] + qt_libs + qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs) diff --git a/selfdrive/ui/qt/ui.cc b/selfdrive/ui/main.cc similarity index 51% rename from selfdrive/ui/qt/ui.cc rename to selfdrive/ui/main.cc index 1f33c75aa..dab8d6654 100644 --- a/selfdrive/ui/qt/ui.cc +++ b/selfdrive/ui/main.cc @@ -1,7 +1,9 @@ #include +#include -#include "window.hpp" -#include "qt_window.hpp" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/window.h" int main(int argc, char *argv[]) { QSurfaceFormat fmt; @@ -14,9 +16,12 @@ int main(int argc, char *argv[]) { #endif QSurfaceFormat::setDefaultFormat(fmt); -#ifdef QCOM - QApplication::setAttribute(Qt::AA_ShareOpenGLContexts); -#endif + if (Hardware::EON()) { + QApplication::setAttribute(Qt::AA_ShareOpenGLContexts); + QSslConfiguration ssl = QSslConfiguration::defaultConfiguration(); + ssl.setCaCertificates(QSslCertificate::fromPath("/usr/etc/tls/cert.pem")); + QSslConfiguration::setDefaultConfiguration(ssl); + } QApplication a(argc, argv); MainWindow w; diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 563dec36a..e6a868697 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -1,25 +1,33 @@ -#include "ui.hpp" +#include "paint.h" #include -#include "common/util.h" -#include "common/timing.h" + #include +#ifdef __APPLE__ +#include +#define NANOVG_GL3_IMPLEMENTATION +#define nvgCreate nvgCreateGL3 +#else +#include #define NANOVG_GLES3_IMPLEMENTATION -#include "nanovg_gl.h" -#include "nanovg_gl_utils.h" -#include "paint.hpp" -#include "sidebar.hpp" +#define nvgCreate nvgCreateGLES3 +#endif + +#define NANOVG_GLES3_IMPLEMENTATION +#include +#include + +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" + +#include "selfdrive/ui/ui.h" // TODO: this is also hardcoded in common/transformations/camera.py // TODO: choose based on frame input size -#ifdef QCOM2 -const float y_offset = 150.0; -const float zoom = 1.1; -#else -const float y_offset = 0.0; -const float zoom = 2.35; -#endif +const float y_offset = Hardware::TICI() ? 150.0 : 0.0; +const float zoom = Hardware::TICI() ? 2912.8 : 2138.5; static void ui_draw_text(const UIState *s, float x, float y, const char *string, float size, NVGcolor color, const char *font_name) { nvgFontFace(s->vg, font_name); @@ -50,19 +58,19 @@ static void draw_chevron(UIState *s, float x, float y, float sz, NVGcolor fillCo nvgFill(s->vg); } -static void ui_draw_circle_image(const UIState *s, int x, int y, int size, const char *image, NVGcolor color, float img_alpha, int img_y = 0) { - const int img_size = size * 1.5; +static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, int radius, const char *image, NVGcolor color, float img_alpha) { nvgBeginPath(s->vg); - nvgCircle(s->vg, x, y + (bdr_s * 1.5), size); + nvgCircle(s->vg, center_x, center_y, radius); nvgFillColor(s->vg, color); nvgFill(s->vg); - ui_draw_image(s, {x - (img_size / 2), img_y ? img_y : y - (size / 4), img_size, img_size}, image, img_alpha); + const int img_size = radius * 1.5; + ui_draw_image(s, {center_x - (img_size / 2), center_y - (img_size / 2), img_size, img_size}, image, img_alpha); } -static void ui_draw_circle_image(const UIState *s, int x, int y, int size, const char *image, bool active) { +static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, int radius, const char *image, bool active) { float bg_alpha = active ? 0.3f : 0.1f; float img_alpha = active ? 1.0f : 0.15f; - ui_draw_circle_image(s, x, y, size, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha); + ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha); } static void draw_lead(UIState *s, int idx) { @@ -83,7 +91,7 @@ static void draw_lead(UIState *s, int idx) { fillAlpha = (int)(fmin(fillAlpha, 255)); } - float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * zoom; + float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * s->zoom; x = std::clamp(x, 0.f, s->viz_rect.right() - sz / 2); y = std::fmin(s->viz_rect.bottom() - sz * .6, y); draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); @@ -120,11 +128,11 @@ static void draw_frame(UIState *s) { if (s->last_frame) { glBindTexture(GL_TEXTURE_2D, s->texture[s->last_frame->idx]->frame_tex); -#ifndef QCOM - // this is handled in ion on QCOM - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, s->last_frame->width, s->last_frame->height, - 0, GL_RGB, GL_UNSIGNED_BYTE, s->last_frame->addr); -#endif + if (!Hardware::EON()) { + // this is handled in ion on QCOM + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, s->last_frame->width, s->last_frame->height, + 0, GL_RGB, GL_UNSIGNED_BYTE, s->last_frame->addr); + } } glUseProgram(s->gl_shader->prog); @@ -215,22 +223,21 @@ static void ui_draw_vision_speed(UIState *s) { static void ui_draw_vision_event(UIState *s) { if (s->scene.controls_state.getEngageable()) { // draw steering wheel - const int bg_wheel_size = 96; - const int bg_wheel_x = s->viz_rect.right() - bg_wheel_size - bdr_s * 2; - const int bg_wheel_y = s->viz_rect.y + (bg_wheel_size / 2) + (bdr_s * 1.5); - ui_draw_circle_image(s, bg_wheel_x, bg_wheel_y, bg_wheel_size, "wheel", bg_colors[s->status], 1.0f, bg_wheel_y - 25); + const int radius = 96; + const int center_x = s->viz_rect.right() - radius - bdr_s * 2; + const int center_y = s->viz_rect.y + radius + (bdr_s * 1.5); + ui_draw_circle_image(s, center_x, center_y, radius, "wheel", bg_colors[s->status], 1.0f); } } static void ui_draw_vision_face(UIState *s) { - const int face_size = 96; - const int face_x = (s->viz_rect.x + face_size + (bdr_s * 2)); - const int face_y = (s->viz_rect.bottom() - footer_h + ((footer_h - face_size) / 2)); - ui_draw_circle_image(s, face_x, face_y, face_size, "driver_face", s->scene.dmonitoring_state.getIsActiveMode()); + const int radius = 96; + const int center_x = s->viz_rect.x + radius + (bdr_s * 2); + const int center_y = s->viz_rect.bottom() - footer_h / 2; + ui_draw_circle_image(s, center_x, center_y, radius, "driver_face", s->scene.dmonitoring_state.getIsActiveMode()); } static void ui_draw_driver_view(UIState *s) { - s->sidebar_collapsed = true; const bool is_rhd = s->scene.is_rhd; const int width = 4 * s->viz_rect.h / 3; const Rect rect = {s->viz_rect.centerX() - width / 2, s->viz_rect.y, width, s->viz_rect.h}; // x, y, w, h @@ -238,13 +245,9 @@ static void ui_draw_driver_view(UIState *s) { // blackout const int blackout_x_r = valid_rect.right(); -#ifndef QCOM2 - const int blackout_w_r = rect.right() - valid_rect.right(); - const int blackout_x_l = rect.x; -#else - const int blackout_w_r = s->viz_rect.right() - valid_rect.right(); - const int blackout_x_l = s->viz_rect.x; -#endif + const Rect &blackout_rect = Hardware::TICI() ? s->viz_rect : rect; + const int blackout_w_r = blackout_rect.right() - valid_rect.right(); + const int blackout_x_l = blackout_rect.x; const int blackout_w_l = valid_rect.x - blackout_x_l; ui_fill_rect(s->vg, {blackout_x_l, rect.y, blackout_w_l, rect.h}, COLOR_BLACK_ALPHA(144)); ui_fill_rect(s->vg, {blackout_x_r, rect.y, blackout_w_r, rect.h}, COLOR_BLACK_ALPHA(144)); @@ -266,10 +269,10 @@ static void ui_draw_driver_view(UIState *s) { } // draw face icon - const int face_size = 85; - const int icon_x = is_rhd ? rect.right() - face_size - bdr_s * 2 : rect.x + face_size + bdr_s * 2; - const int icon_y = rect.bottom() - face_size - bdr_s * 2.5; - ui_draw_circle_image(s, icon_x, icon_y, face_size, "driver_face", face_detected); + const int face_radius = 85; + const int center_x = is_rhd ? rect.right() - face_radius - bdr_s * 2 : rect.x + face_radius + bdr_s * 2; + const int center_y = rect.bottom() - face_radius - bdr_s * 2.5; + ui_draw_circle_image(s, center_x, center_y, face_radius, "driver_face", face_detected); } static void ui_draw_vision_header(UIState *s) { @@ -285,54 +288,6 @@ static void ui_draw_vision_header(UIState *s) { ui_draw_vision_event(s); } -static void ui_draw_vision_footer(UIState *s) { - ui_draw_vision_face(s); -} - -static float get_alert_alpha(float blink_rate) { - return 0.375 * cos((millis_since_boot() / 1000) * 2 * M_PI * blink_rate) + 0.625; -} - -static void ui_draw_vision_alert(UIState *s) { - static std::map alert_size_map = { - {cereal::ControlsState::AlertSize::SMALL, 241}, - {cereal::ControlsState::AlertSize::MID, 390}, - {cereal::ControlsState::AlertSize::FULL, s->fb_h}}; - const UIScene *scene = &s->scene; - bool longAlert1 = scene->alert_text1.length() > 15; - - NVGcolor color = bg_colors[s->status]; - color.a *= get_alert_alpha(scene->alert_blinking_rate); - const int alr_h = alert_size_map[scene->alert_size] + bdr_s; - const Rect rect = {.x = s->viz_rect.x - bdr_s, - .y = s->fb_h - alr_h, - .w = s->viz_rect.w + (bdr_s * 2), - .h = alr_h}; - - ui_fill_rect(s->vg, rect, color); - ui_fill_rect(s->vg, rect, nvgLinearGradient(s->vg, rect.x, rect.y, rect.x, rect.bottom(), - nvgRGBAf(0.0, 0.0, 0.0, 0.05), nvgRGBAf(0.0, 0.0, 0.0, 0.35))); - - nvgFillColor(s->vg, COLOR_WHITE); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - - if (scene->alert_size == cereal::ControlsState::AlertSize::SMALL) { - ui_draw_text(s, rect.centerX(), rect.centerY() + 15, scene->alert_text1.c_str(), 40*2.5, COLOR_WHITE, "sans-semibold"); - } else if (scene->alert_size == cereal::ControlsState::AlertSize::MID) { - ui_draw_text(s, rect.centerX(), rect.centerY() - 45, scene->alert_text1.c_str(), 48*2.5, COLOR_WHITE, "sans-bold"); - ui_draw_text(s, rect.centerX(), rect.centerY() + 75, scene->alert_text2.c_str(), 36*2.5, COLOR_WHITE, "sans-regular"); - } else if (scene->alert_size == cereal::ControlsState::AlertSize::FULL) { - nvgFontSize(s->vg, (longAlert1?72:96)*2.5); - nvgFontFace(s->vg, "sans-bold"); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, rect.x, rect.y+(longAlert1?360:420), rect.w-60, scene->alert_text1.c_str(), NULL); - nvgFontSize(s->vg, 48*2.5); - nvgFontFace(s->vg, "sans-regular"); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); - nvgTextBox(s->vg, rect.x, rect.h-(longAlert1?300:360), rect.w-60, scene->alert_text2.c_str(), NULL); - } -} - static void ui_draw_vision_frame(UIState *s) { // Draw video frames glEnable(GL_SCISSOR_TEST); @@ -353,8 +308,8 @@ static void ui_draw_vision(UIState *s) { } // Set Speed, Current Speed, Status/Events ui_draw_vision_header(s); - if (scene->alert_size == cereal::ControlsState::AlertSize::NONE) { - ui_draw_vision_footer(s); + if (s->scene.controls_state.getAlertSize() == cereal::ControlsState::AlertSize::NONE) { + ui_draw_vision_face(s); } } else { ui_draw_driver_view(s); @@ -367,15 +322,10 @@ static void ui_draw_background(UIState *s) { glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); } -void ui_draw(UIState *s) { - s->viz_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s}; - if (!s->sidebar_collapsed) { - s->viz_rect.x += sbr_w; - s->viz_rect.w -= sbr_w; - } +void ui_draw(UIState *s, int w, int h) { + s->viz_rect = Rect{bdr_s, bdr_s, w - 2 * bdr_s, h - 2 * bdr_s}; - const bool draw_alerts = s->scene.started; - const bool draw_vision = draw_alerts && s->vipc_client->connected; + const bool draw_vision = s->scene.started && s->vipc_client->connected; // GL drawing functions ui_draw_background(s); @@ -388,13 +338,14 @@ void ui_draw(UIState *s) { // NVG drawing functions - should be no GL inside NVG frame nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); - ui_draw_sidebar(s); + if (draw_vision) { ui_draw_vision(s); } - if (draw_alerts && s->scene.alert_size != cereal::ControlsState::AlertSize::NONE) { - ui_draw_vision_alert(s); + if (s->scene.driver_view && !s->vipc_client->connected) { + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + ui_draw_text(s, s->viz_rect.centerX(), s->viz_rect.centerY(), "Please wait for camera to start", 40 * 2.5, COLOR_WHITE, "sans-bold"); } nvgEndFrame(s->vg); glDisable(GL_BLEND); @@ -457,6 +408,10 @@ static const char frame_fragment_shader[] = "out vec4 colorOut;\n" "void main() {\n" " colorOut = texture(uTexture, vTexCoord.xy);\n" +#ifdef QCOM + " vec3 dz = vec3(0.0627f, 0.0627f, 0.0627f);\n" + " colorOut.rgb = ((vec3(1.0f, 1.0f, 1.0f) - dz) * colorOut.rgb / vec3(1.0f, 1.0f, 1.0f)) + dz;\n" +#endif "}\n"; static const mat4 device_transform = {{ @@ -466,41 +421,42 @@ static const mat4 device_transform = {{ 0.0, 0.0, 0.0, 1.0, }}; -static const float driver_view_ratio = 1.333; -#ifndef QCOM2 -// frame from 4/3 to 16/9 display -static const mat4 driver_view_transform = {{ - driver_view_ratio*(1080-2*bdr_s)/(1920-2*bdr_s), 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, -}}; -#else -// from dmonitoring.cc -static const int full_width_tici = 1928; -static const int full_height_tici = 1208; -static const int adapt_width_tici = 668; -static const int crop_x_offset = 32; -static const int crop_y_offset = -196; -static const float yscale = full_height_tici * driver_view_ratio / adapt_width_tici; -static const float xscale = yscale*(1080-2*bdr_s)/(2160-2*bdr_s)*full_width_tici/full_height_tici; - -static const mat4 driver_view_transform = {{ - xscale, 0.0, 0.0, xscale*crop_x_offset/full_width_tici*2, - 0.0, yscale, 0.0, yscale*crop_y_offset/full_height_tici*2, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, -}}; -#endif +static mat4 get_driver_view_transform() { + const float driver_view_ratio = 1.333; + mat4 transform; + if (Hardware::TICI()) { + // from dmonitoring.cc + const int full_width_tici = 1928; + const int full_height_tici = 1208; + const int adapt_width_tici = 668; + const int crop_x_offset = 32; + const int crop_y_offset = -196; + const float yscale = full_height_tici * driver_view_ratio / adapt_width_tici; + const float xscale = yscale*(1080-2*bdr_s)/(2160-2*bdr_s)*full_width_tici/full_height_tici; + transform = (mat4){{ + xscale, 0.0, 0.0, xscale*crop_x_offset/full_width_tici*2, + 0.0, yscale, 0.0, yscale*crop_y_offset/full_height_tici*2, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + + } else { + // frame from 4/3 to 16/9 display + transform = (mat4){{ + driver_view_ratio*(1080-2*bdr_s)/(1920-2*bdr_s), 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + } + return transform; +} void ui_nvg_init(UIState *s) { // init drawing -#ifdef QCOM - // on QCOM, we enable MSAA - s->vg = nvgCreate(0); -#else - s->vg = nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); -#endif + + // on EON, we enable MSAA + s->vg = Hardware::EON() ? nvgCreate(0) : nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); assert(s->vg); // init fonts @@ -516,19 +472,8 @@ void ui_nvg_init(UIState *s) { // init images std::vector> images = { - {"wheel", "../assets/img_chffr_wheel.png"}, - {"trafficSign_turn", "../assets/img_trafficSign_turn.png"}, - {"driver_face", "../assets/img_driver_face.png"}, - {"button_settings", "../assets/images/button_settings.png"}, - {"button_home", "../assets/images/button_home.png"}, - {"battery", "../assets/images/battery.png"}, - {"battery_charging", "../assets/images/battery_charging.png"}, - {"network_0", "../assets/images/network_0.png"}, - {"network_1", "../assets/images/network_1.png"}, - {"network_2", "../assets/images/network_2.png"}, - {"network_3", "../assets/images/network_3.png"}, - {"network_4", "../assets/images/network_4.png"}, - {"network_5", "../assets/images/network_5.png"}, + {"wheel", "../assets/img_chffr_wheel.png"}, + {"driver_face", "../assets/img_driver_face.png"}, }; for (auto [name, file] : images) { s->images[name] = nvgCreateImage(s->vg, file, 1); @@ -586,9 +531,17 @@ void ui_nvg_init(UIState *s) { glBindVertexArray(0); } + auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; + + s->zoom = zoom / intrinsic_matrix.v[0]; + + if (s->wide_camera) { + s->zoom *= 0.5; + } + s->video_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s}; - float zx = zoom * 2 * fcam_intrinsic_matrix.v[2] / s->video_rect.w; - float zy = zoom * 2 * fcam_intrinsic_matrix.v[5] / s->video_rect.h; + float zx = s->zoom * 2 * intrinsic_matrix.v[2] / s->video_rect.w; + float zy = s->zoom * 2 * intrinsic_matrix.v[5] / s->video_rect.h; const mat4 frame_transform = {{ zx, 0.0, 0.0, 0.0, @@ -597,7 +550,7 @@ void ui_nvg_init(UIState *s) { 0.0, 0.0, 0.0, 1.0, }}; - s->front_frame_mat = matmul(device_transform, driver_view_transform); + s->front_frame_mat = matmul(device_transform, get_driver_view_transform()); s->rear_frame_mat = matmul(device_transform, frame_transform); // Apply transformation such that video pixel coordinates match video @@ -605,10 +558,10 @@ void ui_nvg_init(UIState *s) { nvgTranslate(s->vg, s->video_rect.x + s->video_rect.w / 2, s->video_rect.y + s->video_rect.h / 2 + y_offset); // 2) Apply same scaling as video - nvgScale(s->vg, zoom, zoom); + nvgScale(s->vg, s->zoom, s->zoom); // 3) Put (0, 0) in top left corner of video - nvgTranslate(s->vg, -fcam_intrinsic_matrix.v[2], -fcam_intrinsic_matrix.v[5]); + nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); nvgCurrentTransform(s->vg, s->car_space_transform); nvgResetTransform(s->vg); diff --git a/selfdrive/ui/paint.hpp b/selfdrive/ui/paint.h similarity index 85% rename from selfdrive/ui/paint.hpp rename to selfdrive/ui/paint.h index bc0927a71..b955a6163 100644 --- a/selfdrive/ui/paint.hpp +++ b/selfdrive/ui/paint.h @@ -1,7 +1,8 @@ #pragma once -#include "ui.hpp" -void ui_draw(UIState *s); +#include "selfdrive/ui/ui.h" + +void ui_draw(UIState *s, int w, int h); void ui_draw_image(const UIState *s, const Rect &r, const char *name, float alpha); void ui_draw_rect(NVGcontext *vg, const Rect &r, NVGcolor color, int width, float radius = 0); void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGpaint &paint, float radius = 0); diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc index 5522705c4..771af0fcf 100644 --- a/selfdrive/ui/qt/api.cc +++ b/selfdrive/ui/qt/api.cc @@ -1,3 +1,5 @@ +#include "selfdrive/ui/qt/api.h" + #include #include #include @@ -5,23 +7,20 @@ #include #include #include -#include -#include -#include #include +#include +#include +#include -#include "api.hpp" -#include "home.hpp" -#include "common/params.h" -#include "common/util.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" -#if defined(QCOM) || defined(QCOM2) -const std::string private_key_path = "/persist/comma/id_rsa"; -#else -const std::string private_key_path = util::getenv_default("HOME", "/.comma/persist/comma/id_rsa", "/persist/comma/id_rsa"); -#endif +const std::string private_key_path = + Hardware::PC() ? util::getenv_default("HOME", "/.comma/persist/comma/id_rsa", "/persist/comma/id_rsa") + : "/persist/comma/id_rsa"; -QByteArray CommaApi::rsa_sign(QByteArray data) { +QByteArray CommaApi::rsa_sign(const QByteArray &data) { auto file = QFile(private_key_path.c_str()); if (!file.open(QIODevice::ReadOnly)) { qDebug() << "No RSA private key found, please run manager.py or registration.py"; @@ -45,7 +44,7 @@ QByteArray CommaApi::rsa_sign(QByteArray data) { return sig; } -QString CommaApi::create_jwt(QVector> payloads, int expiry) { +QString CommaApi::create_jwt(const QVector> &payloads, int expiry) { QString dongle_id = QString::fromStdString(Params().get("DongleId")); QJsonObject header; @@ -58,7 +57,7 @@ QString CommaApi::create_jwt(QVector> payloads, int e payload.insert("nbf", t); payload.insert("iat", t); payload.insert("exp", t + expiry); - for (auto load : payloads) { + for (auto &load : payloads) { payload.insert(load.first, load.second); } @@ -72,24 +71,17 @@ QString CommaApi::create_jwt(QVector> payloads, int e return jwt; } -QString CommaApi::create_jwt() { - return create_jwt(*(new QVector>())); -} -RequestRepeater::RequestRepeater(QWidget* parent, QString requestURL, int period_seconds, const QString &cache_key, QVector> payloads, bool disableWithScreen) - : disableWithScreen(disableWithScreen), cache_key(cache_key), QObject(parent) { +HttpRequest::HttpRequest(QObject *parent, const QString &requestURL, const QString &cache_key, bool create_jwt_) : cache_key(cache_key), create_jwt(create_jwt_), QObject(parent) { networkAccessManager = new QNetworkAccessManager(this); - reply = NULL; - QTimer* timer = new QTimer(this); - QObject::connect(timer, &QTimer::timeout, [=](){sendRequest(requestURL, payloads);}); - timer->start(period_seconds * 1000); - networkTimer = new QTimer(this); networkTimer->setSingleShot(true); networkTimer->setInterval(20000); - connect(networkTimer, SIGNAL(timeout()), this, SLOT(requestTimeout())); + connect(networkTimer, &QTimer::timeout, this, &HttpRequest::requestTimeout); + + sendRequest(requestURL); if (!cache_key.isEmpty()) { if (std::string cached_resp = Params().get(cache_key.toStdString()); !cached_resp.empty()) { @@ -98,49 +90,47 @@ RequestRepeater::RequestRepeater(QWidget* parent, QString requestURL, int period } } -void RequestRepeater::sendRequest(QString requestURL, QVector> payloads){ - if (GLWindow::ui_state.scene.started || !active || reply != NULL || - (!GLWindow::ui_state.awake && disableWithScreen)) { - return; +void HttpRequest::sendRequest(const QString &requestURL){ + QString token; + if(create_jwt) { + token = CommaApi::create_jwt(); + } else { + QString token_json = QString::fromStdString(util::read_file(util::getenv_default("HOME", "/.comma/auth.json", "/.comma/auth.json"))); + QJsonDocument json_d = QJsonDocument::fromJson(token_json.toUtf8()); + token = json_d["access_token"].toString(); } - QString token = CommaApi::create_jwt(payloads); QNetworkRequest request; request.setUrl(QUrl(requestURL)); request.setRawHeader(QByteArray("Authorization"), ("JWT " + token).toUtf8()); -#ifdef QCOM - QSslConfiguration ssl = QSslConfiguration::defaultConfiguration(); - ssl.setCaCertificates(QSslCertificate::fromPath("/usr/etc/tls/cert.pem", - QSsl::Pem, QRegExp::Wildcard)); - request.setSslConfiguration(ssl); -#endif - reply = networkAccessManager->get(request); networkTimer->start(); - connect(reply, SIGNAL(finished()), this, SLOT(requestFinished())); + connect(reply, &QNetworkReply::finished, this, &HttpRequest::requestFinished); } -void RequestRepeater::requestTimeout(){ +void HttpRequest::requestTimeout(){ reply->abort(); } // This function should always emit something -void RequestRepeater::requestFinished(){ +void HttpRequest::requestFinished(){ if (reply->error() != QNetworkReply::OperationCanceledError) { networkTimer->stop(); QString response = reply->readAll(); + if (reply->error() == QNetworkReply::NoError) { // save to cache if (!cache_key.isEmpty()) { - Params().write_db_value(cache_key.toStdString(), response.toStdString()); + Params().put(cache_key.toStdString(), response.toStdString()); } emit receivedResponse(response); } else { if (!cache_key.isEmpty()) { - Params().delete_db_value(cache_key.toStdString()); + Params().remove(cache_key.toStdString()); } + qDebug() << reply->errorString(); emit failedResponse(reply->errorString()); } } else { diff --git a/selfdrive/ui/qt/api.h b/selfdrive/ui/qt/api.h new file mode 100644 index 000000000..71fd9b868 --- /dev/null +++ b/selfdrive/ui/qt/api.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class CommaApi : public QObject { + Q_OBJECT + +public: + static QByteArray rsa_sign(const QByteArray &data); + static QString create_jwt(const QVector> &payloads = {}, int expiry = 3600); +}; + +/** + * Makes a request to the request endpoint. + */ + +class HttpRequest : public QObject { + Q_OBJECT + +public: + explicit HttpRequest(QObject* parent, const QString &requestURL, const QString &cache_key = "", bool create_jwt_ = true); + QNetworkReply *reply; + void sendRequest(const QString &requestURL); + +private: + QNetworkAccessManager *networkAccessManager; + QTimer *networkTimer; + QString cache_key; + bool create_jwt; + +private slots: + void requestTimeout(); + void requestFinished(); + +signals: + void receivedResponse(const QString &response); + void failedResponse(const QString &errorString); + void timeoutResponse(const QString &errorString); +}; diff --git a/selfdrive/ui/qt/api.hpp b/selfdrive/ui/qt/api.hpp deleted file mode 100644 index 3772727be..000000000 --- a/selfdrive/ui/qt/api.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -class CommaApi : public QObject { - Q_OBJECT - -public: - static QByteArray rsa_sign(QByteArray data); - static QString create_jwt(QVector> payloads, int expiry=3600); - static QString create_jwt(); - -private: - QNetworkAccessManager* networkAccessManager; -}; - -/** - * Makes repeated requests to the request endpoint. - */ -class RequestRepeater : public QObject { - Q_OBJECT - -public: - explicit RequestRepeater(QWidget* parent, QString requestURL, int period = 10, const QString &cache_key = "", QVector> payloads = *(new QVector>()), bool disableWithScreen = true); - bool active = true; - -private: - bool disableWithScreen; - QNetworkReply* reply; - QNetworkAccessManager* networkAccessManager; - QTimer* networkTimer; - QString cache_key; - void sendRequest(QString requestURL, QVector> payloads); - -private slots: - void requestTimeout(); - void requestFinished(); - -signals: - void receivedResponse(QString response); - void failedResponse(QString errorString); - void timeoutResponse(QString errorString); -}; diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index fead4ae1b..18070e4e6 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -1,79 +1,73 @@ -#include -#include -#include -#include -#include +#include "selfdrive/ui/qt/home.h" #include #include #include #include -#include "common/util.h" -#include "common/params.h" -#include "common/timing.h" -#include "common/swaglog.h" -#include "common/watchdog.h" -#include "selfdrive/hardware/hw.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/common/util.h" +#include "selfdrive/ui/qt/widgets/drive_stats.h" +#include "selfdrive/ui/qt/widgets/setup.h" -#include "home.hpp" -#include "paint.hpp" -#include "qt_window.hpp" -#include "widgets/drive_stats.hpp" -#include "widgets/setup.hpp" - -#define BACKLIGHT_DT 0.25 -#define BACKLIGHT_TS 2.00 -#define BACKLIGHT_OFFROAD 50 - -// HomeWindow: the container for the offroad (OffroadHome) and onroad (GLWindow) UIs +// HomeWindow: the container for the offroad and onroad UIs HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { - layout = new QStackedLayout(); - layout->setStackingMode(QStackedLayout::StackAll); + QHBoxLayout *layout = new QHBoxLayout(this); + layout->setMargin(0); + layout->setSpacing(0); - // onroad UI - glWindow = new GLWindow(this); - layout->addWidget(glWindow); + sidebar = new Sidebar(this); + layout->addWidget(sidebar); + QObject::connect(this, &HomeWindow::update, sidebar, &Sidebar::update); + QObject::connect(sidebar, &Sidebar::openSettings, this, &HomeWindow::openSettings); + + slayout = new QStackedLayout(); + layout->addLayout(slayout); + + onroad = new OnroadWindow(this); + slayout->addWidget(onroad); + QObject::connect(this, &HomeWindow::update, onroad, &OnroadWindow::update); + QObject::connect(this, &HomeWindow::offroadTransitionSignal, onroad, &OnroadWindow::offroadTransition); - // draw offroad UI on top of onroad UI home = new OffroadHome(); - layout->addWidget(home); - - QObject::connect(glWindow, SIGNAL(offroadTransition(bool)), home, SLOT(setVisible(bool))); - QObject::connect(glWindow, SIGNAL(offroadTransition(bool)), this, SIGNAL(offroadTransition(bool))); - QObject::connect(glWindow, SIGNAL(screen_shutoff()), this, SIGNAL(closeSettings())); - QObject::connect(this, SIGNAL(openSettings()), home, SLOT(refresh())); + slayout->addWidget(home); + QObject::connect(this, &HomeWindow::openSettings, home, &OffroadHome::refresh); setLayout(layout); } +void HomeWindow::offroadTransition(bool offroad) { + if (offroad) { + slayout->setCurrentWidget(home); + } else { + slayout->setCurrentWidget(onroad); + } + sidebar->setVisible(offroad); + emit offroadTransitionSignal(offroad); +} + void HomeWindow::mousePressEvent(QMouseEvent* e) { - UIState* ui_state = &glWindow->ui_state; - if (GLWindow::ui_state.scene.driver_view) { - Params().write_db_value("IsDriverViewEnabled", "0", 1); + // TODO: make a nice driver view widget + if (QUIState::ui_state.scene.driver_view) { + Params().putBool("IsDriverViewEnabled", false); + QUIState::ui_state.scene.driver_view = false; return; } - glWindow->wake(); - - // Settings button click - if (!ui_state->sidebar_collapsed && settings_btn.ptInRect(e->x(), e->y())) { - emit openSettings(); - } - // Handle sidebar collapsing - if (ui_state->scene.started && (e->x() >= ui_state->viz_rect.x - bdr_s)) { - ui_state->sidebar_collapsed = !ui_state->sidebar_collapsed; + if (onroad->isVisible() && (!sidebar->isVisible() || e->x() > sidebar->width())) { + sidebar->setVisible(!sidebar->isVisible()); } } - // OffroadHome: the offroad home page -OffroadHome::OffroadHome(QWidget* parent) : QWidget(parent) { +OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) { QVBoxLayout* main_layout = new QVBoxLayout(); - main_layout->setContentsMargins(sbr_w + 50, 50, 50, 50); + main_layout->setMargin(50); // top header QHBoxLayout* header_layout = new QHBoxLayout(); @@ -84,10 +78,10 @@ OffroadHome::OffroadHome(QWidget* parent) : QWidget(parent) { alert_notification = new QPushButton(); alert_notification->setVisible(false); - QObject::connect(alert_notification, SIGNAL(released()), this, SLOT(openAlerts())); + QObject::connect(alert_notification, &QPushButton::released, this, &OffroadHome::openAlerts); header_layout->addWidget(alert_notification, 0, Qt::AlignHCenter | Qt::AlignRight); - std::string brand = Params().read_db_bool("Passive") ? "dashcam" : "openpilot"; + std::string brand = Params().getBool("Passive") ? "dashcam" : "openpilot"; QLabel* version = new QLabel(QString::fromStdString(brand + " v" + Params().get("Version"))); version->setStyleSheet(R"(font-size: 55px;)"); header_layout->addWidget(version, 0, Qt::AlignHCenter | Qt::AlignRight); @@ -99,13 +93,13 @@ OffroadHome::OffroadHome(QWidget* parent) : QWidget(parent) { center_layout = new QStackedLayout(); QHBoxLayout* statsAndSetup = new QHBoxLayout(); + statsAndSetup->setMargin(0); DriveStats* drive = new DriveStats; drive->setFixedSize(800, 800); statsAndSetup->addWidget(drive); SetupWidget* setup = new SetupWidget; - //setup->setFixedSize(700, 700); statsAndSetup->addWidget(setup); QWidget* statsAndSetupWidget = new QWidget(); @@ -114,7 +108,7 @@ OffroadHome::OffroadHome(QWidget* parent) : QWidget(parent) { center_layout->addWidget(statsAndSetupWidget); alerts_widget = new OffroadAlert(); - QObject::connect(alerts_widget, SIGNAL(closeAlerts()), this, SLOT(closeAlerts())); + QObject::connect(alerts_widget, &OffroadAlert::closeAlerts, this, &OffroadHome::closeAlerts); center_layout->addWidget(alerts_widget); center_layout->setAlignment(alerts_widget, Qt::AlignCenter); @@ -122,18 +116,24 @@ OffroadHome::OffroadHome(QWidget* parent) : QWidget(parent) { // set up refresh timer timer = new QTimer(this); - QObject::connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); - refresh(); + QObject::connect(timer, &QTimer::timeout, this, &OffroadHome::refresh); timer->start(10 * 1000); setLayout(main_layout); setStyleSheet(R"( + OffroadHome { + background-color: black; + } * { color: white; } )"); } +void OffroadHome::showEvent(QShowEvent *event) { + refresh(); +} + void OffroadHome::openAlerts() { center_layout->setCurrentIndex(1); } @@ -153,7 +153,7 @@ void OffroadHome::refresh() { // update alerts alerts_widget->refresh(); - if (!alerts_widget->alerts.size() && !alerts_widget->updateAvailable) { + if (!alerts_widget->alertCount && !alerts_widget->updateAvailable) { emit closeAlerts(); alert_notification->setVisible(false); return; @@ -162,7 +162,7 @@ void OffroadHome::refresh() { if (alerts_widget->updateAvailable) { alert_notification->setText("UPDATE"); } else { - int alerts = alerts_widget->alerts.size(); + int alerts = alerts_widget->alertCount; alert_notification->setText(QString::number(alerts) + " ALERT" + (alerts == 1 ? "" : "S")); } @@ -187,148 +187,3 @@ void OffroadHome::refresh() { } alert_notification->setStyleSheet(style); } - - -// GLWindow: the onroad UI - -static void handle_display_state(UIState* s, bool user_input) { - static int awake_timeout = 0; - awake_timeout = std::max(awake_timeout - 1, 0); - - constexpr float accel_samples = 5*UI_FREQ; - static float accel_prev = 0., gyro_prev = 0.; - - bool should_wake = s->scene.started || s->scene.ignition || user_input; - if (!should_wake) { - // tap detection while display is off - bool accel_trigger = abs(s->scene.accel_sensor - accel_prev) > 0.2; - bool gyro_trigger = abs(s->scene.gyro_sensor - gyro_prev) > 0.15; - should_wake = accel_trigger && gyro_trigger; - gyro_prev = s->scene.gyro_sensor; - accel_prev = (accel_prev * (accel_samples - 1) + s->scene.accel_sensor) / accel_samples; - } - - if (should_wake) { - awake_timeout = 30 * UI_FREQ; - } else if (awake_timeout > 0) { - should_wake = true; - } - - // handle state transition - if (s->awake != should_wake) { - s->awake = should_wake; - Hardware::set_display_power(s->awake); - LOGD("setting display power %d", s->awake); - } -} - -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 = 10.0; - brightness_m = 0.1; - } - smooth_brightness = BACKLIGHT_OFFROAD; -} - -GLWindow::~GLWindow() { - makeCurrent(); - doneCurrent(); -} - -void GLWindow::initializeGL() { - initializeOpenGLFunctions(); - std::cout << "OpenGL version: " << glGetString(GL_VERSION) << std::endl; - std::cout << "OpenGL vendor: " << glGetString(GL_VENDOR) << std::endl; - std::cout << "OpenGL renderer: " << glGetString(GL_RENDERER) << std::endl; - std::cout << "OpenGL language version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl; - - ui_state.sound = &sound; - ui_state.fb_w = vwp_w; - ui_state.fb_h = vwp_h; - ui_init(&ui_state); - - wake(); - - prev_draw_t = millis_since_boot(); - timer->start(1000 / UI_FREQ); - backlight_timer->start(BACKLIGHT_DT * 1000); -} - -void GLWindow::backlightUpdate() { - // Update brightness - float k = (BACKLIGHT_DT / BACKLIGHT_TS) / (1.0f + BACKLIGHT_DT / BACKLIGHT_TS); - - float clipped_brightness = std::min(100.0f, (ui_state.scene.light_sensor * brightness_m) + brightness_b); - if (!ui_state.scene.started) { - clipped_brightness = BACKLIGHT_OFFROAD; - } - - smooth_brightness = clipped_brightness * k + smooth_brightness * (1.0f - k); - - int brightness = smooth_brightness; - if (!ui_state.awake) { - brightness = 0; - emit screen_shutoff(); - } - - if (brightness != last_brightness) { - std::thread{Hardware::set_brightness, brightness}.detach(); - } - last_brightness = brightness; -} - -void GLWindow::timerUpdate() { - // Connecting to visionIPC requires opengl to be current - if (!ui_state.vipc_client->connected){ - makeCurrent(); - } - - if (ui_state.scene.started != onroad) { - onroad = ui_state.scene.started; - emit offroadTransition(!onroad); - - // Change timeout to 0 when onroad, this will call timerUpdate continously. - // This puts visionIPC in charge of update frequency, reducing video latency - timer->start(onroad ? 0 : 1000 / UI_FREQ); - } - - handle_display_state(&ui_state, false); - - // scale volume with speed - sound.volume = util::map_val(ui_state.scene.car_state.getVEgo(), 0.f, 20.f, - Hardware::MIN_VOLUME, Hardware::MAX_VOLUME); - - ui_update(&ui_state); - repaint(); - watchdog_kick(); -} - -void GLWindow::resizeGL(int w, int h) { - std::cout << "resize " << w << "x" << h << std::endl; -} - -void GLWindow::paintGL() { - if(GLWindow::ui_state.awake){ - ui_draw(&ui_state); - - double cur_draw_t = millis_since_boot(); - double dt = cur_draw_t - prev_draw_t; - if (dt > 66 && onroad && !ui_state.scene.driver_view) { - // warn on sub 15fps - LOGW("slow frame(%llu) time: %.2f", ui_state.sm->frame, dt); - } - prev_draw_t = cur_draw_t; - } -} - -void GLWindow::wake() { - handle_display_state(&ui_state, true); -} diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h new file mode 100644 index 000000000..bac2b2dc5 --- /dev/null +++ b/selfdrive/ui/qt/home.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/onroad.h" +#include "selfdrive/ui/qt/sidebar.h" +#include "selfdrive/ui/qt/widgets/offroad_alerts.h" +#include "selfdrive/ui/ui.h" + +class OffroadHome : public QFrame { + Q_OBJECT + +public: + explicit OffroadHome(QWidget* parent = 0); + +protected: + void showEvent(QShowEvent *event) override; + +private: + QTimer* timer; + + QLabel* date; + QStackedLayout* center_layout; + OffroadAlert* alerts_widget; + QPushButton* alert_notification; + +public slots: + void closeAlerts(); + void openAlerts(); + void refresh(); +}; + +class HomeWindow : public QWidget { + Q_OBJECT + +public: + explicit HomeWindow(QWidget* parent = 0); + +signals: + void openSettings(); + void closeSettings(); + + // forwarded signals + void displayPowerChanged(bool on); + void update(const UIState &s); + void offroadTransitionSignal(bool offroad); + +public slots: + void offroadTransition(bool offroad); + +protected: + void mousePressEvent(QMouseEvent* e) override; + +private: + Sidebar *sidebar; + OffroadHome *home; + OnroadWindow *onroad; + QStackedLayout *slayout; +}; diff --git a/selfdrive/ui/qt/home.hpp b/selfdrive/ui/qt/home.hpp deleted file mode 100644 index f5cf74cb0..000000000 --- a/selfdrive/ui/qt/home.hpp +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "sound.hpp" -#include "ui/ui.hpp" -#include "widgets/offroad_alerts.hpp" - -// container window for onroad NVG UI -class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions { - Q_OBJECT - -public: - using QOpenGLWidget::QOpenGLWidget; - explicit GLWindow(QWidget* parent = 0); - void wake(); - ~GLWindow(); - - inline static UIState ui_state = {0}; - -signals: - void offroadTransition(bool offroad); - void screen_shutoff(); - -protected: - void initializeGL() override; - void resizeGL(int w, int h) override; - void paintGL() override; - -private: - QTimer* timer; - QTimer* backlight_timer; - - Sound sound; - - bool onroad = true; - double prev_draw_t = 0; - - // TODO: make a nice abstraction to handle embedded device stuff - float brightness_b = 0; - float brightness_m = 0; - float smooth_brightness = 0; - float last_brightness = 0; - -public slots: - void timerUpdate(); - void backlightUpdate(); -}; - -// offroad home screen -class OffroadHome : public QWidget { - Q_OBJECT - -public: - explicit OffroadHome(QWidget* parent = 0); - -private: - QTimer* timer; - - QLabel* date; - QStackedLayout* center_layout; - OffroadAlert* alerts_widget; - QPushButton* alert_notification; - -public slots: - void closeAlerts(); - void openAlerts(); - void refresh(); -}; - -class HomeWindow : public QWidget { - Q_OBJECT - -public: - explicit HomeWindow(QWidget* parent = 0); - GLWindow* glWindow; - -signals: - void openSettings(); - void closeSettings(); - void offroadTransition(bool offroad); - -protected: - void mousePressEvent(QMouseEvent* e) override; - -private: - OffroadHome* home; - QStackedLayout* layout; -}; diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index 2c9390634..dd3cd5ba5 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -1,15 +1,11 @@ +#include "networking.h" + #include #include #include #include -#include -#include -#include -#include "common/params.h" -#include "hardware/hw.h" -#include "networking.hpp" -#include "util.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" void clearLayout(QLayout* layout) { while (QLayoutItem* item = layout->takeAt(0)) { @@ -23,12 +19,6 @@ void clearLayout(QLayout* layout) { } } -QWidget* layoutToWidget(QLayout* l, QWidget* parent){ - QWidget* q = new QWidget(parent); - q->setLayout(l); - return q; -} - // Networking functions Networking::Networking(QWidget* parent, bool show_advanced) : QWidget(parent), show_advanced(show_advanced){ @@ -42,7 +32,7 @@ Networking::Networking(QWidget* parent, bool show_advanced) : QWidget(parent), s setLayout(s); QTimer* timer = new QTimer(this); - QObject::connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); + QObject::connect(timer, &QTimer::timeout, this, &Networking::refresh); timer->start(5000); attemptInitialization(); } @@ -55,25 +45,26 @@ void Networking::attemptInitialization(){ return; } - connect(wifi, SIGNAL(wrongPassword(QString)), this, SLOT(wrongPassword(QString))); + connect(wifi, &WifiManager::wrongPassword, this, &Networking::wrongPassword); QVBoxLayout* vlayout = new QVBoxLayout; if (show_advanced) { QPushButton* advancedSettings = new QPushButton("Advanced"); - advancedSettings->setStyleSheet(R"(margin-right: 30px)"); + advancedSettings->setStyleSheet("margin-right: 30px;"); advancedSettings->setFixedSize(350, 100); - connect(advancedSettings, &QPushButton::released, [=](){s->setCurrentWidget(an);}); + connect(advancedSettings, &QPushButton::released, [=](){ s->setCurrentWidget(an); }); vlayout->addSpacing(10); vlayout->addWidget(advancedSettings, 0, Qt::AlignRight); vlayout->addSpacing(10); } - wifiWidget = new WifiUI(0, wifi); - connect(wifiWidget, SIGNAL(connectToNetwork(Network)), this, SLOT(connectToNetwork(Network))); - vlayout->addWidget(wifiWidget, 1); + wifiWidget = new WifiUI(this, wifi); + connect(wifiWidget, &WifiUI::connectToNetwork, this, &Networking::connectToNetwork); + vlayout->addWidget(new ScrollView(wifiWidget, this), 1); - wifiScreen = layoutToWidget(vlayout, this); + QWidget* wifiScreen = new QWidget(this); + wifiScreen->setLayout(vlayout); s->addWidget(wifiScreen); an = new AdvancedNetworking(this, wifi); @@ -138,6 +129,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid QVBoxLayout* vlayout = new QVBoxLayout; vlayout->setMargin(40); + vlayout->setSpacing(20); // Back button QPushButton* back = new QPushButton("Back"); @@ -146,41 +138,24 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid vlayout->addWidget(back, 0, Qt::AlignLeft); // Enable tethering layout - QHBoxLayout* tetheringToggleLayout = new QHBoxLayout; - tetheringToggleLayout->addWidget(new QLabel("Enable tethering")); - Toggle* toggle_switch = new Toggle; - toggle_switch->setFixedSize(150, 100); - tetheringToggleLayout->addWidget(toggle_switch); - tetheringToggleLayout->addSpacing(40); - if (wifi->tetheringEnabled()) { - toggle_switch->togglePosition(); - } - QObject::connect(toggle_switch, SIGNAL(stateChanged(bool)), this, SLOT(toggleTethering(bool))); - vlayout->addWidget(layoutToWidget(tetheringToggleLayout, this), 0); + ToggleControl *tetheringToggle = new ToggleControl("Enable Tethering", "", "", wifi->tetheringEnabled()); + vlayout->addWidget(tetheringToggle); + QObject::connect(tetheringToggle, &ToggleControl::toggleFlipped, this, &AdvancedNetworking::toggleTethering); vlayout->addWidget(horizontal_line(), 0); // Change tethering password - QHBoxLayout *tetheringPassword = new QHBoxLayout; - tetheringPassword->addWidget(new QLabel("Edit tethering password"), 1); - editPasswordButton = new QPushButton("EDIT"); - editPasswordButton->setFixedWidth(500); - connect(editPasswordButton, &QPushButton::released, [=](){ + editPasswordButton = new ButtonControl("Tethering Password", "EDIT", "", [=](){ QString pass = InputDialog::getText("Enter new tethering password", 8); if (pass.size()) { wifi->changeTetheringPassword(pass); } }); - tetheringPassword->addWidget(editPasswordButton, 1, Qt::AlignRight); - vlayout->addWidget(layoutToWidget(tetheringPassword, this), 0); + vlayout->addWidget(editPasswordButton, 0); vlayout->addWidget(horizontal_line(), 0); - // IP adress - QHBoxLayout* IPlayout = new QHBoxLayout; - IPlayout->addWidget(new QLabel("IP address"), 0); - ipLabel = new QLabel(wifi->ipv4_address); - ipLabel->setStyleSheet("color: #aaaaaa"); - IPlayout->addWidget(ipLabel, 0, Qt::AlignRight); - vlayout->addWidget(layoutToWidget(IPlayout, this), 0); + // IP address + ipLabel = new LabelControl("IP Address", wifi->ipv4_address); + vlayout->addWidget(ipLabel, 0); vlayout->addWidget(horizontal_line(), 0); // SSH keys @@ -188,6 +163,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid vlayout->addWidget(horizontal_line(), 0); vlayout->addWidget(new SshControl()); + vlayout->addStretch(1); setLayout(vlayout); } @@ -218,7 +194,6 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi) vlayout->setSpacing(25); setLayout(vlayout); - page = 0; } void WifiUI::refresh() { @@ -227,72 +202,42 @@ void WifiUI::refresh() { clearLayout(vlayout); connectButtons = new QButtonGroup(this); // TODO check if this is a leak - QObject::connect(connectButtons, SIGNAL(buttonClicked(QAbstractButton*)), this, SLOT(handleButton(QAbstractButton*))); - - int networks_per_page = height() / 180; + QObject::connect(connectButtons, qOverload(&QButtonGroup::buttonClicked), this, &WifiUI::handleButton); int i = 0; - int pageCount = (wifi->seen_networks.size() - 1) / networks_per_page; - page = std::max(0, std::min(page, pageCount)); for (Network &network : wifi->seen_networks) { QHBoxLayout *hlayout = new QHBoxLayout; - if (page * networks_per_page <= i && i < (page + 1) * networks_per_page) { - // SSID - hlayout->addSpacing(50); - QString ssid = QString::fromUtf8(network.ssid); - if(ssid.length() > 20){ - ssid = ssid.left(20 - 3) + "…"; - } + hlayout->addSpacing(50); - QLabel *ssid_label = new QLabel(ssid); - ssid_label->setStyleSheet(R"( - font-size: 55px; - )"); - ssid_label->setFixedWidth(this->width()*0.5); - hlayout->addWidget(ssid_label, 0, Qt::AlignLeft); + QLabel *ssid_label = new QLabel(QString::fromUtf8(network.ssid)); + ssid_label->setStyleSheet("font-size: 55px;"); + hlayout->addWidget(ssid_label, 1, Qt::AlignLeft); - // TODO: don't use images for this - // strength indicator - unsigned int strength_scale = network.strength / 17; - QPixmap pix("../assets/images/network_" + QString::number(strength_scale) + ".png"); - QLabel *icon = new QLabel(); - icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); - icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); - hlayout->addWidget(icon, 0, Qt::AlignRight); + // TODO: don't use images for this + // strength indicator + unsigned int strength_scale = network.strength / 17; + QPixmap pix("../assets/images/network_" + QString::number(strength_scale) + ".png"); + QLabel *icon = new QLabel(); + icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); + icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + hlayout->addWidget(icon, 0, Qt::AlignRight); - // connect button - QPushButton* btn = new QPushButton(network.security_type == SecurityType::UNSUPPORTED ? "Unsupported" : (network.connected == ConnectedType::CONNECTED ? "Connected" : (network.connected == ConnectedType::CONNECTING ? "Connecting" : "Connect"))); - btn->setDisabled(network.connected == ConnectedType::CONNECTED || network.connected == ConnectedType::CONNECTING || network.security_type == SecurityType::UNSUPPORTED); - btn->setFixedWidth(350); - hlayout->addWidget(btn, 0, Qt::AlignRight); + // connect button + QPushButton* btn = new QPushButton(network.security_type == SecurityType::UNSUPPORTED ? "Unsupported" : (network.connected == ConnectedType::CONNECTED ? "Connected" : (network.connected == ConnectedType::CONNECTING ? "Connecting" : "Connect"))); + btn->setDisabled(network.connected == ConnectedType::CONNECTED || network.connected == ConnectedType::CONNECTING || network.security_type == SecurityType::UNSUPPORTED); + btn->setFixedWidth(350); + hlayout->addWidget(btn, 0, Qt::AlignRight); - connectButtons->addButton(btn, i); + connectButtons->addButton(btn, i); - vlayout->addLayout(hlayout, 1); - // Don't add the last horizontal line - if (page * networks_per_page <= i+1 && i+1 < (page + 1) * networks_per_page && i+1 < wifi->seen_networks.size()) { - vlayout->addWidget(horizontal_line(), 0); - } + vlayout->addLayout(hlayout, 1); + // Don't add the last horizontal line + if (i+1 < wifi->seen_networks.size()) { + vlayout->addWidget(horizontal_line(), 0); } i++; } vlayout->addStretch(3); - - - // Setup buttons for pagination - QHBoxLayout *prev_next_buttons = new QHBoxLayout; - - QPushButton* prev = new QPushButton("Previous"); - prev->setEnabled(page); - QObject::connect(prev, SIGNAL(released()), this, SLOT(prevPage())); - prev_next_buttons->addWidget(prev); - - QPushButton* next = new QPushButton("Next"); - next->setEnabled(wifi->seen_networks.size() > (page + 1) * networks_per_page); - QObject::connect(next, SIGNAL(released()), this, SLOT(nextPage())); - prev_next_buttons->addWidget(next); - - vlayout->addLayout(prev_next_buttons, 2); } void WifiUI::handleButton(QAbstractButton* button) { @@ -300,13 +245,3 @@ void WifiUI::handleButton(QAbstractButton* button) { Network n = wifi->seen_networks[connectButtons->id(btn)]; emit connectToNetwork(n); } - -void WifiUI::prevPage() { - page--; - refresh(); -} - -void WifiUI::nextPage() { - page++; - refresh(); -} diff --git a/selfdrive/ui/qt/offroad/networking.hpp b/selfdrive/ui/qt/offroad/networking.h similarity index 80% rename from selfdrive/ui/qt/offroad/networking.hpp rename to selfdrive/ui/qt/offroad/networking.h index fba770507..9fa1cfe88 100644 --- a/selfdrive/ui/qt/offroad/networking.hpp +++ b/selfdrive/ui/qt/offroad/networking.h @@ -1,21 +1,20 @@ #pragma once -#include #include -#include -#include #include +#include +#include +#include -#include "wifiManager.hpp" -#include "widgets/input.hpp" -#include "widgets/ssh_keys.hpp" -#include "widgets/toggle.hpp" +#include "selfdrive/ui/qt/offroad/wifiManager.h" +#include "selfdrive/ui/qt/widgets/input.h" +#include "selfdrive/ui/qt/widgets/ssh_keys.h" +#include "selfdrive/ui/qt/widgets/toggle.h" class WifiUI : public QWidget { Q_OBJECT public: - int page; explicit WifiUI(QWidget *parent = 0, WifiManager* wifi = 0); private: @@ -29,10 +28,8 @@ signals: void connectToNetwork(Network n); public slots: - void handleButton(QAbstractButton* m_button); void refresh(); - void prevPage(); - void nextPage(); + void handleButton(QAbstractButton* m_button); }; class AdvancedNetworking : public QWidget { @@ -41,8 +38,8 @@ public: explicit AdvancedNetworking(QWidget* parent = 0, WifiManager* wifi = 0); private: - QLabel* ipLabel; - QPushButton* editPasswordButton; + LabelControl* ipLabel; + ButtonControl* editPasswordButton; WifiManager* wifi = nullptr; signals: @@ -60,7 +57,7 @@ public: explicit Networking(QWidget* parent = 0, bool show_advanced = true); private: - QStackedLayout* s = nullptr; // nm_warning, keyboard, wifiScreen, advanced + QStackedLayout* s = nullptr; // nm_warning, wifiScreen, advanced QWidget* wifiScreen = nullptr; AdvancedNetworking* an = nullptr; bool ui_setup_complete = false; diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc index f9f8972b8..8ce48becd 100644 --- a/selfdrive/ui/qt/offroad/onboarding.cc +++ b/selfdrive/ui/qt/offroad/onboarding.cc @@ -1,15 +1,15 @@ +#include "onboarding.h" + +#include #include #include -#include -#include #include -#include - -#include "common/params.h" -#include "onboarding.hpp" -#include "home.hpp" -#include "util.h" +#include +#include +#include "selfdrive/common/params.h" +#include "selfdrive/common/util.h" +#include "selfdrive/ui/qt/home.h" void TrainingGuide::mouseReleaseEvent(QMouseEvent *e) { QPoint touch = QPoint(e->x(), e->y()) - imageCorner; @@ -50,7 +50,12 @@ void TrainingGuide::paintEvent(QPaintEvent *event) { imageCorner = rect.topLeft(); } -TermsPage::TermsPage(QWidget *parent) : QFrame(parent){ +void TermsPage::showEvent(QShowEvent *event) { + // late init, building QML widget takes 200ms + if (layout()) { + return; + } + QVBoxLayout *main_layout = new QVBoxLayout; main_layout->setMargin(40); main_layout->setSpacing(40); @@ -61,15 +66,17 @@ TermsPage::TermsPage(QWidget *parent) : QFrame(parent){ text->setAttribute(Qt::WA_AlwaysStackOnTop); text->setClearColor(Qt::transparent); - text->rootContext()->setContextProperty("font_size", 55); - QString text_view = util::read_file("../assets/offroad/tc.html").c_str(); text->rootContext()->setContextProperty("text_view", text_view); + text->rootContext()->setContextProperty("font_size", 55); text->setSource(QUrl::fromLocalFile("qt/offroad/text_view.qml")); main_layout->addWidget(text); + QObject *obj = (QObject*)text->rootObject(); + QObject::connect(obj, SIGNAL(qmlSignal()), SLOT(enableAccept())); + // TODO: add decline page QHBoxLayout* buttons = new QHBoxLayout; main_layout->addLayout(buttons); @@ -80,19 +87,13 @@ TermsPage::TermsPage(QWidget *parent) : QFrame(parent){ accept_btn = new QPushButton("Scroll to accept"); accept_btn->setEnabled(false); buttons->addWidget(accept_btn); - QObject::connect(accept_btn, &QPushButton::released, [=]() { - emit acceptedTerms(); - }); + QObject::connect(accept_btn, &QPushButton::released, this, &TermsPage::acceptedTerms); - QObject *obj = (QObject*)text->rootObject(); - QObject::connect(obj, SIGNAL(qmlSignal()), SLOT(enableAccept())); setLayout(main_layout); setStyleSheet(R"( - * { - font-size: 50px; - } QPushButton { padding: 50px; + font-size: 50px; border-radius: 10px; background-color: #292929; } @@ -127,18 +128,17 @@ OnboardingWindow::OnboardingWindow(QWidget *parent) : QStackedWidget(parent) { addWidget(terms); connect(terms, &TermsPage::acceptedTerms, [=](){ - Params().write_db_value("HasAcceptedTerms", current_terms_version); + Params().put("HasAcceptedTerms", current_terms_version); updateActiveScreen(); }); TrainingGuide* tr = new TrainingGuide(this); connect(tr, &TrainingGuide::completedTraining, [=](){ - Params().write_db_value("CompletedTrainingVersion", current_training_version); + Params().put("CompletedTrainingVersion", current_training_version); updateActiveScreen(); }); addWidget(tr); - setStyleSheet(R"( * { color: white; diff --git a/selfdrive/ui/qt/offroad/onboarding.hpp b/selfdrive/ui/qt/offroad/onboarding.h similarity index 93% rename from selfdrive/ui/qt/offroad/onboarding.hpp rename to selfdrive/ui/qt/offroad/onboarding.h index 227090fd3..987fad336 100644 --- a/selfdrive/ui/qt/offroad/onboarding.hpp +++ b/selfdrive/ui/qt/offroad/onboarding.h @@ -1,11 +1,11 @@ #pragma once -#include -#include -#include +#include #include #include -#include +#include +#include +#include #include "selfdrive/common/params.h" @@ -57,7 +57,10 @@ class TermsPage : public QFrame { Q_OBJECT public: - explicit TermsPage(QWidget *parent = 0); + explicit TermsPage(QWidget *parent = 0) : QFrame(parent) {}; + +protected: + void showEvent(QShowEvent *event) override; private: QPushButton *accept_btn; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 8abaeb7ba..06c3a3671 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -1,72 +1,99 @@ -#include +#include "settings.h" + +#include #include #include -#include +#include #ifndef QCOM -#include "networking.hpp" +#include "selfdrive/ui/qt/offroad/networking.h" #endif -#include "settings.hpp" -#include "widgets/input.hpp" -#include "widgets/toggle.hpp" -#include "widgets/offroad_alerts.hpp" -#include "widgets/controls.hpp" -#include "widgets/ssh_keys.hpp" -#include "common/params.h" -#include "common/util.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/widgets/input.h" +#include "selfdrive/ui/qt/widgets/offroad_alerts.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" +#include "selfdrive/ui/qt/widgets/ssh_keys.h" +#include "selfdrive/ui/qt/widgets/toggle.h" +#include "selfdrive/ui/ui.h" - -QWidget * toggles_panel() { +TogglesPanel::TogglesPanel(QWidget *parent) : QWidget(parent) { QVBoxLayout *toggles_list = new QVBoxLayout(); - toggles_list->addWidget(new ParamControl("OpenpilotEnabledToggle", - "Enable openpilot", - "Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.", - "../assets/offroad/icon_openpilot.png" - )); - toggles_list->addWidget(horizontal_line()); - toggles_list->addWidget(new ParamControl("IsLdwEnabled", - "Enable Lane Departure Warnings", - "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31mph (50kph).", - "../assets/offroad/icon_warning.png" - )); - toggles_list->addWidget(horizontal_line()); - toggles_list->addWidget(new ParamControl("IsRHD", - "Enable Right-Hand Drive", - "Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.", - "../assets/offroad/icon_openpilot_mirrored.png" - )); - toggles_list->addWidget(horizontal_line()); - toggles_list->addWidget(new ParamControl("IsMetric", - "Use Metric System", - "Display speed in km/h instead of mp/h.", - "../assets/offroad/icon_metric.png" - )); - toggles_list->addWidget(horizontal_line()); - toggles_list->addWidget(new ParamControl("CommunityFeaturesToggle", - "Enable Community Features", - "Use features from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. These features include community supported cars and community supported hardware. Be extra cautious when using these features", - "../assets/offroad/icon_shell.png" - )); - toggles_list->addWidget(horizontal_line()); - ParamControl *record_toggle = new ParamControl("RecordFront", - "Record and Upload Driver Camera", - "Upload data from the driver facing camera and help improve the driver monitoring algorithm.", - "../assets/offroad/icon_network.png"); - toggles_list->addWidget(record_toggle); - toggles_list->addWidget(horizontal_line()); - toggles_list->addWidget(new ParamControl("EndToEndToggle", - "\U0001f96c Disable use of lanelines (Alpha) \U0001f96c", - "In this mode openpilot will ignore lanelines and just drive how it thinks a human would.", - "../assets/offroad/icon_road.png")); + QList toggles; - bool record_lock = Params().read_db_bool("RecordFrontLock"); + toggles.append(new ParamControl("OpenpilotEnabledToggle", + "Enable openpilot", + "Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.", + "../assets/offroad/icon_openpilot.png", + this)); + toggles.append(new ParamControl("IsLdwEnabled", + "Enable Lane Departure Warnings", + "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31mph (50kph).", + "../assets/offroad/icon_warning.png", + this)); + toggles.append(new ParamControl("IsRHD", + "Enable Right-Hand Drive", + "Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.", + "../assets/offroad/icon_openpilot_mirrored.png", + this)); + toggles.append(new ParamControl("IsMetric", + "Use Metric System", + "Display speed in km/h instead of mp/h.", + "../assets/offroad/icon_metric.png", + this)); + toggles.append(new ParamControl("CommunityFeaturesToggle", + "Enable Community Features", + "Use features from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. These features include community supported cars and community supported hardware. Be extra cautious when using these features", + "../assets/offroad/icon_shell.png", + this)); + + if (!Hardware::TICI()) { + toggles.append(new ParamControl("IsUploadRawEnabled", + "Upload Raw Logs", + "Upload full logs and full resolution video by default while on WiFi. If not enabled, individual logs can be marked for upload at my.comma.ai/useradmin.", + "../assets/offroad/icon_network.png", + this)); + } + + ParamControl *record_toggle = new ParamControl("RecordFront", + "Record and Upload Driver Camera", + "Upload data from the driver facing camera and help improve the driver monitoring algorithm.", + "../assets/offroad/icon_monitoring.png", + this); + toggles.append(record_toggle); + toggles.append(new ParamControl("EndToEndToggle", + "\U0001f96c Disable use of lanelines (Alpha) \U0001f96c", + "In this mode openpilot will ignore lanelines and just drive how it thinks a human would.", + "../assets/offroad/icon_road.png", + this)); + + if (Hardware::TICI()) { + toggles.append(new ParamControl("EnableWideCamera", + "Enable use of Wide Angle Camera", + "Use wide angle camera for driving and ui. Only takes effect after reboot.", + "../assets/offroad/icon_openpilot.png", + this)); + toggles.append(new ParamControl("EnableLteOnroad", + "Enable LTE while onroad", + "", + "../assets/offroad/icon_network.png", + this)); + } + + bool record_lock = Params().getBool("RecordFrontLock"); record_toggle->setEnabled(!record_lock); - QWidget *widget = new QWidget; - widget->setLayout(toggles_list); - return widget; + for(ParamControl *toggle : toggles){ + if(toggles_list->count() != 0){ + toggles_list->addWidget(horizontal_line()); + } + toggles_list->addWidget(toggle); + } + + setLayout(toggles_list); } DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) { @@ -85,30 +112,55 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) { QList offroad_btns; offroad_btns.append(new ButtonControl("Driver Camera", "PREVIEW", - "Preview the driver facing camera to help optimize device mounting position for best driver monitoring experience. (vehicle must be off)", - [=]() { Params().write_db_value("IsDriverViewEnabled", "1", 1); })); + "Preview the driver facing camera to help optimize device mounting position for best driver monitoring experience. (vehicle must be off)", + [=]() { + Params().putBool("IsDriverViewEnabled", true); + QUIState::ui_state.scene.driver_view = true; + }, "", this)); - offroad_btns.append(new ButtonControl("Reset Calibration", "RESET", - "openpilot requires the device to be mounted within 4° left or right and within 5° up or down. openpilot is continuously calibrating, resetting is rarely required.", [=]() { - if (ConfirmationDialog::confirm("Are you sure you want to reset calibration?")) { - Params().delete_db_value("CalibrationParams"); + QString resetCalibDesc = "openpilot requires the device to be mounted within 4° left or right and within 5° up or down. openpilot is continuously calibrating, resetting is rarely required."; + ButtonControl *resetCalibBtn = new ButtonControl("Reset Calibration", "RESET", resetCalibDesc, [=]() { + if (ConfirmationDialog::confirm("Are you sure you want to reset calibration?", this)) { + Params().remove("CalibrationParams"); } - })); + }, "", this); + connect(resetCalibBtn, &ButtonControl::showDescription, [=]() { + QString desc = resetCalibDesc; + std::string calib_bytes = Params().get("CalibrationParams"); + if (!calib_bytes.empty()) { + try { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(calib_bytes.data(), calib_bytes.size())); + auto calib = cmsg.getRoot().getLiveCalibration(); + if (calib.getCalStatus() != 0) { + double pitch = calib.getRpyCalib()[1] * (180 / M_PI); + double yaw = calib.getRpyCalib()[2] * (180 / M_PI); + desc += QString(" Your device is pointed %1° %2 and %3° %4.") + .arg(QString::number(std::abs(pitch), 'g', 1), pitch > 0 ? "up" : "down", + QString::number(std::abs(yaw), 'g', 1), yaw > 0 ? "right" : "left"); + } + } catch (kj::Exception) { + qInfo() << "invalid CalibrationParams"; + } + } + resetCalibBtn->setDescription(desc); + }); + offroad_btns.append(resetCalibBtn); offroad_btns.append(new ButtonControl("Review Training Guide", "REVIEW", "Review the rules, features, and limitations of openpilot", [=]() { - if (ConfirmationDialog::confirm("Are you sure you want to review the training guide?")) { - Params().delete_db_value("CompletedTrainingVersion"); + if (ConfirmationDialog::confirm("Are you sure you want to review the training guide?", this)) { + Params().remove("CompletedTrainingVersion"); emit reviewTrainingGuide(); } - })); + }, "", this)); - QString brand = params.read_db_bool("Passive") ? "dashcam" : "openpilot"; + QString brand = params.getBool("Passive") ? "dashcam" : "openpilot"; offroad_btns.append(new ButtonControl("Uninstall " + brand, "UNINSTALL", "", [=]() { - if (ConfirmationDialog::confirm("Are you sure you want to uninstall?")) { - Params().write_db_value("DoUninstall", "1"); + if (ConfirmationDialog::confirm("Are you sure you want to uninstall?", this)) { + Params().putBool("DoUninstall", true); } - })); + }, "", this)); for(auto &btn : offroad_btns){ device_layout->addWidget(horizontal_line()); @@ -123,7 +175,7 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) { QPushButton *reboot_btn = new QPushButton("Reboot"); power_layout->addWidget(reboot_btn); QObject::connect(reboot_btn, &QPushButton::released, [=]() { - if (ConfirmationDialog::confirm("Are you sure you want to reboot?")) { + if (ConfirmationDialog::confirm("Are you sure you want to reboot?", this)) { Hardware::reboot(); } }); @@ -132,7 +184,7 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) { poweroff_btn->setStyleSheet("background-color: #E22C2C;"); power_layout->addWidget(poweroff_btn); QObject::connect(poweroff_btn, &QPushButton::released, [=]() { - if (ConfirmationDialog::confirm("Are you sure you want to power off?")) { + if (ConfirmationDialog::confirm("Are you sure you want to power off?", this)) { Hardware::poweroff(); } }); @@ -158,7 +210,7 @@ DeveloperPanel::DeveloperPanel(QWidget* parent) : QFrame(parent) { void DeveloperPanel::showEvent(QShowEvent *event) { Params params = Params(); - std::string brand = params.read_db_bool("Passive") ? "dashcam" : "openpilot"; + std::string brand = params.getBool("Passive") ? "dashcam" : "openpilot"; QList> dev_params = { {"Version", brand + " v" + params.get("Version", false).substr(0, 14)}, {"Git Branch", params.get("GitBranch", false)}, @@ -211,7 +263,13 @@ QWidget * network_panel(QWidget * parent) { return w; } -SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { +void SettingsWindow::showEvent(QShowEvent *event) { + if (layout()) { + panel_widget->setCurrentIndex(0); + nav_btns->buttons()[0]->setChecked(true); + return; + } + // setup two main layouts QVBoxLayout *sidebar_layout = new QVBoxLayout(); sidebar_layout->setMargin(0); @@ -233,16 +291,16 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { close_btn->setFixedSize(200, 200); sidebar_layout->addSpacing(45); sidebar_layout->addWidget(close_btn, 0, Qt::AlignCenter); - QObject::connect(close_btn, SIGNAL(released()), this, SIGNAL(closeSettings())); + QObject::connect(close_btn, &QPushButton::released, this, &SettingsWindow::closeSettings); // setup panels DevicePanel *device = new DevicePanel(this); - QObject::connect(device, SIGNAL(reviewTrainingGuide()), this, SIGNAL(reviewTrainingGuide())); + QObject::connect(device, &DevicePanel::reviewTrainingGuide, this, &SettingsWindow::reviewTrainingGuide); QPair panels[] = { {"Device", device}, {"Network", network_panel(this)}, - {"Toggles", toggles_panel()}, + {"Toggles", new TogglesPanel(this)}, {"Developer", new DeveloperPanel()}, }; @@ -251,6 +309,7 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { for (auto &[name, panel] : panels) { QPushButton *btn = new QPushButton(name); btn->setCheckable(true); + btn->setChecked(nav_btns->buttons().size() == 0); btn->setStyleSheet(R"( QPushButton { color: grey; @@ -270,28 +329,14 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { sidebar_layout->addWidget(btn, 0, Qt::AlignRight); panel->setContentsMargins(50, 25, 50, 25); - QScrollArea *panel_frame = new QScrollArea; - panel_frame->setWidget(panel); - panel_frame->setWidgetResizable(true); - panel_frame->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - panel_frame->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - panel_frame->setStyleSheet("background-color:transparent;"); - - QScroller *scroller = QScroller::scroller(panel_frame->viewport()); - auto sp = scroller->scrollerProperties(); - - sp.setScrollMetric(QScrollerProperties::VerticalOvershootPolicy, QVariant::fromValue(QScrollerProperties::OvershootAlwaysOff)); - - scroller->grabGesture(panel_frame->viewport(), QScroller::LeftMouseButtonGesture); - scroller->setScrollerProperties(sp); + ScrollView *panel_frame = new ScrollView(panel, this); panel_widget->addWidget(panel_frame); QObject::connect(btn, &QPushButton::released, [=, w = panel_frame]() { panel_widget->setCurrentWidget(w); }); } - qobject_cast(nav_btns->buttons()[0])->setChecked(true); sidebar_layout->setContentsMargins(50, 50, 100, 50); // main settings layout, sidebar + main panel @@ -314,3 +359,17 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { } )"); } + +void SettingsWindow::hideEvent(QHideEvent *event){ +#ifdef QCOM + HardwareEon::close_activities(); +#endif + + // TODO: this should be handled by the Dialog classes + QList children = findChildren(); + for(auto &w : children){ + if(w->metaObject()->superClass()->className() == QString("QDialog")){ + w->close(); + } + } +} diff --git a/selfdrive/ui/qt/offroad/settings.hpp b/selfdrive/ui/qt/offroad/settings.h similarity index 74% rename from selfdrive/ui/qt/offroad/settings.hpp rename to selfdrive/ui/qt/offroad/settings.h index 2dee5b62d..09a907c8e 100644 --- a/selfdrive/ui/qt/offroad/settings.hpp +++ b/selfdrive/ui/qt/offroad/settings.h @@ -1,15 +1,15 @@ #pragma once -#include +#include #include -#include #include #include -#include #include #include +#include +#include -#include "selfdrive/ui/qt/widgets/controls.hpp" +#include "selfdrive/ui/qt/widgets/controls.h" // ********** settings window + top-level panels ********** @@ -21,6 +21,12 @@ signals: void reviewTrainingGuide(); }; +class TogglesPanel : public QWidget { + Q_OBJECT +public: + explicit TogglesPanel(QWidget *parent = nullptr); +}; + class DeveloperPanel : public QFrame { Q_OBJECT public: @@ -35,7 +41,11 @@ class SettingsWindow : public QFrame { Q_OBJECT public: - explicit SettingsWindow(QWidget *parent = 0); + explicit SettingsWindow(QWidget *parent = 0) : QFrame(parent) {}; + +protected: + void hideEvent(QHideEvent *event); + void showEvent(QShowEvent *event); signals: void closeSettings(); diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index 8caf6f6d7..d7ff5bddb 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -1,11 +1,13 @@ -#include -#include -#include -#include +#include "wifiManager.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "wifiManager.hpp" +#include + +#include +#include +#include + +#include "selfdrive/common/params.h" +#include "selfdrive/common/swaglog.h" /** * We are using a NetworkManager DBUS API : https://developer.gnome.org/NetworkManager/1.26/spec.html diff --git a/selfdrive/ui/qt/offroad/wifiManager.hpp b/selfdrive/ui/qt/offroad/wifiManager.h similarity index 100% rename from selfdrive/ui/qt/offroad/wifiManager.hpp rename to selfdrive/ui/qt/offroad/wifiManager.h diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc new file mode 100644 index 000000000..c285c0728 --- /dev/null +++ b/selfdrive/ui/qt/onroad.cc @@ -0,0 +1,206 @@ +#include "selfdrive/ui/qt/onroad.h" + +#include + +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/timing.h" +#include "selfdrive/ui/paint.h" +#include "selfdrive/ui/qt/util.h" + +OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { + layout = new QStackedLayout(); + layout->setStackingMode(QStackedLayout::StackAll); + + // old UI on bottom + nvg = new NvgWindow(this); + layout->addWidget(nvg); + QObject::connect(this, &OnroadWindow::update, nvg, &NvgWindow::update); + + alerts = new OnroadAlerts(this); + QObject::connect(this, &OnroadWindow::update, alerts, &OnroadAlerts::updateState); + QObject::connect(this, &OnroadWindow::offroadTransition, alerts, &OnroadAlerts::offroadTransition); + layout->addWidget(alerts); + + // setup stacking order + alerts->raise(); + + setLayout(layout); + setAttribute(Qt::WA_OpaquePaintEvent); +} + +// ***** onroad widgets ***** + +OnroadAlerts::OnroadAlerts(QWidget *parent) : QWidget(parent) { + for (auto &kv : sound_map) { + auto path = QUrl::fromLocalFile(kv.second.first); + sounds[kv.first].setSource(path); + } +} + +void OnroadAlerts::updateState(const UIState &s) { + SubMaster &sm = *(s.sm); + if (sm.updated("carState")) { + // scale volume with speed + volume = util::map_val(sm["carState"].getCarState().getVEgo(), 0.f, 20.f, + Hardware::MIN_VOLUME, Hardware::MAX_VOLUME); + } + if (s.scene.deviceState.getStarted()) { + if (sm.updated("controlsState")) { + const cereal::ControlsState::Reader &cs = sm["controlsState"].getControlsState(); + updateAlert(QString::fromStdString(cs.getAlertText1()), QString::fromStdString(cs.getAlertText2()), + cs.getAlertBlinkingRate(), cs.getAlertType(), cs.getAlertSize(), cs.getAlertSound()); + } else if ((sm.frame - s.scene.started_frame) > 10 * UI_FREQ) { + // Handle controls timeout + if (sm.rcv_frame("controlsState") < s.scene.started_frame) { + // car is started, but controlsState hasn't been seen at all + updateAlert("openpilot Unavailable", "Waiting for controls to start", 0, + "controlsWaiting", cereal::ControlsState::AlertSize::MID, AudibleAlert::NONE); + } else if ((sm.frame - sm.rcv_frame("controlsState")) > 5 * UI_FREQ) { + // car is started, but controls is lagging or died + updateAlert("TAKE CONTROL IMMEDIATELY", "Controls Unresponsive", 0, + "controlsUnresponsive", cereal::ControlsState::AlertSize::FULL, AudibleAlert::CHIME_WARNING_REPEAT); + + // TODO: clean this up once Qt handles the border + QUIState::ui_state.status = STATUS_ALERT; + } + } + } + + // TODO: add blinking back if performant + //float alpha = 0.375 * cos((millis_since_boot() / 1000) * 2 * M_PI * blinking_rate) + 0.625; + auto c = bg_colors[s.status]; + bg.setRgbF(c.r, c.g, c.b, c.a); +} + +void OnroadAlerts::offroadTransition(bool offroad) { + updateAlert("", "", 0, "", cereal::ControlsState::AlertSize::NONE, AudibleAlert::NONE); +} + +void OnroadAlerts::updateAlert(const QString &t1, const QString &t2, float blink_rate, + const std::string &type, cereal::ControlsState::AlertSize size, AudibleAlert sound) { + if (alert_type.compare(type) == 0 && text1.compare(t1) == 0) { + return; + } + + stopSounds(); + if (sound != AudibleAlert::NONE) { + playSound(sound); + } + + text1 = t1; + text2 = t2; + alert_type = type; + alert_size = size; + blinking_rate = blink_rate; + + update(); +} + +void OnroadAlerts::playSound(AudibleAlert alert) { + int loops = sound_map[alert].second ? QSoundEffect::Infinite : 0; + sounds[alert].setLoopCount(loops); + sounds[alert].setVolume(volume); + sounds[alert].play(); +} + +void OnroadAlerts::stopSounds() { + for (auto &kv : sounds) { + // Only stop repeating sounds + if (kv.second.loopsRemaining() == QSoundEffect::Infinite) { + kv.second.stop(); + } + } +} + +void OnroadAlerts::paintEvent(QPaintEvent *event) { + QPainter p(this); + + if (alert_size == cereal::ControlsState::AlertSize::NONE) { + return; + } + static std::map alert_sizes = { + {cereal::ControlsState::AlertSize::SMALL, 271}, + {cereal::ControlsState::AlertSize::MID, 420}, + {cereal::ControlsState::AlertSize::FULL, height()}, + }; + int h = alert_sizes[alert_size]; + QRect r = QRect(0, height() - h, width(), h); + + // draw background + gradient + p.setPen(Qt::NoPen); + p.setCompositionMode(QPainter::CompositionMode_DestinationOver); + + p.setBrush(QBrush(bg)); + p.drawRect(r); + + QLinearGradient g(0, r.y(), 0, r.bottom()); + g.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.05)); + g.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0.35)); + p.setBrush(QBrush(g)); + p.fillRect(r, g); + p.setCompositionMode(QPainter::CompositionMode_SourceOver); + + // remove bottom border + r = QRect(0, height() - h, width(), h - 30); + + // text + const QPoint c = r.center(); + p.setPen(QColor(0xff, 0xff, 0xff)); + p.setRenderHint(QPainter::TextAntialiasing); + if (alert_size == cereal::ControlsState::AlertSize::SMALL) { + configFont(p, "Open Sans", 74, "SemiBold"); + p.drawText(r, Qt::AlignCenter, text1); + } else if (alert_size == cereal::ControlsState::AlertSize::MID) { + configFont(p, "Open Sans", 88, "Bold"); + p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, text1); + configFont(p, "Open Sans", 66, "Regular"); + p.drawText(QRect(0, c.y() + 21, width(), 90), Qt::AlignHCenter, text2); + } else if (alert_size == cereal::ControlsState::AlertSize::FULL) { + bool l = text1.length() > 15; + configFont(p, "Open Sans", l ? 132 : 177, "Bold"); + p.drawText(QRect(0, r.y() + (l ? 240 : 270), width(), 600), Qt::AlignHCenter | Qt::TextWordWrap, text1); + configFont(p, "Open Sans", 88, "Regular"); + p.drawText(QRect(0, r.height() - (l ? 361 : 420), width(), 300), Qt::AlignHCenter | Qt::TextWordWrap, text2); + } +} + + +NvgWindow::NvgWindow(QWidget *parent) : QOpenGLWidget(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); +} + +NvgWindow::~NvgWindow() { + makeCurrent(); + doneCurrent(); +} + +void NvgWindow::initializeGL() { + initializeOpenGLFunctions(); + std::cout << "OpenGL version: " << glGetString(GL_VERSION) << std::endl; + std::cout << "OpenGL vendor: " << glGetString(GL_VENDOR) << std::endl; + std::cout << "OpenGL renderer: " << glGetString(GL_RENDERER) << std::endl; + std::cout << "OpenGL language version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl; + + ui_nvg_init(&QUIState::ui_state); + prev_draw_t = millis_since_boot(); +} + +void NvgWindow::update(const UIState &s) { + // Connecting to visionIPC requires opengl to be current + if (s.vipc_client->connected){ + makeCurrent(); + } + repaint(); +} + +void NvgWindow::paintGL() { + ui_draw(&QUIState::ui_state, width(), height()); + + double cur_draw_t = millis_since_boot(); + double dt = cur_draw_t - prev_draw_t; + if (dt > 66 && !QUIState::ui_state.scene.driver_view) { + // warn on sub 15fps + LOGW("slow frame time: %.2f", dt); + } + prev_draw_t = cur_draw_t; +} diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h new file mode 100644 index 000000000..9e93164e5 --- /dev/null +++ b/selfdrive/ui/qt/onroad.h @@ -0,0 +1,92 @@ +#pragma once + +#include + +#include +#include + +#include "cereal/gen/cpp/log.capnp.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/qt_window.h" + +typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; + +// ***** onroad widgets ***** + +class OnroadAlerts : public QWidget { + Q_OBJECT + +public: + OnroadAlerts(QWidget *parent = 0); + +protected: + void paintEvent(QPaintEvent*) override; + +private: + void stopSounds(); + void playSound(AudibleAlert alert); + void updateAlert(const QString &t1, const QString &t2, float blink_rate, + const std::string &type, cereal::ControlsState::AlertSize size, AudibleAlert sound); + + std::map> sound_map { + // AudibleAlert, (file path, inf loop) + {AudibleAlert::CHIME_DISENGAGE, {"../assets/sounds/disengaged.wav", false}}, + {AudibleAlert::CHIME_ENGAGE, {"../assets/sounds/engaged.wav", false}}, + {AudibleAlert::CHIME_WARNING1, {"../assets/sounds/warning_1.wav", false}}, + {AudibleAlert::CHIME_WARNING2, {"../assets/sounds/warning_2.wav", false}}, + {AudibleAlert::CHIME_WARNING2_REPEAT, {"../assets/sounds/warning_2.wav", true}}, + {AudibleAlert::CHIME_WARNING_REPEAT, {"../assets/sounds/warning_repeat.wav", true}}, + {AudibleAlert::CHIME_ERROR, {"../assets/sounds/error.wav", false}}, + {AudibleAlert::CHIME_PROMPT, {"../assets/sounds/error.wav", false}} + }; + + QColor bg; + float volume = Hardware::MIN_VOLUME; + std::map sounds; + float blinking_rate = 0; + QString text1, text2; + std::string alert_type; + cereal::ControlsState::AlertSize alert_size; + +public slots: + void updateState(const UIState &s); + void offroadTransition(bool offroad); +}; + +// container window for the NVG UI +class NvgWindow : public QOpenGLWidget, protected QOpenGLFunctions { + Q_OBJECT + +public: + using QOpenGLWidget::QOpenGLWidget; + explicit NvgWindow(QWidget* parent = 0); + ~NvgWindow(); + +protected: + void paintGL() override; + void initializeGL() override; + +private: + double prev_draw_t = 0; + +public slots: + void update(const UIState &s); +}; + +// container for all onroad widgets +class OnroadWindow : public QWidget { + Q_OBJECT + +public: + OnroadWindow(QWidget* parent = 0); + +private: + OnroadAlerts *alerts; + NvgWindow *nvg; + QStackedLayout *layout; + +signals: + void update(const UIState &s); + void offroadTransition(bool offroad); +}; diff --git a/selfdrive/ui/qt/qt_window.hpp b/selfdrive/ui/qt/qt_window.h similarity index 85% rename from selfdrive/ui/qt/qt_window.hpp rename to selfdrive/ui/qt/qt_window.h index 2e02ec30c..295ab083f 100644 --- a/selfdrive/ui/qt/qt_window.hpp +++ b/selfdrive/ui/qt/qt_window.h @@ -1,20 +1,20 @@ +#pragma once + #include -#include #include +#include #ifdef QCOM2 #include -#include #include +#include #endif +#include "selfdrive/hardware/hw.h" -#ifdef QCOM2 - const int vwp_w = 2160, vwp_h = 1080; -#else - const int vwp_w = 1920, vwp_h = 1080; -#endif +const int vwp_w = Hardware::TICI() ? 2160 : 1920; +const int vwp_h = 1080; inline void setMainWindow(QWidget *w) { const float scale = getenv("SCALE") != NULL ? std::stof(getenv("SCALE")) : 1.0; diff --git a/selfdrive/ui/qt/request_repeater.cc b/selfdrive/ui/qt/request_repeater.cc new file mode 100644 index 000000000..ee6680cc7 --- /dev/null +++ b/selfdrive/ui/qt/request_repeater.cc @@ -0,0 +1,13 @@ +#include "request_repeater.h" + +RequestRepeater::RequestRepeater(QObject *parent, const QString &requestURL, const QString &cacheKey, + int period) : HttpRequest(parent, requestURL, cacheKey) { + timer = new QTimer(this); + timer->setTimerType(Qt::VeryCoarseTimer); + QObject::connect(timer, &QTimer::timeout, [=](){ + if (!QUIState::ui_state.scene.started && QUIState::ui_state.awake && reply == NULL) { + sendRequest(requestURL); + } + }); + timer->start(period * 1000); +} diff --git a/selfdrive/ui/qt/request_repeater.h b/selfdrive/ui/qt/request_repeater.h new file mode 100644 index 000000000..c23bbbcf1 --- /dev/null +++ b/selfdrive/ui/qt/request_repeater.h @@ -0,0 +1,12 @@ +#pragma once + +#include "selfdrive/ui/qt/api.h" +#include "selfdrive/ui/ui.h" + +class RequestRepeater : public HttpRequest { +public: + RequestRepeater(QObject *parent, const QString &requestURL, const QString &cacheKey = "", int period = 0); + +private: + QTimer *timer; +}; diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc new file mode 100644 index 000000000..cf1319a70 --- /dev/null +++ b/selfdrive/ui/qt/sidebar.cc @@ -0,0 +1,113 @@ +#include "selfdrive/ui/qt/sidebar.h" + +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/qt/util.h" + +void Sidebar::drawMetric(QPainter &p, const QString &label, const QString &val, QColor c, int y) { + const QRect rect = {30, y, 240, val.isEmpty() ? (label.contains("\n") ? 124 : 100) : 148}; + + p.setPen(Qt::NoPen); + p.setBrush(QBrush(c)); + p.setClipRect(rect.x() + 6, rect.y(), 18, rect.height(), Qt::ClipOperation::ReplaceClip); + p.drawRoundedRect(QRect(rect.x() + 6, rect.y() + 6, 100, rect.height() - 12), 10, 10); + p.setClipping(false); + + QPen pen = QPen(QColor(0xff, 0xff, 0xff, 0x55)); + pen.setWidth(2); + p.setPen(pen); + p.setBrush(Qt::NoBrush); + p.drawRoundedRect(rect, 20, 20); + + p.setPen(QColor(0xff, 0xff, 0xff)); + if (val.isEmpty()) { + configFont(p, "Open Sans", 35, "Bold"); + const QRect r = QRect(rect.x() + 35, rect.y(), rect.width() - 50, rect.height()); + p.drawText(r, Qt::AlignCenter, label); + } else { + configFont(p, "Open Sans", 58, "Bold"); + p.drawText(rect.x() + 50, rect.y() + 71, val); + configFont(p, "Open Sans", 35, "Regular"); + p.drawText(rect.x() + 50, rect.y() + 50 + 77, label); + } +} + +Sidebar::Sidebar(QWidget *parent) : QFrame(parent) { + home_img = QImage("../assets/images/button_home.png").scaled(180, 180, Qt::KeepAspectRatio, Qt::SmoothTransformation); + settings_img = QImage("../assets/images/button_settings.png").scaled(settings_btn.width(), settings_btn.height(), Qt::IgnoreAspectRatio, Qt::SmoothTransformation);; + + setFixedWidth(300); + setMinimumHeight(vwp_h); + setStyleSheet("background-color: rgb(57, 57, 57);"); +} + +void Sidebar::mousePressEvent(QMouseEvent *event) { + if (settings_btn.contains(event->pos())) { + emit openSettings(); + } +} + +void Sidebar::update(const UIState &s) { + if (s.sm->frame % (6*UI_FREQ) == 0) { + connect_str = "OFFLINE"; + connect_status = warning_color; + auto last_ping = params.get("LastAthenaPingTime"); + if (last_ping) { + bool online = nanos_since_boot() - *last_ping < 70e9; + connect_str = online ? "ONLINE" : "ERROR"; + connect_status = online ? good_color : danger_color; + } + repaint(); + } + + net_type = s.scene.deviceState.getNetworkType(); + strength = s.scene.deviceState.getNetworkStrength(); + + temp_status = danger_color; + auto ts = s.scene.deviceState.getThermalStatus(); + if (ts == cereal::DeviceState::ThermalStatus::GREEN) { + temp_status = good_color; + } else if (ts == cereal::DeviceState::ThermalStatus::YELLOW) { + temp_status = warning_color; + } + temp_val = (int)s.scene.deviceState.getAmbientTempC(); + + panda_str = "VEHICLE\nONLINE"; + panda_status = good_color; + if (s.scene.pandaType == cereal::PandaState::PandaType::UNKNOWN) { + panda_status = danger_color; + panda_str = "NO\nPANDA"; + } else if (Hardware::TICI() && s.scene.started) { + panda_str = QString("SAT CNT\n%1").arg(s.scene.satelliteCount); + panda_status = s.scene.gpsOK ? good_color : warning_color; + } + + if (s.sm->updated("deviceState") || s.sm->updated("pandaState")) { + repaint(); + } +} + +void Sidebar::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setPen(Qt::NoPen); + p.setRenderHint(QPainter::Antialiasing); + + // static imgs + p.setOpacity(0.65); + p.drawImage(settings_btn.x(), settings_btn.y(), settings_img); + p.setOpacity(1.0); + p.drawImage(60, 1080 - 180 - 40, home_img); + + // network + p.drawImage(58, 196, signal_imgs[strength]); + configFont(p, "Open Sans", 35, "Regular"); + p.setPen(QColor(0xff, 0xff, 0xff)); + const QRect r = QRect(50, 247, 100, 50); + p.drawText(r, Qt::AlignCenter, network_type[net_type]); + + // metrics + drawMetric(p, "TEMP", QString("%1°C").arg(temp_val), temp_status, 338); + drawMetric(p, panda_str, "", panda_status, 518); + drawMetric(p, "CONNECT\n" + connect_str, "", connect_status, 676); +} diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h new file mode 100644 index 000000000..e108a6a5f --- /dev/null +++ b/selfdrive/ui/qt/sidebar.h @@ -0,0 +1,57 @@ +#pragma once + +#include + +#include "selfdrive/ui/ui.h" + +class Sidebar : public QFrame { + Q_OBJECT + +public: + explicit Sidebar(QWidget* parent = 0); + +signals: + void openSettings(); + +public slots: + void update(const UIState &s); + +protected: + void paintEvent(QPaintEvent *event) override; + void mousePressEvent(QMouseEvent *event) override; + +private: + void drawMetric(QPainter &p, const QString &label, const QString &val, QColor c, int y); + + QImage home_img, settings_img; + const QMap network_type = { + {cereal::DeviceState::NetworkType::NONE, "--"}, + {cereal::DeviceState::NetworkType::WIFI, "WiFi"}, + {cereal::DeviceState::NetworkType::CELL2_G, "2G"}, + {cereal::DeviceState::NetworkType::CELL3_G, "3G"}, + {cereal::DeviceState::NetworkType::CELL4_G, "4G"}, + {cereal::DeviceState::NetworkType::CELL5_G, "5G"} + }; + const QMap signal_imgs = { + {cereal::DeviceState::NetworkStrength::UNKNOWN, QImage("../assets/images/network_0.png")}, + {cereal::DeviceState::NetworkStrength::POOR, QImage("../assets/images/network_1.png")}, + {cereal::DeviceState::NetworkStrength::MODERATE, QImage("../assets/images/network_2.png")}, + {cereal::DeviceState::NetworkStrength::GOOD, QImage("../assets/images/network_3.png")}, + {cereal::DeviceState::NetworkStrength::GREAT, QImage("../assets/images/network_4.png")}, + }; + + const QRect settings_btn = QRect(50, 35, 200, 117); + const QColor good_color = QColor(255, 255, 255); + const QColor warning_color = QColor(218, 202, 37); + const QColor danger_color = QColor(201, 34, 49); + + Params params; + QString connect_str = "OFFLINE"; + QColor connect_status = warning_color; + QString panda_str = "NO\nPANDA"; + QColor panda_status = warning_color; + int temp_val = 0; + QColor temp_status = warning_color; + cereal::DeviceState::NetworkType net_type; + cereal::DeviceState::NetworkStrength strength; +}; diff --git a/selfdrive/ui/qt/sound.cc b/selfdrive/ui/qt/sound.cc deleted file mode 100644 index 641ffb22a..000000000 --- a/selfdrive/ui/qt/sound.cc +++ /dev/null @@ -1,25 +0,0 @@ -#include -#include "sound.hpp" - -Sound::Sound() { - for (auto &kv : sound_map) { - auto path = QUrl::fromLocalFile(kv.second.first); - sounds[kv.first].setSource(path); - } -} - -void Sound::play(AudibleAlert alert) { - int loops = sound_map[alert].second> - 1 ? sound_map[alert].second : QSoundEffect::Infinite; - sounds[alert].setLoopCount(loops); - sounds[alert].setVolume(volume); - sounds[alert].play(); -} - -void Sound::stop() { - for (auto &kv : sounds) { - // Only stop repeating sounds - if (sound_map[kv.first].second != 0) { - kv.second.stop(); - } - } -} diff --git a/selfdrive/ui/qt/sound.hpp b/selfdrive/ui/qt/sound.hpp deleted file mode 100644 index cee1ea51a..000000000 --- a/selfdrive/ui/qt/sound.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include -#include -#include "cereal/gen/cpp/log.capnp.h" - -typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; - -static std::map> sound_map { - // AudibleAlert, (file path, loop count) - {AudibleAlert::CHIME_DISENGAGE, {"../assets/sounds/disengaged.wav", 0}}, - {AudibleAlert::CHIME_ENGAGE, {"../assets/sounds/engaged.wav", 0}}, - {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", -1}}, - {AudibleAlert::CHIME_ERROR, {"../assets/sounds/error.wav", 0}}, - {AudibleAlert::CHIME_PROMPT, {"../assets/sounds/error.wav", 0}} -}; - -class Sound { -public: - Sound(); - void play(AudibleAlert alert); - void stop(); - float volume = 0; - -private: - std::map sounds; -}; diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc index c3bf5a3f3..b91bd7bc4 100644 --- a/selfdrive/ui/qt/spinner.cc +++ b/selfdrive/ui/qt/spinner.cc @@ -1,39 +1,63 @@ -#include -#include -#include +#include "spinner.h" +#include +#include +#include + +#include +#include +#include #include #include -#include -#include -#include "spinner.hpp" -#include "qt_window.hpp" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/qt/qt_window.h" + +TrackWidget::TrackWidget(QWidget *parent) : QWidget(parent) { + setFixedSize(spinner_size); + setAutoFillBackground(false); + + comma_img = QPixmap("../assets/img_spinner_comma.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation); + + // pre-compute all the track imgs. make this a gif instead? + QTransform transform; + QPixmap track_img = QPixmap("../assets/img_spinner_track.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation); + for (auto &img : track_imgs) { + img = track_img.transformed(transform.rotate(360/spinner_fps), Qt::SmoothTransformation); + } + + m_anim.setDuration(1000); + m_anim.setStartValue(0); + m_anim.setEndValue(int(track_imgs.size() -1)); + m_anim.setLoopCount(-1); + m_anim.start(); + connect(&m_anim, SIGNAL(valueChanged(QVariant)), SLOT(update())); +} + +void TrackWidget::paintEvent(QPaintEvent *event) { + QPainter painter(this); + QRect bg(0, 0, painter.device()->width(), painter.device()->height()); + QBrush bgBrush("#000000"); + painter.fillRect(bg, bgBrush); + + int track_idx = m_anim.currentValue().toInt(); + QRect rect(track_imgs[track_idx].rect()); + rect.moveCenter(bg.center()); + painter.drawPixmap(rect.topLeft(), track_imgs[track_idx]); + + rect = comma_img.rect(); + rect.moveCenter(bg.center()); + painter.drawPixmap(rect.topLeft(), comma_img); +} + +// Spinner Spinner::Spinner(QWidget *parent) { QGridLayout *main_layout = new QGridLayout(); main_layout->setSpacing(0); - main_layout->setContentsMargins(200, 200, 200, 200); + main_layout->setMargin(200); - comma = new QLabel(); - comma->setPixmap(QPixmap("../assets/img_spinner_comma.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation)); - comma->setFixedSize(spinner_size); - main_layout->addWidget(comma, 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); - - track = new QLabel(); - track->setFixedSize(spinner_size); - main_layout->addWidget(track, 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); - - // pre-compute all the track imgs. make this a gif instead? - track_idx = 0; - QTransform transform; - QPixmap track_img = QPixmap("../assets/img_spinner_track.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation); - for (auto &img : track_imgs) { - QPixmap r = track_img.transformed(transform.rotate(360/spinner_fps), Qt::SmoothTransformation); - int x = (r.width() - track->width()) / 2; - int y = (r.height() - track->height()) / 2; - img = r.copy(x, y, track->width(), track->height()); - } + main_layout->addWidget(new TrackWidget(this), 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); text = new QLabel(); text->setVisible(false); @@ -51,6 +75,9 @@ Spinner::Spinner(QWidget *parent) { Spinner { background-color: black; } + * { + background-color: transparent; + } QLabel { color: white; font-size: 80px; @@ -67,17 +94,8 @@ Spinner::Spinner(QWidget *parent) { } )"); - rotate_timer = new QTimer(this); - rotate_timer->start(1000./spinner_fps); - QObject::connect(rotate_timer, SIGNAL(timeout()), this, SLOT(rotate())); - notifier = new QSocketNotifier(fileno(stdin), QSocketNotifier::Read); - QObject::connect(notifier, SIGNAL(activated(int)), this, SLOT(update(int))); -}; - -void Spinner::rotate() { - track_idx = (track_idx+1) % track_imgs.size(); - track->setPixmap(track_imgs[track_idx]); + QObject::connect(notifier, &QSocketNotifier::activated, this, &Spinner::update); }; void Spinner::update(int n) { @@ -96,6 +114,19 @@ void Spinner::update(int n) { } int main(int argc, char *argv[]) { + QSurfaceFormat fmt; +#ifdef __APPLE__ + fmt.setVersion(3, 2); + fmt.setProfile(QSurfaceFormat::OpenGLContextProfile::CoreProfile); + fmt.setRenderableType(QSurfaceFormat::OpenGL); +#else + fmt.setRenderableType(QSurfaceFormat::OpenGLES); +#endif + QSurfaceFormat::setDefaultFormat(fmt); + + Hardware::set_display_power(true); + Hardware::set_brightness(65); + QApplication a(argc, argv); Spinner spinner; setMainWindow(&spinner); diff --git a/selfdrive/ui/qt/spinner.hpp b/selfdrive/ui/qt/spinner.h similarity index 64% rename from selfdrive/ui/qt/spinner.hpp rename to selfdrive/ui/qt/spinner.h index 8bdd13a3d..c54aea103 100644 --- a/selfdrive/ui/qt/spinner.hpp +++ b/selfdrive/ui/qt/spinner.h @@ -1,15 +1,28 @@ #include -#include #include -#include +#include #include #include #include +#include +#include constexpr int spinner_fps = 30; constexpr QSize spinner_size = QSize(360, 360); +class TrackWidget : public QWidget { + Q_OBJECT +public: + TrackWidget(QWidget *parent = nullptr); + +private: + void paintEvent(QPaintEvent *event) override; + std::array track_imgs; + QPixmap comma_img; + QVariantAnimation m_anim; +}; + class Spinner : public QWidget { Q_OBJECT @@ -17,16 +30,10 @@ public: explicit Spinner(QWidget *parent = 0); private: - int track_idx; - QLabel *comma, *track; QLabel *text; QProgressBar *progress_bar; - std::array track_imgs; - - QTimer *rotate_timer; QSocketNotifier *notifier; public slots: - void rotate(); void update(int n); }; diff --git a/selfdrive/ui/qt/spinner_aarch64 b/selfdrive/ui/qt/spinner_aarch64 index be45fb51c..a1fa69566 100755 Binary files a/selfdrive/ui/qt/spinner_aarch64 and b/selfdrive/ui/qt/spinner_aarch64 differ diff --git a/selfdrive/ui/qt/text.cc b/selfdrive/ui/qt/text.cc index d44570d99..0708b30db 100644 --- a/selfdrive/ui/qt/text.cc +++ b/selfdrive/ui/qt/text.cc @@ -1,22 +1,36 @@ -#include -#include -#include -#include #include +#include +#include +#include +#include +#include -#include "qt_window.hpp" #include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" int main(int argc, char *argv[]) { QApplication a(argc, argv); QWidget window; setMainWindow(&window); - QVBoxLayout *layout = new QVBoxLayout(); - layout->setContentsMargins(125, 125, 125, 125); + Hardware::set_display_power(true); + Hardware::set_brightness(65); - // TODO: make this scroll - layout->addWidget(new QLabel(argv[1]), 0, Qt::AlignTop); + QGridLayout *layout = new QGridLayout; + layout->setMargin(50); + + QLabel *label = new QLabel(argv[1]); + label->setWordWrap(true); + label->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::MinimumExpanding); + ScrollView *scroll = new ScrollView(label); + scroll->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + layout->addWidget(scroll, 0, 0, Qt::AlignTop); + + // Scroll to the bottom + QObject::connect(scroll->verticalScrollBar(), &QAbstractSlider::rangeChanged, [=](){ + scroll->verticalScrollBar()->setValue(scroll->verticalScrollBar()->maximum()); + }); QPushButton *btn = new QPushButton(); #ifdef __aarch64__ @@ -26,9 +40,9 @@ int main(int argc, char *argv[]) { }); #else btn->setText("Exit"); - QObject::connect(btn, SIGNAL(released()), &a, SLOT(quit())); + QObject::connect(btn, &QPushButton::released, &a, &QApplication::quit); #endif - layout->addWidget(btn, 0, Qt::AlignRight); + layout->addWidget(btn, 0, 0, Qt::AlignRight | Qt::AlignBottom); window.setLayout(layout); window.setStyleSheet(R"( @@ -44,6 +58,7 @@ int main(int argc, char *argv[]) { padding-left: 100px; border: 2px solid white; border-radius: 20px; + margin-right: 40px; } )"); diff --git a/selfdrive/ui/qt/text_aarch64 b/selfdrive/ui/qt/text_aarch64 index 67ce52e13..f4f770afd 100755 Binary files a/selfdrive/ui/qt/text_aarch64 and b/selfdrive/ui/qt/text_aarch64 differ diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h new file mode 100644 index 000000000..0ba42d3d8 --- /dev/null +++ b/selfdrive/ui/qt/util.h @@ -0,0 +1,10 @@ +#pragma once + +#include + +inline void configFont(QPainter &p, QString family, int size, const QString &style) { + QFont f(family); + f.setPixelSize(size); + f.setStyleName(style); + p.setFont(f); +} diff --git a/selfdrive/ui/qt/widgets/controls.cc b/selfdrive/ui/qt/widgets/controls.cc index d9c170e27..24c586113 100644 --- a/selfdrive/ui/qt/widgets/controls.cc +++ b/selfdrive/ui/qt/widgets/controls.cc @@ -1,4 +1,4 @@ -#include "controls.hpp" +#include "controls.h" QFrame *horizontal_line(QWidget *parent) { QFrame *line = new QFrame(parent); @@ -48,6 +48,9 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons vlayout->addWidget(description); connect(title_label, &QPushButton::clicked, [=]() { + if (!description->isVisible()) { + emit showDescription(); + } description->setVisible(!description->isVisible()); }); } @@ -55,3 +58,9 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons setLayout(vlayout); setStyleSheet("background-color: transparent;"); } + +void AbstractControl::hideEvent(QHideEvent *e){ + if(description != nullptr){ + description->hide(); + } +} diff --git a/selfdrive/ui/qt/widgets/controls.hpp b/selfdrive/ui/qt/widgets/controls.h similarity index 84% rename from selfdrive/ui/qt/widgets/controls.hpp rename to selfdrive/ui/qt/widgets/controls.h index 3f048c4bb..22e36ea4f 100644 --- a/selfdrive/ui/qt/widgets/controls.hpp +++ b/selfdrive/ui/qt/widgets/controls.h @@ -6,16 +6,25 @@ #include #include -#include "common/params.h" -#include "toggle.hpp" +#include "selfdrive/common/params.h" +#include "selfdrive/ui/qt/widgets/toggle.h" QFrame *horizontal_line(QWidget *parent = nullptr); class AbstractControl : public QFrame { Q_OBJECT +public: + void setDescription(const QString &desc) { + if(description) description->setText(desc); + } + +signals: + void showDescription(); + protected: AbstractControl(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr); + void hideEvent(QHideEvent *e); QSize minimumSizeHint() const override { QSize size = QFrame::minimumSizeHint(); @@ -107,14 +116,15 @@ class ParamControl : public ToggleControl { Q_OBJECT public: - ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr) : ToggleControl(title, desc, icon, parent) { - // set initial state from param - if (Params().read_db_bool(param.toStdString().c_str())) { + ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr) : ToggleControl(title, desc, icon, false, parent) { + if (params.getBool(param.toStdString().c_str())) { toggle.togglePosition(); } - QObject::connect(this, &ToggleControl::toggleFlipped, [=](int state) { - char value = state ? '1' : '0'; - Params().write_db_value(param.toStdString().c_str(), &value, 1); + QObject::connect(this, &ToggleControl::toggleFlipped, [=](bool state) { + params.putBool(param.toStdString().c_str(), state); }); } + +private: + Params params; }; diff --git a/selfdrive/ui/qt/widgets/drive_stats.cc b/selfdrive/ui/qt/widgets/drive_stats.cc index 7cbcc0aab..1395c150c 100644 --- a/selfdrive/ui/qt/widgets/drive_stats.cc +++ b/selfdrive/ui/qt/widgets/drive_stats.cc @@ -1,11 +1,12 @@ +#include "drive_stats.h" + #include #include #include #include -#include "api.hpp" -#include "common/params.h" -#include "drive_stats.hpp" +#include "selfdrive/common/params.h" +#include "selfdrive/ui/qt/request_repeater.h" const double MILE_TO_KM = 1.60934; @@ -22,9 +23,8 @@ static QLayout* build_stat_layout(QLabel** metric, const QString& name) { return layout; } -void DriveStats::parseResponse(QString response) { - response = response.trimmed(); - QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); +void DriveStats::parseResponse(const QString& response) { + QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8()); if (doc.isNull()) { qDebug() << "JSON Parse failed on getting past drives statistics"; return; @@ -36,35 +36,35 @@ void DriveStats::parseResponse(QString response) { labels.hours->setText(QString::number((int)(obj["minutes"].toDouble() / 60))); }; - bool metric = Params().read_db_bool("IsMetric"); QJsonObject json = doc.object(); update(json["all"].toObject(), all_, metric); update(json["week"].toObject(), week_, metric); } DriveStats::DriveStats(QWidget* parent) : QWidget(parent) { - setStyleSheet("QLabel {font-size: 48px; font-weight: 500;}"); + metric = Params().getBool("IsMetric"); + QString distance_unit = metric ? "KM" : "MILES"; - auto add_stats_layouts = [&](QGridLayout* gl, StatsLabels& labels, int row, const char* distance_unit) { + auto add_stats_layouts = [&](QGridLayout* gl, StatsLabels& labels, int row, const QString &distance_unit) { gl->addLayout(build_stat_layout(&labels.routes, "DRIVES"), row, 0, 3, 1); gl->addLayout(build_stat_layout(&labels.distance, distance_unit), row, 1, 3, 1); gl->addLayout(build_stat_layout(&labels.hours, "HOURS"), row, 2, 3, 1); }; - const char* distance_unit = Params().read_db_bool("IsMetric") ? "KM" : "MILES"; - QGridLayout* gl = new QGridLayout(); + QGridLayout* gl = new QGridLayout(this); gl->setMargin(0); + gl->addWidget(new QLabel("ALL TIME"), 0, 0, 1, 3); add_stats_layouts(gl, all_, 1, distance_unit); + gl->addWidget(new QLabel("PAST WEEK"), 6, 0, 1, 3); add_stats_layouts(gl, week_, 7, distance_unit); - QVBoxLayout* vlayout = new QVBoxLayout(this); - vlayout->addLayout(gl); - - // TODO: do we really need to update this frequently? QString dongleId = QString::fromStdString(Params().get("DongleId")); QString url = "https://api.commadotai.com/v1.1/devices/" + dongleId + "/stats"; - RequestRepeater* repeater = new RequestRepeater(this, url, 13, "ApiCache_DriveStats"); - QObject::connect(repeater, SIGNAL(receivedResponse(QString)), this, SLOT(parseResponse(QString))); + RequestRepeater *repeater = new RequestRepeater(this, url, "ApiCache_DriveStats", 30); + QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &DriveStats::parseResponse); + + setLayout(gl); + setStyleSheet(R"(QLabel {font-size: 48px; font-weight: 500;})"); } diff --git a/selfdrive/ui/qt/widgets/drive_stats.hpp b/selfdrive/ui/qt/widgets/drive_stats.h similarity index 79% rename from selfdrive/ui/qt/widgets/drive_stats.hpp rename to selfdrive/ui/qt/widgets/drive_stats.h index 64b93cfe7..deeb80f65 100644 --- a/selfdrive/ui/qt/widgets/drive_stats.hpp +++ b/selfdrive/ui/qt/widgets/drive_stats.h @@ -9,10 +9,11 @@ public: explicit DriveStats(QWidget* parent = 0); private: + bool metric; struct StatsLabels { QLabel *routes, *distance, *hours; } all_, week_; private slots: - void parseResponse(QString response); + void parseResponse(const QString &response); }; diff --git a/selfdrive/ui/qt/widgets/input.cc b/selfdrive/ui/qt/widgets/input.cc index cc0dada7d..4e9d2e668 100644 --- a/selfdrive/ui/qt/widgets/input.cc +++ b/selfdrive/ui/qt/widgets/input.cc @@ -1,7 +1,9 @@ +#include "input.h" + #include -#include "input.hpp" -#include "qt_window.hpp" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/hardware/hw.h" InputDialog::InputDialog(const QString &prompt_text, QWidget *parent) : QDialog(parent) { layout = new QVBoxLayout(); @@ -25,8 +27,8 @@ InputDialog::InputDialog(const QString &prompt_text, QWidget *parent) : QDialog( background-color: #444444; )"); header_layout->addWidget(cancel_btn, 0, Qt::AlignRight); - QObject::connect(cancel_btn, SIGNAL(released()), this, SLOT(reject())); - QObject::connect(cancel_btn, SIGNAL(released()), this, SIGNAL(cancel())); + QObject::connect(cancel_btn, &QPushButton::released, this, &InputDialog::reject); + QObject::connect(cancel_btn, &QPushButton::released, this, &InputDialog::cancel); layout->addLayout(header_layout); @@ -43,7 +45,7 @@ InputDialog::InputDialog(const QString &prompt_text, QWidget *parent) : QDialog( layout->addWidget(line, 1, Qt::AlignTop); k = new Keyboard(this); - QObject::connect(k, SIGNAL(emitButton(const QString&)), this, SLOT(handleInput(const QString&))); + QObject::connect(k, &Keyboard::emitButton, this, &InputDialog::handleInput); layout->addWidget(k, 2, Qt::AlignBottom); setStyleSheet(R"( @@ -111,7 +113,6 @@ void InputDialog::setMinLength(int length){ minLength = length; } - ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString &confirm_text, const QString &cancel_text, QWidget *parent):QDialog(parent) { setWindowFlags(Qt::Popup); @@ -133,13 +134,13 @@ ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString if (cancel_text.length()) { QPushButton* cancel_btn = new QPushButton(cancel_text); btn_layout->addWidget(cancel_btn, 0, Qt::AlignRight); - QObject::connect(cancel_btn, SIGNAL(released()), this, SLOT(reject())); + QObject::connect(cancel_btn, &QPushButton::released, this, &ConfirmationDialog::reject); } if (confirm_text.length()) { QPushButton* confirm_btn = new QPushButton(confirm_text); btn_layout->addWidget(confirm_btn, 0, Qt::AlignRight); - QObject::connect(confirm_btn, SIGNAL(released()), this, SLOT(accept())); + QObject::connect(confirm_btn, &QPushButton::released, this, &ConfirmationDialog::accept); } setFixedSize(900, 350); @@ -161,20 +162,20 @@ ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString setLayout(layout); } -bool ConfirmationDialog::alert(const QString &prompt_text) { - ConfirmationDialog d = ConfirmationDialog(prompt_text, "Ok", ""); +bool ConfirmationDialog::alert(const QString &prompt_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, "Ok", "", parent); return d.exec(); } -bool ConfirmationDialog::confirm(const QString &prompt_text) { - ConfirmationDialog d = ConfirmationDialog(prompt_text); +bool ConfirmationDialog::confirm(const QString &prompt_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, "Ok", "Cancel", parent); return d.exec(); } int ConfirmationDialog::exec() { // TODO: make this work without fullscreen -#ifdef QCOM2 - setMainWindow(this); -#endif + if (Hardware::TICI()) { + setMainWindow(this); + } return QDialog::exec(); } diff --git a/selfdrive/ui/qt/widgets/input.hpp b/selfdrive/ui/qt/widgets/input.h similarity index 85% rename from selfdrive/ui/qt/widgets/input.hpp rename to selfdrive/ui/qt/widgets/input.h index f52ad4416..4ffd0a9bb 100644 --- a/selfdrive/ui/qt/widgets/input.hpp +++ b/selfdrive/ui/qt/widgets/input.h @@ -1,13 +1,13 @@ #pragma once -#include -#include -#include #include +#include #include +#include #include +#include -#include "keyboard.hpp" +#include "selfdrive/ui/qt/widgets/keyboard.h" class InputDialog : public QDialog { Q_OBJECT @@ -44,8 +44,8 @@ class ConfirmationDialog : public QDialog { public: explicit ConfirmationDialog(const QString &prompt_text, const QString &confirm_text = "Ok", const QString &cancel_text = "Cancel", QWidget* parent = 0); - static bool alert(const QString &prompt_text); - static bool confirm(const QString &prompt_text); + static bool alert(const QString &prompt_text, QWidget *parent = 0); + static bool confirm(const QString &prompt_text, QWidget *parent = 0); private: QLabel *prompt; diff --git a/selfdrive/ui/qt/widgets/keyboard.cc b/selfdrive/ui/qt/widgets/keyboard.cc index 0eda100ed..6613897b1 100644 --- a/selfdrive/ui/qt/widgets/keyboard.cc +++ b/selfdrive/ui/qt/widgets/keyboard.cc @@ -1,11 +1,11 @@ +#include "keyboard.h" + +#include #include -#include #include #include -#include #include - -#include "keyboard.hpp" +#include const int DEFAULT_STRETCH = 1; const int SPACEBAR_STRETCH = 3; diff --git a/selfdrive/ui/qt/widgets/keyboard.hpp b/selfdrive/ui/qt/widgets/keyboard.h similarity index 99% rename from selfdrive/ui/qt/widgets/keyboard.hpp rename to selfdrive/ui/qt/widgets/keyboard.h index 5ff50382e..c89f4fc6a 100644 --- a/selfdrive/ui/qt/widgets/keyboard.hpp +++ b/selfdrive/ui/qt/widgets/keyboard.h @@ -2,12 +2,11 @@ #include +#include #include +#include #include #include -#include -#include - class KeyboardLayout : public QWidget { Q_OBJECT diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc index cd1173722..7816b251f 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.cc +++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc @@ -1,47 +1,53 @@ -#include -#include -#include -#include -#include -#include -#include +#include "offroad_alerts.h" -#include "offroad_alerts.hpp" -#include "common/params.h" +#include +#include +#include +#include + +#include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" -void cleanStackedWidget(QStackedWidget* swidget) { - while(swidget->count() > 0) { - QWidget *w = swidget->widget(0); - swidget->removeWidget(w); - w->deleteLater(); - } -} - OffroadAlert::OffroadAlert(QWidget* parent) : QFrame(parent) { - QVBoxLayout *main_layout = new QVBoxLayout(); - main_layout->setMargin(25); + QVBoxLayout *layout = new QVBoxLayout; + layout->setMargin(50); + layout->setSpacing(30); - alerts_stack = new QStackedWidget(); - main_layout->addWidget(alerts_stack, 1); + QWidget *alerts_widget = new QWidget(this); + alerts_layout = new QVBoxLayout; + alerts_layout->setMargin(0); + alerts_layout->setSpacing(30); + alerts_widget->setLayout(alerts_layout); + alerts_widget->setStyleSheet("background-color: transparent;"); - // bottom footer + // release notes + releaseNotes.setWordWrap(true); + releaseNotes.setVisible(false); + releaseNotes.setStyleSheet("font-size: 48px;"); + releaseNotes.setAlignment(Qt::AlignTop); + + releaseNotesScroll = new ScrollView(&releaseNotes, this); + layout->addWidget(releaseNotesScroll); + + alertsScroll = new ScrollView(alerts_widget, this); + layout->addWidget(alertsScroll); + + // bottom footer, dismiss + reboot buttons QHBoxLayout *footer_layout = new QHBoxLayout(); - main_layout->addLayout(footer_layout); + layout->addLayout(footer_layout); QPushButton *dismiss_btn = new QPushButton("Dismiss"); dismiss_btn->setFixedSize(400, 125); - footer_layout->addWidget(dismiss_btn, 0, Qt::AlignLeft); + footer_layout->addWidget(dismiss_btn, 0, Qt::AlignBottom | Qt::AlignLeft); + QObject::connect(dismiss_btn, &QPushButton::released, this, &OffroadAlert::closeAlerts); - reboot_btn = new QPushButton("Reboot and Update"); - reboot_btn->setFixedSize(600, 125); - reboot_btn->setVisible(false); - footer_layout->addWidget(reboot_btn, 0, Qt::AlignRight); + rebootBtn.setText("Reboot and Update"); + rebootBtn.setFixedSize(600, 125); + rebootBtn.setVisible(false); + footer_layout->addWidget(&rebootBtn, 0, Qt::AlignBottom | Qt::AlignRight); + QObject::connect(&rebootBtn, &QPushButton::released, [=]() { Hardware::reboot(); }); - QObject::connect(dismiss_btn, SIGNAL(released()), this, SIGNAL(closeAlerts())); - QObject::connect(reboot_btn, &QPushButton::released, [=]() { Hardware::reboot(); }); - - setLayout(main_layout); + setLayout(layout); setStyleSheet(R"( * { font-size: 48px; @@ -58,54 +64,51 @@ OffroadAlert::OffroadAlert(QWidget* parent) : QFrame(parent) { background-color: white; } )"); - main_layout->setMargin(50); - - QFile inFile("../controls/lib/alerts_offroad.json"); - bool ret = inFile.open(QIODevice::ReadOnly | QIODevice::Text); - assert(ret); - QJsonDocument doc = QJsonDocument::fromJson(inFile.readAll()); - assert(!doc.isNull()); - alert_keys = doc.object().keys(); } void OffroadAlert::refresh() { - parse_alerts(); - cleanStackedWidget(alerts_stack); + if (alerts.empty()) { + // setup labels for each alert + QString json = QString::fromStdString(util::read_file("../controls/lib/alerts_offroad.json")); + QJsonObject obj = QJsonDocument::fromJson(json.toUtf8()).object(); + for (auto &k : obj.keys()) { + QLabel *l = new QLabel(this); + alerts[k.toStdString()] = l; + int severity = obj[k].toObject()["severity"].toInt(); - updateAvailable = Params().read_db_bool("UpdateAvailable"); - reboot_btn->setVisible(updateAvailable); - - QVBoxLayout *layout = new QVBoxLayout; - layout->setSpacing(20); - - if (updateAvailable) { - QLabel *body = new QLabel(QString::fromStdString(Params().get("ReleaseNotes"))); - body->setStyleSheet(R"(font-size: 48px;)"); - layout->addWidget(body, 0, Qt::AlignLeft | Qt::AlignTop); - } else { - for (const auto &alert : alerts) { - QLabel *l = new QLabel(alert.text); l->setMargin(60); l->setWordWrap(true); - l->setStyleSheet("background-color: " + QString(alert.severity ? "#E22C2C" : "#292929")); - layout->addWidget(l, 0, Qt::AlignTop); + l->setStyleSheet("background-color: " + QString(severity ? "#E22C2C" : "#292929")); + l->setVisible(false); + alerts_layout->addWidget(l); } + alerts_layout->addStretch(1); } - QWidget *w = new QWidget(); - w->setLayout(layout); - alerts_stack->addWidget(w); + updateAlerts(); + + rebootBtn.setVisible(updateAvailable); + releaseNotesScroll->setVisible(updateAvailable); + releaseNotes.setText(QString::fromStdString(params.get("ReleaseNotes"))); + + alertsScroll->setVisible(!updateAvailable); + for (const auto& [k, label] : alerts) { + label->setVisible(!label->text().isEmpty()); + } } -void OffroadAlert::parse_alerts() { - alerts.clear(); - for (const QString &key : alert_keys) { - std::vector bytes = Params().read_db_bytes(key.toStdString().c_str()); +void OffroadAlert::updateAlerts() { + alertCount = 0; + updateAvailable = params.getBool("UpdateAvailable"); + for (const auto& [key, label] : alerts) { + auto bytes = params.get(key.c_str()); if (bytes.size()) { QJsonDocument doc_par = QJsonDocument::fromJson(QByteArray(bytes.data(), bytes.size())); QJsonObject obj = doc_par.object(); - Alert alert = {obj.value("text").toString(), obj.value("severity").toInt()}; - alerts.push_back(alert); + label->setText(obj.value("text").toString()); + alertCount++; + } else { + label->setText(""); } } } diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.h b/selfdrive/ui/qt/widgets/offroad_alerts.h new file mode 100644 index 000000000..88a69c8da --- /dev/null +++ b/selfdrive/ui/qt/widgets/offroad_alerts.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include +#include +#include +#include + +#include "selfdrive/common/params.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" + +class OffroadAlert : public QFrame { + Q_OBJECT + +public: + explicit OffroadAlert(QWidget *parent = 0); + int alertCount = 0; + bool updateAvailable; + +private: + void updateAlerts(); + + Params params; + std::map alerts; + + QLabel releaseNotes; + QPushButton rebootBtn; + ScrollView *alertsScroll; + ScrollView *releaseNotesScroll; + QVBoxLayout *alerts_layout; + +signals: + void closeAlerts(); + +public slots: + void refresh(); +}; diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.hpp b/selfdrive/ui/qt/widgets/offroad_alerts.hpp deleted file mode 100644 index ec48f89c7..000000000 --- a/selfdrive/ui/qt/widgets/offroad_alerts.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -struct Alert { - QString text; - int severity; -}; - -class OffroadAlert : public QFrame { - Q_OBJECT - -public: - explicit OffroadAlert(QWidget *parent = 0); - QVector alerts; - QStringList alert_keys; - bool updateAvailable; - -private: - QStackedWidget *alerts_stack; - QPushButton *reboot_btn; - void parse_alerts(); - -signals: - void closeAlerts(); - -public slots: - void refresh(); -}; diff --git a/selfdrive/ui/qt/widgets/scrollview.cc b/selfdrive/ui/qt/widgets/scrollview.cc new file mode 100644 index 000000000..31be17c7b --- /dev/null +++ b/selfdrive/ui/qt/widgets/scrollview.cc @@ -0,0 +1,47 @@ +#include "scrollview.h" + +#include + +ScrollView::ScrollView(QWidget *w, QWidget *parent) : QScrollArea(parent){ + setWidget(w); + setWidgetResizable(true); + setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + setStyleSheet("ScrollView { background-color:transparent; }"); + + QString style = R"( + QScrollBar:vertical { + border: none; + background: transparent; + width:10px; + margin: 0; + } + QScrollBar::handle:vertical { + min-height: 0px; + border-radius: 4px; + background-color: white; + } + QScrollBar::add-line:vertical, QScrollBar::sub-line:vertical { + height: 0px; + } + QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical { + background: none; + } + )"; + + verticalScrollBar()->setStyleSheet(style); + horizontalScrollBar()->setStyleSheet(style); + + QScroller *scroller = QScroller::scroller(this->viewport()); + QScrollerProperties sp = scroller->scrollerProperties(); + + sp.setScrollMetric(QScrollerProperties::VerticalOvershootPolicy, QVariant::fromValue(QScrollerProperties::OvershootAlwaysOff)); + sp.setScrollMetric(QScrollerProperties::HorizontalOvershootPolicy, QVariant::fromValue(QScrollerProperties::OvershootAlwaysOff)); + + scroller->grabGesture(this->viewport(), QScroller::LeftMouseButtonGesture); + scroller->setScrollerProperties(sp); +} + +void ScrollView::hideEvent(QHideEvent *e){ + verticalScrollBar()->setValue(0); +} diff --git a/selfdrive/ui/qt/widgets/scrollview.h b/selfdrive/ui/qt/widgets/scrollview.h new file mode 100644 index 000000000..aaec224e2 --- /dev/null +++ b/selfdrive/ui/qt/widgets/scrollview.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +class ScrollView : public QScrollArea { + Q_OBJECT + +public: + explicit ScrollView(QWidget *w = nullptr, QWidget *parent = nullptr); +protected: + void hideEvent(QHideEvent *e); +}; diff --git a/selfdrive/ui/qt/widgets/setup.cc b/selfdrive/ui/qt/widgets/setup.cc index 32e14f20d..2633220ea 100644 --- a/selfdrive/ui/qt/widgets/setup.cc +++ b/selfdrive/ui/qt/widgets/setup.cc @@ -1,3 +1,5 @@ +#include "setup.h" + #include #include #include @@ -6,11 +8,10 @@ #include #include #include +#include -#include "QrCode.hpp" -#include "api.hpp" -#include "common/params.h" -#include "setup.hpp" +#include "selfdrive/common/params.h" +#include "selfdrive/ui/qt/request_repeater.h" using qrcodegen::QrCode; @@ -23,13 +24,17 @@ PairingQRWidget::PairingQRWidget(QWidget* parent) : QWidget(parent) { QTimer* timer = new QTimer(this); timer->start(30 * 1000); - connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); - refresh(); // don't wait for the first refresh + connect(timer, &QTimer::timeout, this, &PairingQRWidget::refresh); +} + +void PairingQRWidget::showEvent(QShowEvent *event){ + refresh(); } void PairingQRWidget::refresh(){ - QString IMEI = QString::fromStdString(Params().get("IMEI")); - QString serial = QString::fromStdString(Params().get("HardwareSerial")); + Params params; + QString IMEI = QString::fromStdString(params.get("IMEI")); + QString serial = QString::fromStdString(params.get("HardwareSerial")); if (std::min(IMEI.length(), serial.length()) <= 5) { qrCode->setText("Error getting serial: contact support"); @@ -37,9 +42,7 @@ void PairingQRWidget::refresh(){ qrCode->setStyleSheet(R"(font-size: 60px;)"); return; } - QVector> payloads; - payloads.push_back(qMakePair(QString("pair"), true)); - QString pairToken = CommaApi::create_jwt(payloads); + QString pairToken = CommaApi::create_jwt({{"pair", true}}); QString qrString = IMEI + "--" + serial + "--" + pairToken; this->updateQrCode(qrString); @@ -105,13 +108,12 @@ PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QWidget(parent) { return; } - // TODO: only send the request when widget is shown QString url = "https://api.commadotai.com/v1/devices/" + dongleId + "/owner"; - RequestRepeater* repeater = new RequestRepeater(this, url, 6, "ApiCache_Owner"); - QObject::connect(repeater, SIGNAL(receivedResponse(QString)), this, SLOT(replyFinished(QString))); + RequestRepeater *repeater = new RequestRepeater(this, url, "ApiCache_Owner", 6); + QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &PrimeUserWidget::replyFinished); } -void PrimeUserWidget::replyFinished(QString response) { +void PrimeUserWidget::replyFinished(const QString &response) { QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); if (doc.isNull()) { qDebug() << "JSON Parse failed on getting username and points"; @@ -136,7 +138,7 @@ PrimeAdWidget::PrimeAdWidget(QWidget* parent) : QWidget(parent) { vlayout->addWidget(new QLabel("Upgrade now"), 1, Qt::AlignTop); - QLabel* description = new QLabel("Become a comma prime member in the comma connect app and get premium features!"); + QLabel* description = new QLabel("Become a comma prime member at my.comma.ai and get premium features!"); description->setStyleSheet(R"( font-size: 50px; color: #b8b8b8; @@ -182,7 +184,7 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { background: #585858; )"); finishRegistationLayout->addWidget(finishButton); - QObject::connect(finishButton, SIGNAL(released()), this, SLOT(showQrCode())); + QObject::connect(finishButton, &QPushButton::released, this, &SetupWidget::showQrCode); QWidget* finishRegistration = new QWidget; finishRegistration->setLayout(finishRegistationLayout); @@ -192,7 +194,7 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { QVBoxLayout* qrLayout = new QVBoxLayout; - qrLayout->addSpacing(40); + qrLayout->addSpacing(30); QLabel* qrLabel = new QLabel("Scan with comma connect!"); qrLabel->setWordWrap(true); qrLabel->setAlignment(Qt::AlignHCenter); @@ -239,14 +241,14 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { // set up API requests QString dongleId = QString::fromStdString(Params().get("DongleId")); QString url = "https://api.commadotai.com/v1.1/devices/" + dongleId + "/"; - RequestRepeater* repeater = new RequestRepeater(this, url, 5, "ApiCache_Device"); + RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_Device", 5); - QObject::connect(repeater, SIGNAL(receivedResponse(QString)), this, SLOT(replyFinished(QString))); - QObject::connect(repeater, SIGNAL(failedResponse(QString)), this, SLOT(parseError(QString))); + QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &SetupWidget::replyFinished); + QObject::connect(repeater, &RequestRepeater::failedResponse, this, &SetupWidget::parseError); hide(); // Only show when first request comes back } -void SetupWidget::parseError(QString response) { +void SetupWidget::parseError(const QString &response) { show(); showQr = false; mainLayout->setCurrentIndex(0); @@ -257,7 +259,7 @@ void SetupWidget::showQrCode(){ mainLayout->setCurrentIndex(1); } -void SetupWidget::replyFinished(QString response) { +void SetupWidget::replyFinished(const QString &response) { show(); QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); if (doc.isNull()) { diff --git a/selfdrive/ui/qt/widgets/setup.hpp b/selfdrive/ui/qt/widgets/setup.h similarity index 81% rename from selfdrive/ui/qt/widgets/setup.hpp rename to selfdrive/ui/qt/widgets/setup.h index 0479db8f9..ad6e0dc8d 100644 --- a/selfdrive/ui/qt/widgets/setup.hpp +++ b/selfdrive/ui/qt/widgets/setup.h @@ -5,7 +5,7 @@ #include #include -#include "api.hpp" +#include "selfdrive/ui/qt/api.h" class PairingQRWidget : public QWidget { Q_OBJECT @@ -16,6 +16,7 @@ public: private: QLabel* qrCode; void updateQrCode(QString text); + void showEvent(QShowEvent *event); private slots: void refresh(); @@ -33,7 +34,7 @@ private: CommaApi* api; private slots: - void replyFinished(QString response); + void replyFinished(const QString &response); }; class PrimeAdWidget : public QWidget { @@ -56,7 +57,7 @@ private: bool showQr = false; private slots: - void parseError(QString response); - void replyFinished(QString response); + void parseError(const QString &response); + void replyFinished(const QString &response); void showQrCode(); }; diff --git a/selfdrive/ui/qt/widgets/ssh_keys.cc b/selfdrive/ui/qt/widgets/ssh_keys.cc index 6a22defbe..d180d558c 100644 --- a/selfdrive/ui/qt/widgets/ssh_keys.cc +++ b/selfdrive/ui/qt/widgets/ssh_keys.cc @@ -1,9 +1,11 @@ -#include -#include -#include "widgets/input.hpp" -#include "widgets/ssh_keys.hpp" -#include "common/params.h" +#include "ssh_keys.h" +#include +#include + +#include "selfdrive/common/params.h" +#include "selfdrive/ui/qt/api.h" +#include "selfdrive/ui/qt/widgets/input.h" SshControl::SshControl() : AbstractControl("SSH Keys", "Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.", "") { @@ -27,33 +29,26 @@ SshControl::SshControl() : AbstractControl("SSH Keys", "Warning: This grants SSH QObject::connect(&btn, &QPushButton::released, [=]() { if (btn.text() == "ADD") { - username = InputDialog::getText("Enter your GitHub username"); + QString username = InputDialog::getText("Enter your GitHub username"); if (username.length() > 0) { btn.setText("LOADING"); btn.setEnabled(false); getUserKeys(username); } } else { - Params().delete_db_value("GithubUsername"); - Params().delete_db_value("GithubSshKeys"); + params.remove("GithubUsername"); + params.remove("GithubSshKeys"); refresh(); } }); - // setup networking - manager = new QNetworkAccessManager(this); - networkTimer = new QTimer(this); - networkTimer->setSingleShot(true); - networkTimer->setInterval(5000); - connect(networkTimer, SIGNAL(timeout()), this, SLOT(timeout())); - refresh(); } void SshControl::refresh() { - QString param = QString::fromStdString(Params().get("GithubSshKeys")); + QString param = QString::fromStdString(params.get("GithubSshKeys")); if (param.length()) { - username_label.setText(QString::fromStdString(Params().get("GithubUsername"))); + username_label.setText(QString::fromStdString(params.get("GithubUsername"))); btn.setText("REMOVE"); } else { username_label.setText(""); @@ -62,49 +57,27 @@ void SshControl::refresh() { btn.setEnabled(true); } -void SshControl::getUserKeys(QString username){ - QString url = "https://github.com/" + username + ".keys"; - - QNetworkRequest request; - request.setUrl(QUrl(url)); -#ifdef QCOM - QSslConfiguration ssl = QSslConfiguration::defaultConfiguration(); - ssl.setCaCertificates(QSslCertificate::fromPath("/usr/etc/tls/cert.pem", - QSsl::Pem, QRegExp::Wildcard)); - request.setSslConfiguration(ssl); -#endif - - reply = manager->get(request); - connect(reply, SIGNAL(finished()), this, SLOT(parseResponse())); - networkTimer->start(); -} - -void SshControl::timeout(){ - reply->abort(); -} - -void SshControl::parseResponse(){ - QString err = ""; - if (reply->error() != QNetworkReply::OperationCanceledError) { - networkTimer->stop(); - QString response = reply->readAll(); - if (reply->error() == QNetworkReply::NoError && response.length()) { - Params().write_db_value("GithubUsername", username.toStdString()); - Params().write_db_value("GithubSshKeys", response.toStdString()); - } else if(reply->error() == QNetworkReply::NoError){ - err = "Username '" + username + "' has no keys on GitHub"; +void SshControl::getUserKeys(const QString &username) { + HttpRequest *request = new HttpRequest(this, "https://github.com/" + username + ".keys", "", false); + QObject::connect(request, &HttpRequest::receivedResponse, [=](const QString &resp) { + if (!resp.isEmpty()) { + Params params; + params.put("GithubUsername", username.toStdString()); + params.put("GithubSshKeys", resp.toStdString()); } else { - err = "Username '" + username + "' doesn't exist on GitHub"; + ConfirmationDialog::alert("Username '" + username + "' has no keys on GitHub"); } - } else { - err = "Request timed out"; - } - - if (err.length()) { - ConfirmationDialog::alert(err); - } - - refresh(); - reply->deleteLater(); - reply = nullptr; + refresh(); + request->deleteLater(); + }); + QObject::connect(request, &HttpRequest::failedResponse, [=] { + ConfirmationDialog::alert("Username '" + username + "' doesn't exist on GitHub"); + refresh(); + request->deleteLater(); + }); + QObject::connect(request, &HttpRequest::timeoutResponse, [=] { + ConfirmationDialog::alert("Request timed out"); + refresh(); + request->deleteLater(); + }); } diff --git a/selfdrive/ui/qt/widgets/ssh_keys.hpp b/selfdrive/ui/qt/widgets/ssh_keys.h similarity index 61% rename from selfdrive/ui/qt/widgets/ssh_keys.hpp rename to selfdrive/ui/qt/widgets/ssh_keys.h index 24deee953..aaa7f80dc 100644 --- a/selfdrive/ui/qt/widgets/ssh_keys.hpp +++ b/selfdrive/ui/qt/widgets/ssh_keys.h @@ -1,11 +1,9 @@ #pragma once -#include #include -#include -#include "widgets/controls.hpp" #include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/qt/widgets/controls.h" // SSH enable toggle class SshToggle : public ToggleControl { @@ -27,22 +25,11 @@ public: SshControl(); private: + Params params; + QPushButton btn; - QString username; QLabel username_label; - // networking - QTimer* networkTimer; - QNetworkReply* reply; - QNetworkAccessManager* manager; - void refresh(); - void getUserKeys(QString username); - -signals: - void failedResponse(QString errorString); - -private slots: - void timeout(); - void parseResponse(); + void getUserKeys(const QString &username); }; diff --git a/selfdrive/ui/qt/widgets/toggle.cc b/selfdrive/ui/qt/widgets/toggle.cc index 551630dd7..d4eb215d3 100644 --- a/selfdrive/ui/qt/widgets/toggle.cc +++ b/selfdrive/ui/qt/widgets/toggle.cc @@ -1,4 +1,4 @@ -#include "toggle.hpp" +#include "toggle.h" Toggle::Toggle(QWidget *parent) : QAbstractButton(parent), _height(80), diff --git a/selfdrive/ui/qt/widgets/toggle.hpp b/selfdrive/ui/qt/widgets/toggle.h similarity index 99% rename from selfdrive/ui/qt/widgets/toggle.hpp rename to selfdrive/ui/qt/widgets/toggle.h index 8d9c779de..d2773f207 100644 --- a/selfdrive/ui/qt/widgets/toggle.hpp +++ b/selfdrive/ui/qt/widgets/toggle.h @@ -1,4 +1,5 @@ #pragma once + #include class Toggle : public QAbstractButton { diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index c88a47dd9..864dabf15 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -1,4 +1,5 @@ -#include "window.hpp" +#include "window.h" + #include "selfdrive/hardware/hw.h" MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { @@ -7,25 +8,36 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { homeWindow = new HomeWindow(this); main_layout->addWidget(homeWindow); + QObject::connect(homeWindow, &HomeWindow::openSettings, this, &MainWindow::openSettings); + QObject::connect(homeWindow, &HomeWindow::closeSettings, this, &MainWindow::closeSettings); + QObject::connect(&qs, &QUIState::uiUpdate, homeWindow, &HomeWindow::update); + QObject::connect(&qs, &QUIState::offroadTransition, homeWindow, &HomeWindow::offroadTransition); + QObject::connect(&qs, &QUIState::offroadTransition, homeWindow, &HomeWindow::offroadTransitionSignal); + QObject::connect(&device, &Device::displayPowerChanged, homeWindow, &HomeWindow::displayPowerChanged); settingsWindow = new SettingsWindow(this); main_layout->addWidget(settingsWindow); + QObject::connect(settingsWindow, &SettingsWindow::closeSettings, this, &MainWindow::closeSettings); + QObject::connect(&qs, &QUIState::offroadTransition, settingsWindow, &SettingsWindow::offroadTransition); + QObject::connect(settingsWindow, &SettingsWindow::reviewTrainingGuide, this, &MainWindow::reviewTrainingGuide); onboardingWindow = new OnboardingWindow(this); main_layout->addWidget(onboardingWindow); - QObject::connect(homeWindow, SIGNAL(openSettings()), this, SLOT(openSettings())); - QObject::connect(homeWindow, SIGNAL(closeSettings()), this, SLOT(closeSettings())); - QObject::connect(homeWindow, SIGNAL(offroadTransition(bool)), this, SLOT(offroadTransition(bool))); - QObject::connect(homeWindow, SIGNAL(offroadTransition(bool)), settingsWindow, SIGNAL(offroadTransition(bool))); - QObject::connect(settingsWindow, SIGNAL(closeSettings()), this, SLOT(closeSettings())); - QObject::connect(settingsWindow, SIGNAL(reviewTrainingGuide()), this, SLOT(reviewTrainingGuide())); - - // start at onboarding main_layout->setCurrentWidget(onboardingWindow); - QObject::connect(onboardingWindow, SIGNAL(onboardingDone()), this, SLOT(closeSettings())); + QObject::connect(onboardingWindow, &OnboardingWindow::onboardingDone, this, &MainWindow::closeSettings); onboardingWindow->updateActiveScreen(); + device.setAwake(true, true); + QObject::connect(&qs, &QUIState::uiUpdate, &device, &Device::update); + QObject::connect(&qs, &QUIState::offroadTransition, this, &MainWindow::offroadTransition); + QObject::connect(&device, &Device::displayPowerChanged, this, &MainWindow::closeSettings); + + // load fonts + QFontDatabase::addApplicationFont("../assets/fonts/opensans_regular.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/opensans_bold.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/opensans_semibold.ttf"); + // no outline to prevent the focus rectangle setLayout(main_layout); setStyleSheet(R"( @@ -58,11 +70,11 @@ void MainWindow::reviewTrainingGuide() { bool MainWindow::eventFilter(QObject *obj, QEvent *event){ // wake screen on tap if (event->type() == QEvent::MouseButtonPress) { - homeWindow->glWindow->wake(); + device.setAwake(true, true); } - // filter out touches while in android activity #ifdef QCOM + // filter out touches while in android activity const QList filter_events = {QEvent::MouseButtonPress, QEvent::MouseMove, QEvent::TouchBegin, QEvent::TouchUpdate, QEvent::TouchEnd}; if (HardwareEon::launched_activity && filter_events.contains(event->type())) { HardwareEon::check_activity(); diff --git a/selfdrive/ui/qt/window.hpp b/selfdrive/ui/qt/window.h similarity index 72% rename from selfdrive/ui/qt/window.hpp rename to selfdrive/ui/qt/window.h index 37cc70722..9d3a2a891 100644 --- a/selfdrive/ui/qt/window.hpp +++ b/selfdrive/ui/qt/window.h @@ -1,11 +1,12 @@ #pragma once -#include #include +#include -#include "offroad/settings.hpp" -#include "offroad/onboarding.hpp" -#include "home.hpp" +#include "selfdrive/ui/qt/home.h" +#include "selfdrive/ui/qt/offroad/onboarding.h" +#include "selfdrive/ui/qt/offroad/settings.h" +#include "selfdrive/ui/ui.h" class MainWindow : public QWidget { Q_OBJECT @@ -17,6 +18,9 @@ public: explicit MainWindow(QWidget *parent = 0); private: + Device device; + QUIState qs; + QStackedLayout *main_layout; HomeWindow *homeWindow; SettingsWindow *settingsWindow; diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc deleted file mode 100644 index 805e1520c..000000000 --- a/selfdrive/ui/sidebar.cc +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include -#include -#include -#include "common/util.h" -#include "paint.hpp" -#include "sidebar.hpp" - -static void draw_background(UIState *s) { - const NVGcolor color = nvgRGBA(0x39, 0x39, 0x39, 0xff); - ui_fill_rect(s->vg, {0, 0, sbr_w, s->fb_h}, color); -} - -static void draw_settings_button(UIState *s) { - ui_draw_image(s, settings_btn, "button_settings", 0.65f); -} - -static void draw_home_button(UIState *s) { - ui_draw_image(s, home_btn, "button_home", 1.0f); -} - -static void draw_network_strength(UIState *s) { - static std::map network_strength_map = { - {cereal::DeviceState::NetworkStrength::UNKNOWN, 1}, - {cereal::DeviceState::NetworkStrength::POOR, 2}, - {cereal::DeviceState::NetworkStrength::MODERATE, 3}, - {cereal::DeviceState::NetworkStrength::GOOD, 4}, - {cereal::DeviceState::NetworkStrength::GREAT, 5}}; - const int img_idx = s->scene.deviceState.getNetworkType() == cereal::DeviceState::NetworkType::NONE ? 0 : network_strength_map[s->scene.deviceState.getNetworkStrength()]; - ui_draw_image(s, {58, 196, 176, 27}, util::string_format("network_%d", img_idx).c_str(), 1.0f); -} - -static void draw_network_type(UIState *s) { - static std::map network_type_map = { - {cereal::DeviceState::NetworkType::NONE, "--"}, - {cereal::DeviceState::NetworkType::WIFI, "WiFi"}, - {cereal::DeviceState::NetworkType::CELL2_G, "2G"}, - {cereal::DeviceState::NetworkType::CELL3_G, "3G"}, - {cereal::DeviceState::NetworkType::CELL4_G, "4G"}, - {cereal::DeviceState::NetworkType::CELL5_G, "5G"}}; - const int network_x = 50; - const int network_y = 273; - const int network_w = 100; - const char *network_type = network_type_map[s->scene.deviceState.getNetworkType()]; - nvgFillColor(s->vg, COLOR_WHITE); - nvgFontSize(s->vg, 48); - nvgFontFace(s->vg, "sans-regular"); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, network_x, network_y, network_w, network_type ? network_type : "--", NULL); -} - -static void draw_metric(UIState *s, const char *label_str, const char *value_str, const int severity, const int y_offset, const char *message_str) { - NVGcolor status_color; - - if (severity == 0) { - status_color = COLOR_WHITE; - } else if (severity == 1) { - status_color = COLOR_YELLOW; - } else if (severity > 1) { - status_color = COLOR_RED; - } - - const Rect rect = {30, 338 + y_offset, 240, message_str ? strchr(message_str, '\n') ? 124 : 100 : 148}; - ui_draw_rect(s->vg, rect, severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA(85), 2, 20.); - - nvgBeginPath(s->vg); - nvgRoundedRectVarying(s->vg, rect.x + 6, rect.y + 6, 18, rect.h - 12, 25, 0, 0, 25); - nvgFillColor(s->vg, status_color); - nvgFill(s->vg); - - if (!message_str) { - nvgFillColor(s->vg, COLOR_WHITE); - nvgFontSize(s->vg, 78); - nvgFontFace(s->vg, "sans-bold"); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, rect.x + 50, rect.y + 50, rect.w - 60, value_str, NULL); - - nvgFillColor(s->vg, COLOR_WHITE); - nvgFontSize(s->vg, 48); - nvgFontFace(s->vg, "sans-regular"); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, rect.x + 50, rect.y + 50 + 66, rect.w - 60, label_str, NULL); - } else { - nvgFillColor(s->vg, COLOR_WHITE); - nvgFontSize(s->vg, 48); - nvgFontFace(s->vg, "sans-bold"); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, rect.x + 35, rect.y + (strchr(message_str, '\n') ? 40 : 50), rect.w - 50, message_str, NULL); - } -} - -static void draw_temp_metric(UIState *s) { - static std::map temp_severity_map = { - {cereal::DeviceState::ThermalStatus::GREEN, 0}, - {cereal::DeviceState::ThermalStatus::YELLOW, 1}, - {cereal::DeviceState::ThermalStatus::RED, 2}, - {cereal::DeviceState::ThermalStatus::DANGER, 3}}; - std::string temp_val = std::to_string((int)s->scene.deviceState.getAmbientTempC()) + "°C"; - draw_metric(s, "TEMP", temp_val.c_str(), temp_severity_map[s->scene.deviceState.getThermalStatus()], 0, NULL); -} - -static void draw_panda_metric(UIState *s) { - const int panda_y_offset = 32 + 148; - - int panda_severity = 0; - std::string panda_message = "VEHICLE\nONLINE"; - if (s->scene.pandaType == cereal::PandaState::PandaType::UNKNOWN) { - panda_severity = 2; - panda_message = "NO\nPANDA"; - } -#ifdef QCOM2 - else if (s->scene.started) { - panda_severity = s->scene.gpsOK ? 0 : 1; - panda_message = util::string_format("SAT CNT\n%d", s->scene.satelliteCount); - } -#endif - - draw_metric(s, NULL, NULL, panda_severity, panda_y_offset, panda_message.c_str()); -} - -static void draw_connectivity(UIState *s) { - static std::map> connectivity_map = { - {NET_ERROR, {"CONNECT\nERROR", 2}}, - {NET_CONNECTED, {"CONNECT\nONLINE", 0}}, - {NET_DISCONNECTED, {"CONNECT\nOFFLINE", 1}}, - }; - auto net_params = connectivity_map[s->scene.athenaStatus]; - draw_metric(s, NULL, NULL, net_params.second, 180 + 158, net_params.first); -} - -void ui_draw_sidebar(UIState *s) { - if (s->sidebar_collapsed) { - return; - } - draw_background(s); - draw_settings_button(s); - draw_home_button(s); - draw_network_strength(s); - draw_network_type(s); - draw_temp_metric(s); - draw_panda_metric(s); - draw_connectivity(s); -} diff --git a/selfdrive/ui/sidebar.hpp b/selfdrive/ui/sidebar.hpp deleted file mode 100644 index f273e16f8..000000000 --- a/selfdrive/ui/sidebar.hpp +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once -#include "ui.hpp" - -void ui_draw_sidebar(UIState *s); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2d268f795..0d91560f1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -1,21 +1,24 @@ -#include -#include -#include -#include +#include "selfdrive/ui/ui.h" + #include +#include +#include -#include "common/util.h" -#include "common/swaglog.h" -#include "common/visionimg.h" -#include "ui.hpp" -#include "paint.hpp" +#include +#include +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/common/visionimg.h" +#include "selfdrive/common/watchdog.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/paint.h" +#include "selfdrive/ui/qt/qt_window.h" + +#define BACKLIGHT_DT 0.25 +#define BACKLIGHT_TS 2.00 +#define BACKLIGHT_OFFROAD 50 -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 Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s)); -} // Projects a point in car to space to the corresponding point in full frame // image space. @@ -23,7 +26,7 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, const float margin = 500.0f; const vec3 pt = (vec3){{in_x, in_y, in_z}}; const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); - const vec3 KEp = matvecmul3(fcam_intrinsic_matrix, Ep); + const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); // Project. float x = KEp.v[0] / KEp.v[2]; @@ -52,27 +55,6 @@ static void ui_init_vision(UIState *s) { assert(glGetError() == GL_NO_ERROR); } - -void ui_init(UIState *s) { - s->sm = new SubMaster({ - "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "liveLocationKalman", - "pandaState", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss", -#ifdef QCOM2 - "roadCameraState", -#endif - }); - - s->scene.started = false; - s->status = STATUS_OFFROAD; - - ui_nvg_init(s); - - s->last_frame = nullptr; - s->vipc_client_rear = new VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, true); - s->vipc_client_front = new VisionIpcClient("camerad", VISION_STREAM_RGB_FRONT, true); - s->vipc_client = s->vipc_client_rear; -} - static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { const auto line_x = line.getX(); int max_idx = 0; @@ -140,9 +122,13 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { update_line_data(s, model_position, 0.5, 1.22, &scene.track_vertices, max_idx); } -static void update_sockets(UIState *s) { +static void update_sockets(UIState *s){ + SubMaster &sm = *(s->sm); + sm.update(0); +} + +static void update_state(UIState *s) { SubMaster &sm = *(s->sm); - if (sm.update(0) == 0) return; UIScene &scene = s->scene; if (scene.started && sm.updated("controlsState")) { @@ -205,19 +191,13 @@ static void update_sockets(UIState *s) { } if (sm.updated("driverMonitoringState")) { scene.dmonitoring_state = sm["driverMonitoringState"].getDriverMonitoringState(); - if(!scene.driver_view && !scene.ignition) { - read_param(&scene.driver_view, "IsDriverViewEnabled"); - } - } else if ((sm.frame - sm.rcv_frame("driverMonitoringState")) > UI_FREQ/2) { - scene.driver_view = false; } if (sm.updated("sensorEvents")) { for (auto sensor : sm["sensorEvents"].getSensorEvents()) { - if (sensor.which() == cereal::SensorEventData::LIGHT) { -#ifndef QCOM2 + if (!Hardware::TICI() && sensor.which() == cereal::SensorEventData::LIGHT) { scene.light_sensor = sensor.getLight(); -#endif - } else if (!scene.started && sensor.which() == cereal::SensorEventData::ACCELERATION) { + } + if (!scene.started && sensor.which() == cereal::SensorEventData::ACCELERATION) { auto accel = sensor.getAcceleration().getV(); if (accel.totalSize().wordCount){ // TODO: sometimes empty lists are received. Figure out why scene.accel_sensor = accel[2]; @@ -230,69 +210,19 @@ static void update_sockets(UIState *s) { } } } -#ifdef QCOM2 - if (sm.updated("roadCameraState")) { + if (Hardware::TICI() && sm.updated("roadCameraState")) { auto camera_state = sm["roadCameraState"].getRoadCameraState(); float gain = camera_state.getGainFrac() * (camera_state.getGlobalGain() > 100 ? 2.5 : 1.0) / 10.0; scene.light_sensor = std::clamp((1023.0 / 1757.0) * (1757.0 - camera_state.getIntegLines()) * (1.0 - gain), 0.0, 1023.0); } -#endif scene.started = scene.deviceState.getStarted() || scene.driver_view; } -static void update_alert(UIState *s) { - UIScene &scene = s->scene; - if (s->sm->updated("controlsState")) { - auto alert_sound = scene.controls_state.getAlertSound(); - if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) { - if (alert_sound == AudibleAlert::NONE) { - s->sound->stop(); - } else { - s->sound->play(alert_sound); - } - } - scene.alert_text1 = scene.controls_state.getAlertText1(); - scene.alert_text2 = scene.controls_state.getAlertText2(); - scene.alert_size = scene.controls_state.getAlertSize(); - scene.alert_type = scene.controls_state.getAlertType(); - scene.alert_blinking_rate = scene.controls_state.getAlertBlinkingRate(); - } - - // Handle controls timeout - if (scene.deviceState.getStarted() && (s->sm->frame - scene.started_frame) > 10 * UI_FREQ) { - const uint64_t cs_frame = s->sm->rcv_frame("controlsState"); - if (cs_frame < scene.started_frame) { - // car is started, but controlsState hasn't been seen at all - scene.alert_text1 = "openpilot Unavailable"; - scene.alert_text2 = "Waiting for controls to start"; - scene.alert_size = cereal::ControlsState::AlertSize::MID; - } else if ((s->sm->frame - cs_frame) > 5 * UI_FREQ) { - // car is started, but controls is lagging or died - if (scene.alert_text2 != "Controls Unresponsive") { - s->sound->play(AudibleAlert::CHIME_WARNING_REPEAT); - LOGE("Controls unresponsive"); - } - - scene.alert_text1 = "TAKE CONTROL IMMEDIATELY"; - scene.alert_text2 = "Controls Unresponsive"; - scene.alert_size = cereal::ControlsState::AlertSize::FULL; - s->status = STATUS_ALERT; - } - } -} - static void update_params(UIState *s) { const uint64_t frame = s->sm->frame; UIScene &scene = s->scene; - if (frame % (5*UI_FREQ) == 0) { - read_param(&scene.is_metric, "IsMetric"); - } else if (frame % (6*UI_FREQ) == 0) { - scene.athenaStatus = NET_DISCONNECTED; - uint64_t last_ping = 0; - if (read_param(&last_ping, "LastAthenaPingTime") == 0) { - scene.athenaStatus = nanos_since_boot() - last_ping < 70e9 ? NET_CONNECTED : NET_ERROR; - } + scene.is_metric = Params().getBool("IsMetric"); } } @@ -307,10 +237,8 @@ static void update_vision(UIState *s) { VisionBuf * buf = s->vipc_client->recv(); if (buf != nullptr){ s->last_frame = buf; - } else { -#if defined(QCOM) || defined(QCOM2) + } else if (!Hardware::PC()) { LOGE("visionIPC receive timeout"); -#endif } } } @@ -334,25 +262,117 @@ static void update_status(UIState *s) { s->status = STATUS_DISENGAGED; s->scene.started_frame = s->sm->frame; - read_param(&s->scene.is_rhd, "IsRHD"); - read_param(&s->scene.end_to_end, "EndToEndToggle"); - s->sidebar_collapsed = true; - s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; + s->scene.is_rhd = Params().getBool("IsRHD"); + s->scene.end_to_end = Params().getBool("EndToEndToggle"); s->vipc_client = s->scene.driver_view ? s->vipc_client_front : s->vipc_client_rear; } else { - s->status = STATUS_OFFROAD; - s->sidebar_collapsed = false; - s->sound->stop(); s->vipc_client->connected = false; } } started_prev = s->scene.started; } -void ui_update(UIState *s) { - update_params(s); - update_sockets(s); - update_status(s); - update_alert(s); - update_vision(s); + +QUIState::QUIState(QObject *parent) : QObject(parent) { + ui_state.sm = std::make_unique>({ + "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "liveLocationKalman", + "pandaState", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss", +#ifdef QCOM2 + "roadCameraState", +#endif + }); + + ui_state.fb_w = vwp_w; + ui_state.fb_h = vwp_h; + ui_state.scene.started = false; + ui_state.last_frame = nullptr; + ui_state.wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false; + + ui_state.vipc_client_rear = new VisionIpcClient("camerad", ui_state.wide_camera ? VISION_STREAM_RGB_WIDE : VISION_STREAM_RGB_BACK, true); + ui_state.vipc_client_front = new VisionIpcClient("camerad", VISION_STREAM_RGB_FRONT, true); + ui_state.vipc_client = ui_state.vipc_client_rear; + + // update timer + timer = new QTimer(this); + QObject::connect(timer, &QTimer::timeout, this, &QUIState::update); + timer->start(0); +} + +void QUIState::update() { + update_params(&ui_state); + update_sockets(&ui_state); + update_state(&ui_state); + update_status(&ui_state); + update_vision(&ui_state); + + if (ui_state.scene.started != started_prev || ui_state.sm->frame == 1) { + started_prev = ui_state.scene.started; + emit offroadTransition(!ui_state.scene.started); + + // Change timeout to 0 when onroad, this will call update continously. + // This puts visionIPC in charge of update frequency, reducing video latency + timer->start(ui_state.scene.started ? 0 : 1000 / UI_FREQ); + } + + watchdog_kick(); + emit uiUpdate(ui_state); +} + +Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QObject(parent) { + brightness_b = Params(true).get("BRIGHTNESS_B").value_or(10.0); + brightness_m = Params(true).get("BRIGHTNESS_M").value_or(0.1); +} + +void Device::update(const UIState &s) { + updateBrightness(s); + updateWakefulness(s); + + // TODO: remove from UIState and use signals + QUIState::ui_state.awake = awake; +} + +void Device::setAwake(bool on, bool reset) { + if (on != awake) { + awake = on; + Hardware::set_display_power(awake); + LOGD("setting display power %d", awake); + emit displayPowerChanged(awake); + } + + if (reset) { + awake_timeout = 30 * UI_FREQ; + } +} + +void Device::updateBrightness(const UIState &s) { + float clipped_brightness = std::min(100.0f, (s.scene.light_sensor * brightness_m) + brightness_b); + if (Hardware::TICI() && !s.scene.started) { + clipped_brightness = BACKLIGHT_OFFROAD; + } + + int brightness = brightness_filter.update(clipped_brightness); + if (!awake) { + brightness = 0; + } + + if (brightness != last_brightness) { + std::thread{Hardware::set_brightness, brightness}.detach(); + } + last_brightness = brightness; +} + +void Device::updateWakefulness(const UIState &s) { + awake_timeout = std::max(awake_timeout - 1, 0); + + bool should_wake = s.scene.started || s.scene.ignition; + if (!should_wake) { + // tap detection while display is off + bool accel_trigger = abs(s.scene.accel_sensor - accel_prev) > 0.2; + bool gyro_trigger = abs(s.scene.gyro_sensor - gyro_prev) > 0.15; + should_wake = accel_trigger && gyro_trigger; + gyro_prev = s.scene.gyro_sensor; + accel_prev = (accel_prev * (accel_samples - 1) + s.scene.accel_sensor) / accel_samples; + } + + setAwake(awake_timeout, should_wake); } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.h similarity index 63% rename from selfdrive/ui/ui.hpp rename to selfdrive/ui/ui.h index aec9a2256..9c20d202c 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.h @@ -1,33 +1,27 @@ #pragma once -#include "messaging.hpp" - -#ifdef __APPLE__ -#include -#define NANOVG_GL3_IMPLEMENTATION -#define nvgCreate nvgCreateGL3 -#else -#include -#define NANOVG_GLES3_IMPLEMENTATION -#define nvgCreate nvgCreateGLES3 -#endif #include #include #include -#include #include +#include + +#include +#include #include "nanovg.h" -#include "common/mat.h" -#include "common/visionimg.h" -#include "common/modeldata.h" -#include "common/params.h" -#include "common/glutil.h" +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc.h" +#include "cereal/visionipc/visionipc_client.h" #include "common/transformations/orientation.hpp" -#include "qt/sound.hpp" -#include "visionipc.h" -#include "visionipc_client.h" +#include "selfdrive/camerad/cameras/camera_common.h" +#include "selfdrive/common/glutil.h" +#include "selfdrive/common/mat.h" +#include "selfdrive/common/modeldata.h" +#include "selfdrive/common/params.h" +#include "selfdrive/common/util.h" +#include "selfdrive/common/visionimg.h" #define COLOR_BLACK nvgRGBA(0, 0, 0, 255) #define COLOR_BLACK_ALPHA(x) nvgRGBA(0, 0, 0, x) @@ -37,8 +31,6 @@ #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) -#define UI_BUF_COUNT 4 - typedef struct Rect { int x, y, w, h; int centerX() const { return x + w / 2; } @@ -50,23 +42,13 @@ typedef struct Rect { } } Rect; -const int sbr_w = 300; const int bdr_s = 30; const int header_h = 420; const int footer_h = 280; -const Rect settings_btn = {50, 35, 200, 117}; -const Rect home_btn = {60, 1080 - 180 - 40, 180, 180}; const int UI_FREQ = 20; // Hz -typedef enum NetStatus { - NET_CONNECTED, - NET_DISCONNECTED, - NET_ERROR, -} NetStatus; - typedef enum UIStatus { - STATUS_OFFROAD, STATUS_DISENGAGED, STATUS_ENGAGED, STATUS_WARNING, @@ -74,7 +56,6 @@ typedef enum UIStatus { } UIStatus; static std::map bg_colors = { - {STATUS_OFFROAD, nvgRGBA(0x0, 0x0, 0x0, 0xff)}, {STATUS_DISENGAGED, nvgRGBA(0x17, 0x33, 0x49, 0xc8)}, {STATUS_ENGAGED, nvgRGBA(0x17, 0x86, 0x44, 0xf1)}, {STATUS_WARNING, nvgRGBA(0xDA, 0x6F, 0x25, 0xf1)}, @@ -98,14 +79,7 @@ typedef struct UIScene { bool is_rhd; bool driver_view; - std::string alert_text1; - std::string alert_text2; - std::string alert_type; - float alert_blinking_rate; - cereal::ControlsState::AlertSize alert_size; - cereal::PandaState::PandaType pandaType; - NetStatus athenaStatus; cereal::DeviceState::Reader deviceState; cereal::RadarState::LeadData::Reader lead_data[2]; @@ -148,9 +122,8 @@ typedef struct UIState { // images std::map images; - SubMaster *sm; + std::unique_ptr sm; - Sound *sound; UIStatus status; UIScene scene; @@ -161,38 +134,67 @@ typedef struct UIState { GLuint frame_vao[2], frame_vbo[2], frame_ibo[2]; mat4 rear_frame_mat, front_frame_mat; - // device state bool awake; - bool sidebar_collapsed; Rect video_rect, viz_rect; float car_space_transform[6]; + bool wide_camera; + float zoom; } UIState; -void ui_init(UIState *s); -void ui_update(UIState *s); -int write_param_float(float param, const char* param_name, bool persistent_param = false); -template -int read_param(T* param, const char *param_name, bool persistent_param = false){ - T param_orig = *param; - char *value; - size_t sz; +class QUIState : public QObject { + Q_OBJECT - 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); +public: + QUIState(QObject* parent = 0); - // Parse result - std::istringstream iss(s); - iss >> *param; + // TODO: get rid of this, only use signal + inline static UIState ui_state = {0}; - // Restore original value if parsing failed - if (iss.fail()) { - *param = param_orig; - result = -1; - } - } - return result; -} +signals: + void uiUpdate(const UIState &s); + void offroadTransition(bool offroad); + +private slots: + void update(); + +private: + QTimer *timer; + bool started_prev = true; +}; + + +// device management class + +class Device : public QObject { + Q_OBJECT + +public: + Device(QObject *parent = 0); + +private: + // auto brightness + const float accel_samples = 5*UI_FREQ; + + bool awake; + int awake_timeout = 0; + float accel_prev = 0; + float gyro_prev = 0; + float brightness_b = 0; + float brightness_m = 0; + float last_brightness = 0; + FirstOrderFilter brightness_filter; + + QTimer *timer; + + void updateBrightness(const UIState &s); + void updateWakefulness(const UIState &s); + +signals: + void displayPowerChanged(bool on); + +public slots: + void setAwake(bool on, bool reset); + void update(const UIState &s); +}; diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 035b9a967..6ec0b4208 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -119,7 +119,7 @@ def set_params(new_version: bool, failed_count: int, exception: Optional[str]) - params.put("ReleaseNotes", r + b"\n") except Exception: params.put("ReleaseNotes", "") - params.put("UpdateAvailable", "1") + params.put_bool("UpdateAvailable", True) def setup_git_options(cwd: str) -> None: @@ -165,7 +165,7 @@ def init_overlay() -> None: cloudlog.info("preparing new safe staging area") params = Params() - params.put("UpdateAvailable", "0") + params.put_bool("UpdateAvailable", False) set_consistent_flag(False) dismount_overlay() if os.path.isdir(STAGING_ROOT): @@ -281,12 +281,17 @@ def check_git_fetch_result(fetch_txt): def check_for_update() -> Tuple[bool, bool]: setup_git_options(OVERLAY_MERGED) + fetch_output = None try: - git_fetch_output = run(["git", "fetch", "--dry-run"], OVERLAY_MERGED, low_priority=True) - return True, check_git_fetch_result(git_fetch_output) + fetch_output = run(["git", "fetch", "--dry-run"], OVERLAY_MERGED, low_priority=True) + return True, check_git_fetch_result(fetch_output) except subprocess.CalledProcessError: - return False, False + # check for internet + if fetch_output is not None and fetch_output.startswith("fatal: unable to access") and \ + "Could not resolve host:" in str(fetch_output): + return False, False + raise def fetch_update(wait_helper: WaitTimeHelper) -> bool: cloudlog.info("attempting git fetch inside staging overlay") @@ -332,7 +337,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool: def main(): params = Params() - if params.get("DisableUpdates") == b"1": + if params.get_bool("DisableUpdates"): raise RuntimeError("updates are disabled by the DisableUpdates param") if EON and os.geteuid() != 0: @@ -370,7 +375,7 @@ def main(): # Don't run updater while onroad or if the time's wrong time_wrong = datetime.datetime.utcnow().year < 2019 - is_onroad = params.get("IsOffroad") != b"1" + is_onroad = not params.get_bool("IsOffroad") if is_onroad or time_wrong: wait_helper.sleep(30) cloudlog.info("not running updater, not offroad")