diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 8f54cdf0789faf..6a9f715b3d57ec 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -28,7 +28,7 @@ Code is automatically checked for style by Github Actions as part of the automat We've released a [Model Port guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for porting to Toyota/Lexus models. -If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty. +If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty. ## Pull Requests diff --git a/README.md b/README.md index 7722721839a43b..517cde940d0fe2 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ Supported Cars | ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------| | Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 25mph | | Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 12mph | -| Acura | RDX 2020 | AcuraWatch | Stock | 0mph | 3mph | +| Acura | RDX 2020 | All | Stock | 0mph | 3mph | | Honda | Accord 2018-20 | All | Stock | 0mph | 3mph | | Honda | Accord Hybrid 2018-20 | All | Stock | 0mph | 3mph | | Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph | @@ -78,6 +78,7 @@ Supported Cars | Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph1 | 12mph | | Honda | HR-V 2019 | Honda Sensing | openpilot | 25mph1 | 12mph | | Honda | Insight 2019-20 | All | Stock | 0mph | 3mph | +| Honda | Inspire 2018 | All | Stock | 0mph | 3mph | | Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph1 | 0mph | | Honda | Passport 2019 | All | openpilot | 25mph1 | 12mph | | Honda | Pilot 2016-19 | Honda Sensing | openpilot | 25mph1 | 12mph | @@ -85,10 +86,11 @@ Supported Cars | Hyundai | Palisade 2020 | All | Stock | 0mph | 0mph | | Hyundai | Sonata 2020 | All | Stock | 0mph | 0mph | | Lexus | CT Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph | -| Lexus | ES 2019 | All | openpilot | 0mph | 0mph | +| Lexus | ES 2019-20 | All | openpilot | 0mph | 0mph | | Lexus | ES Hybrid 2019 | 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 Hybrid 2018 | All | Stock3| 0mph | 0mph | | Lexus | RX 2016-18 | All | Stock3| 0mph | 0mph | | Lexus | RX 2020 | All | openpilot | 0mph | 0mph | @@ -96,11 +98,11 @@ Supported Cars | Lexus | RX Hybrid 2020 | All | openpilot | 0mph | 0mph | | Toyota | Avalon 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph | -| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph4 | 0mph | +| Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph4 | 0mph | | Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | | Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | | Toyota | Corolla 2017-19 | All | Stock3| 20mph1 | 0mph | -| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Corolla 2020-21 | All | openpilot | 0mph | 0mph | | Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph | | Toyota | Corolla Hybrid 2020-21 | All | openpilot | 0mph | 0mph | | Toyota | Highlander 2017-19 | All | Stock3| 0mph | 0mph | @@ -108,11 +110,12 @@ Supported Cars | Toyota | Highlander Hybrid 2017-19 | All | Stock3| 0mph | 0mph | | Toyota | Highlander Hybrid 2020 | 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 | | Toyota | Rav4 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Rav4 2019-21 | All | openpilot | 0mph | 0mph | | Toyota | Rav4 Hybrid 2016-18 | TSS-P | Stock3| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph | +| Toyota | Rav4 Hybrid 2019-21 | All | openpilot | 0mph | 0mph | | Toyota | Sienna 2018-20 | All | Stock3| 0mph | 0mph | 1[Comma Pedal](https://github.com/commaai/openpilot/wiki/comma-pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).***
@@ -136,7 +139,7 @@ Community Maintained Cars and Features | Genesis | G70 2018 | All | Stock | 0mph | 0mph | | Genesis | G80 2018 | All | Stock | 0mph | 0mph | | Genesis | G90 2018 | All | Stock | 0mph | 0mph | -| GMC | Acadia Denali 20182| Adaptive Cruise | openpilot | 0mph | 7mph | +| GMC | Acadia 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | | Holden | Astra 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | | Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph | | Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph | @@ -149,6 +152,7 @@ Community Maintained Cars and Features | Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | | Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | | Kia | Forte 2018-19 | 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 | Sorento 2018 | SCC + LKAS | Stock | 0mph | 0mph | @@ -162,8 +166,7 @@ Community Maintained Cars and Features | Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | | Volkswagen| Golf 2015-19 | Driver Assistance | Stock | 0mph | 0mph | -1Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built giraffe](https://github.com/commaai/openpilot/wiki/GM). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
-2Requires a custom connector for the developer [car harness](https://comma.ai/shop/products/car-harness)
+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).***
Although it's not upstream, there's a community of people getting openpilot to run on Tesla's [here](https://tinkla.us/) @@ -239,7 +242,6 @@ Many factors can impact the performance of openpilot DM, causing it to be unable * Low light conditions, such as driving at night or in dark tunnels. * Bright light (due to oncoming headlights, direct sunlight, etc.). * The driver's face is partially or completely outside field of view of the driver facing camera. -* Right hand driving vehicles. * The driver facing camera is obstructed, covered, or damaged. The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention. @@ -269,7 +271,7 @@ Safety and Testing Testing on PC ------ -For simplified development and experimentation, openpilot can be run in the CARLA driving simulator, which allows you to develop openpilot without a car. The whole setup should only take a few minutes. +For simplified development and experimentation, openpilot can be run in the CARLA driving simulator, which allows you to develop openpilot without a car. The whole setup should only take a few minutes. Steps: 1) Start the CARLA server on first terminal diff --git a/RELEASES.md b/RELEASES.md index c3a5bd0eaf43d5..b8869f840944f4 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,14 @@ +Version 0.8.0 (2020-11-30) +======================== + * New driving model: fully 3D and improved cut-in detection + * UI draws 2 road edges, 4 lanelines and paths in 3D + * Major fixes to cut-in detection for openpilot longitudinal + * Grey panda is no longer supported, upgrade to comma two or black panda + * Lexus NX 2018 support thanks to matt12eagles! + * Kia Niro EV 2020 support thanks to nickn17! + * Improved lane positioning with uncertain lanelines, wide lanes and exits + * Improved lateral control for Prius and Subaru + Version 0.7.10 (2020-10-29) ======================== * Grey panda is deprecated, upgrade to comma two or black panda diff --git a/SConstruct b/SConstruct index 1b863bebaed13f..758d494f3b19a8 100644 --- a/SConstruct +++ b/SConstruct @@ -5,6 +5,8 @@ import shutil import subprocess import sys import platform +import numpy as np +from sysconfig import get_paths TICI = os.path.isfile('/TICI') Decider('MD5-timestamp') @@ -128,6 +130,10 @@ else: # change pythonpath to this lenv["PYTHONPATH"] = Dir("#").path +#Get the path for Python.h for cython linking +python_path = get_paths()['include'] +numpy_path = np.get_include() + env = Environment( ENV=lenv, CCFLAGS=[ @@ -159,6 +165,7 @@ env = Environment( "#phonelibs/linux/include", "#phonelibs/snpe/include", "#phonelibs/nanovg", + "#selfdrive/boardd", "#selfdrive/common", "#selfdrive/camerad", "#selfdrive/camerad/include", @@ -181,50 +188,14 @@ env = Environment( CXXFLAGS=["-std=c++1z"] + cxxflags, LIBPATH=libpath + [ "#cereal", + "#selfdrive/boardd", "#selfdrive/common", "#phonelibs", - ] + ], + CYTHONCFILESUFFIX=".cpp", + tools=["default", "cython"] ) -qt_env = None -if arch in ["x86_64", "Darwin", "larch64"]: - qt_env = env.Clone() - - if arch == "Darwin": - qt_env['QTDIR'] = "/usr/local/opt/qt" - QT_BASE = "/usr/local/opt/qt/" - qt_dirs = [ - QT_BASE + "include/", - QT_BASE + "include/QtWidgets", - QT_BASE + "include/QtGui", - QT_BASE + "include/QtCore", - QT_BASE + "include/QtDBus", - QT_BASE + "include/QtMultimedia", - ] - qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"] - else: - qt_env['QTDIR'] = "/usr" - qt_dirs = [ - f"/usr/include/{real_arch}-linux-gnu/qt5", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtWidgets", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtCore", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtDBus", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtMultimedia", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.5.1/QtGui", - ] - - qt_env.Tool('qt') - qt_env['CPPPATH'] += qt_dirs - qt_flags = [ - "-D_REENTRANT", - "-DQT_NO_DEBUG", - "-DQT_WIDGETS_LIB", - "-DQT_GUI_LIB", - "-DQT_CORE_LIB" - ] - qt_env['CXXFLAGS'] += qt_flags - if os.environ.get('SCONS_CACHE'): cache_dir = '/tmp/scons_cache' @@ -238,6 +209,9 @@ if os.environ.get('SCONS_CACHE'): if not os.path.isdir(cache_dir_branch) and os.path.isdir(cache_dir): shutil.copytree(cache_dir, cache_dir_branch) cache_dir = cache_dir_branch + elif TICI: + cache_dir = '/data/scons_cache' + CacheDir(cache_dir) node_interval = 5 @@ -261,9 +235,28 @@ def abspath(x): # rpath works elsewhere return x[0].path.rsplit("/", 1)[1][:-3] +#Cython build enviroment +envCython = env.Clone() +envCython["CPPPATH"] += [python_path, numpy_path] +envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-deprecated-declarations"] + +python_libs = [] +if arch == "Darwin": + envCython["LINKFLAGS"]=["-bundle", "-undefined", "dynamic_lookup"] +elif arch == "aarch64": + envCython["LINKFLAGS"]=["-shared"] + + python_libs.append(os.path.basename(python_path)) +else: + envCython["LINKFLAGS"]=["-pthread", "-shared"] + +envCython["LIBS"] = python_libs + +Export('envCython') + # still needed for apks zmq = 'zmq' -Export('env', 'qt_env', 'arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY') +Export('env', 'arch', 'real_arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY') # cereal and messaging are shared with the system SConscript(['cereal/SConscript']) @@ -316,6 +309,5 @@ SConscript(['selfdrive/ui/SConscript']) if arch != "Darwin": SConscript(['selfdrive/logcatd/SConscript']) - if arch == "x86_64": SConscript(['tools/lib/index_log/SConscript']) diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index f6d37b3e412be0..90570253edabdb 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/SConscript b/cereal/SConscript index 0ab943db974b65..c0ade46fa9aa46 100644 --- a/cereal/SConscript +++ b/cereal/SConscript @@ -2,6 +2,7 @@ Import('env', 'arch', 'zmq', 'cython_dependencies') import shutil +cereal_dir = Dir('.') gen_dir = Dir('gen') messaging_dir = Dir('messaging') @@ -9,12 +10,12 @@ messaging_dir = Dir('messaging') env.Command(["gen/c/include/c++.capnp.h", "gen/c/include/java.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS") env.Command(['gen/cpp/car.capnp.c++', 'gen/cpp/log.capnp.c++', 'gen/cpp/car.capnp.h', 'gen/cpp/log.capnp.h'], ['car.capnp', 'log.capnp'], - 'capnpc $SOURCES --src-prefix=cereal -o c++:' + gen_dir.path + '/cpp/') + f"capnpc --src-prefix={cereal_dir.path} $SOURCES -o c++:{gen_dir.path}/cpp/") if shutil.which('capnpc-java'): env.Command(['gen/java/Car.java', 'gen/java/Log.java'], ['car.capnp', 'log.capnp'], - 'capnpc $SOURCES --src-prefix=cereal -o java:' + gen_dir.path + '/java/') + f"capnpc $SOURCES --src-prefix={cereal_dir.path} -o java:{gen_dir.path}/java/") # TODO: remove non shared cereal and messaging cereal_objects = env.SharedObject([ diff --git a/cereal/car.capnp b/cereal/car.capnp index 2c34b45007c374..4e4f9e7564a6cd 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -73,7 +73,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { preLaneChangeLeft @57; preLaneChangeRight @58; laneChange @59; - invalidGiraffeToyota @60; internetConnectivityNeeded @61; communityFeatureDisallowed @62; lowMemory @63; @@ -94,14 +93,13 @@ struct CarEvent @0x9b1657f34caf3ad3 { startupMaster @78; fcw @79; steerSaturated @80; - whitePandaUnsupported @81; - startupGreyPanda @82; belowEngageSpeed @84; noGps @85; wrongCruiseMode @87; modeldLagging @89; deviceFalling @90; fanMalfunction @91; + cameraMalfunction @92; gasUnavailableDEPRECATED @3; dataNeededDEPRECATED @16; @@ -112,9 +110,13 @@ struct CarEvent @0x9b1657f34caf3ad3 { driverMonitorOffDEPRECATED @42; calibrationProgressDEPRECATED @47; invalidGiraffeHondaDEPRECATED @49; + invalidGiraffeToyotaDEPRECATED @60; + whitePandaUnsupportedDEPRECATED @81; + startupGreyPandaDEPRECATED @82; canErrorPersistentDEPRECATED @83; focusRecoverActiveDEPRECATED @86; neosUpdateRequiredDEPRECATED @88; + modelLagWarningDEPRECATED @93; } } diff --git a/cereal/log.capnp b/cereal/log.capnp index ac86bb02538072..bbf6a6be0fae11 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -628,6 +628,8 @@ struct ModelData { frameAge @12 :UInt32; frameDropPerc @13 :Float32; timestampEof @9 :UInt64; + modelExecutionTime @14 :Float32; + rawPred @15 :Data; path @1 :PathData; leftLane @2 :PathData; @@ -694,6 +696,8 @@ struct ModelDataV2 { frameAge @1 :UInt32; frameDropPerc @2 :Float32; timestampEof @3 :UInt64; + modelExecutionTime @15 :Float32; + rawPred @16 :Data; position @4 :XYZTData; orientation @5 :XYZTData; @@ -701,7 +705,9 @@ struct ModelDataV2 { orientationRate @7 :XYZTData; laneLines @8 :List(XYZTData); laneLineProbs @9 :List(Float32); + laneLineStds @13 :List(Float32); roadEdges @10 :List(XYZTData); + roadEdgeStds @14 :List(Float32); leads @11 :List(LeadDataV2); meta @12 :MetaData; @@ -754,6 +760,8 @@ struct EncodeIndex { segmentId @4 :UInt32; # index into camera file in segment in encode order segmentIdEncode @5 :UInt32; + timestampSof @6 :UInt64; + timestampEof @7 :UInt64; enum Type { bigBoxLossless @0; # rcamera.mkv @@ -1943,6 +1951,9 @@ struct OrbKeyFrame { struct DriverState { frameId @0 :UInt32; + modelExecutionTime @14 :Float32; + rawPred @15 :Data; + descriptorDEPRECATED @1 :List(Float32); stdDEPRECATED @2 :Float32; faceOrientation @3 :List(Float32); @@ -2119,11 +2130,13 @@ struct Event { thumbnail @66: Thumbnail; carEvents @68: List(Car.CarEvent); carParams @69: Car.CarParams; - frontFrame @70: FrameData; + frontFrame @70: FrameData; # driver facing camera dMonitoringState @71: DMonitoringState; liveLocationKalman @72 :LiveLocationKalman; sentinel @73 :Sentinel; wideFrame @74: FrameData; modelV2 @75 :ModelDataV2; + frontEncodeIdx @76 :EncodeIndex; # driver facing camera + wideEncodeIdx @77 :EncodeIndex; } } diff --git a/common/SConscript b/common/SConscript index d17b35fed6c1f7..542e10d437b658 100644 --- a/common/SConscript +++ b/common/SConscript @@ -1,14 +1,4 @@ -Import('env', 'cython_dependencies') +Import('envCython') -# Build cython clock module -env.Command(['common_pyx.so', 'clock.cpp'], - cython_dependencies + ['common_pyx_setup.py', 'clock.pyx'], - "cd common && python3 common_pyx_setup.py build_ext --inplace") - -# Build cython params module -env.Command(['params_pyx.so', 'params_pyx.cpp'], - cython_dependencies + [ - 'params_pyx_setup.py', 'params_pyx.pyx', 'params_pxd.pxd', - '#selfdrive/common/params.cc', '#selfdrive/common/params.h', - '#selfdrive/common/util.c', '#selfdrive/common/util.h'], - "cd common && python3 params_pyx_setup.py build_ext --inplace") +envCython.Program('clock.so', 'clock.pyx') +envCython.Program('params_pyx.so', 'params_pyx.pyx') diff --git a/common/clock.pyx b/common/clock.pyx index 9702e488cf6263..81333565c58089 100644 --- a/common/clock.pyx +++ b/common/clock.pyx @@ -1,3 +1,4 @@ +# distutils: language = c++ # cython: language_level = 3 from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t diff --git a/common/common_pyx_setup.py b/common/common_pyx_setup.py deleted file mode 100644 index d8f653e67dff12..00000000000000 --- a/common/common_pyx_setup.py +++ /dev/null @@ -1,20 +0,0 @@ -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module -from Cython.Build import cythonize - -from common.cython_hacks import BuildExtWithoutPlatformSuffix - -sourcefiles = ['clock.pyx'] -extra_compile_args = ["-std=c++1z"] - -setup(name='common', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "common_pyx", - language="c++", - sources=sourcefiles, - extra_compile_args=extra_compile_args, - ), - nthreads=4, - ), -) diff --git a/common/kalman/SConscript b/common/kalman/SConscript index 3d7011fe295d6e..d60354c987ab60 100644 --- a/common/kalman/SConscript +++ b/common/kalman/SConscript @@ -1,6 +1,3 @@ -Import('env', 'cython_dependencies') - -env.Command(['simple_kalman_impl.so'], - cython_dependencies + ['simple_kalman_impl.pyx', 'simple_kalman_impl.pxd', 'simple_kalman_setup.py'], - "cd common/kalman && python3 simple_kalman_setup.py build_ext --inplace") +Import('envCython') +envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx') diff --git a/common/kalman/simple_kalman_impl.pyx b/common/kalman/simple_kalman_impl.pyx index e65efd577e6b63..16aefba2e5b796 100644 --- a/common/kalman/simple_kalman_impl.pyx +++ b/common/kalman/simple_kalman_impl.pyx @@ -1,3 +1,4 @@ +# distutils: language = c++ # cython: language_level=3 cdef class KF1D: diff --git a/common/kalman/simple_kalman_setup.py b/common/kalman/simple_kalman_setup.py deleted file mode 100644 index d0781c9ff36de0..00000000000000 --- a/common/kalman/simple_kalman_setup.py +++ /dev/null @@ -1,10 +0,0 @@ -from distutils.core import Extension, setup - -from Cython.Build import cythonize - -from common.cython_hacks import BuildExtWithoutPlatformSuffix - -setup(name='Simple Kalman Implementation', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize(Extension("simple_kalman_impl", - ["simple_kalman_impl.pyx"]))) diff --git a/common/kalman/tests/test_simple_kalman.py b/common/kalman/tests/test_simple_kalman.py index 630875998487dd..7b327918a3d33d 100644 --- a/common/kalman/tests/test_simple_kalman.py +++ b/common/kalman/tests/test_simple_kalman.py @@ -82,3 +82,6 @@ def test_new_is_faster(self): kf_speed = timeit.timeit("kf.update(1234)", setup=setup, number=10000) kf_old_speed = timeit.timeit("kf_old.update(1234)", setup=setup, number=10000) self.assertTrue(kf_speed < kf_old_speed / 4) + +if __name__ == "__main__": + unittest.main() diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 75ebacb3f5faa4..b104011e28ca91 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -2,7 +2,7 @@ # cython: language_level = 3 from libcpp cimport bool from libcpp.string cimport string -from params_pxd cimport Params as c_Params +from common.params_pxd cimport Params as c_Params import os import threading @@ -69,6 +69,7 @@ keys = { 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], } def ensure_bytes(v): diff --git a/common/params_pyx_setup.py b/common/params_pyx_setup.py deleted file mode 100644 index 28dd9ee50f40d4..00000000000000 --- a/common/params_pyx_setup.py +++ /dev/null @@ -1,33 +0,0 @@ -import os -import subprocess -from distutils.core import Extension, setup -from Cython.Build import cythonize - -from common.cython_hacks import BuildExtWithoutPlatformSuffix -from common.basedir import BASEDIR -from common.hardware import TICI - -ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg - -sourcefiles = ['params_pyx.pyx'] -extra_compile_args = ["-std=c++11"] - -if ARCH == "aarch64": - if TICI: - extra_compile_args += ["-DQCOM2"] - else: - extra_compile_args += ["-DQCOM"] - - -setup(name='common', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "params_pyx", - language="c++", - sources=sourcefiles, - include_dirs=[BASEDIR, os.path.join(BASEDIR, 'selfdrive')], - extra_compile_args=extra_compile_args - ) - ) -) diff --git a/common/realtime.py b/common/realtime.py index c883ec0530c11d..9f73150072a27d 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -5,7 +5,7 @@ import multiprocessing from common.hardware import PC -from common.common_pyx import sec_since_boot # pylint: disable=no-name-in-module, import-error +from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error # time step for each process diff --git a/common/spinner.py b/common/spinner.py index 53e8ee5215829d..1232371d8ffe5b 100644 --- a/common/spinner.py +++ b/common/spinner.py @@ -4,17 +4,14 @@ class Spinner(): - def __init__(self, noop=False): - # spinner is currently only implemented for android - self.spinner_proc = None - if not noop: - try: - self.spinner_proc = subprocess.Popen(["./spinner"], - stdin=subprocess.PIPE, - cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), - close_fds=True) - except OSError: - self.spinner_proc = None + def __init__(self): + try: + self.spinner_proc = subprocess.Popen(["./spinner"], + stdin=subprocess.PIPE, + cwd=os.path.join(BASEDIR, "selfdrive", "ui"), + close_fds=True) + except OSError: + self.spinner_proc = None def __enter__(self): return self diff --git a/common/text_window.py b/common/text_window.py index 1aa3b885ea2298..bea3a149f8d193 100755 --- a/common/text_window.py +++ b/common/text_window.py @@ -6,17 +6,14 @@ class TextWindow: - def __init__(self, s, noop=False): - # text window is only implemented for android currently - self.text_proc = None - if not noop: - try: - self.text_proc = subprocess.Popen(["./text", s], - stdin=subprocess.PIPE, - cwd=os.path.join(BASEDIR, "selfdrive", "ui", "text"), - close_fds=True) - except OSError: - self.text_proc = None + def __init__(self, text): + try: + self.text_proc = subprocess.Popen(["./text", text], + stdin=subprocess.PIPE, + cwd=os.path.join(BASEDIR, "selfdrive", "ui"), + close_fds=True) + except OSError: + self.text_proc = None def get_status(self): if self.text_proc is not None: diff --git a/common/transformations/SConscript b/common/transformations/SConscript index 0f729522590649..adc9642a49aa5c 100644 --- a/common/transformations/SConscript +++ b/common/transformations/SConscript @@ -1,8 +1,3 @@ -Import('env', 'cython_dependencies') +Import('envCython') -d = Dir('.') - -env.Command(['transformations.so'], - cython_dependencies + ['transformations.pxd', 'transformations.pyx', - 'coordinates.cc', 'orientation.cc', 'coordinates.hpp', 'orientation.hpp'], - 'cd ' + d.path + ' && python3 setup.py build_ext --inplace') +envCython.Program('transformations.so', 'transformations.pyx') diff --git a/common/transformations/camera.py b/common/transformations/camera.py index b406c33fd7d551..e1fbcb546022da 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,29 +1,67 @@ import numpy as np + import common.transformations.orientation as orient +from common.hardware import TICI -FULL_FRAME_SIZE = (1164, 874) -W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] -eon_focal_length = FOCAL = 910.0 +## -- hardcoded hardware params -- +eon_f_focal_length = 910.0 +eon_d_focal_length = 860.0 +leon_d_focal_length = 650.0 +tici_f_focal_length = 2648.0 +tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame -# aka 'K' aka camera_frame_from_view_frame -eon_intrinsics = np.array([ - [FOCAL, 0., W/2.], - [ 0., FOCAL, H/2.], - [ 0., 0., 1.]]) +eon_f_frame_size = (1164, 874) +eon_d_frame_size = (1152, 864) +leon_d_frame_size = (816, 612) +tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208) +# aka 'K' aka camera_frame_from_view_frame +eon_fcam_intrinsics = np.array([ + [eon_f_focal_length, 0.0, float(eon_f_frame_size[0])/2], + [0.0, eon_f_focal_length, float(eon_f_frame_size[1])/2], + [0.0, 0.0, 1.0]]) +eon_intrinsics = eon_fcam_intrinsics # xx leon_dcam_intrinsics = np.array([ - [650, 0, 816//2], - [ 0, 650, 612//2], - [ 0, 0, 1]]) + [leon_d_focal_length, 0.0, float(leon_d_frame_size[0])/2], + [0.0, leon_d_focal_length, float(leon_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) eon_dcam_intrinsics = np.array([ - [860, 0, 1152//2], - [ 0, 860, 864//2], - [ 0, 0, 1]]) + [eon_d_focal_length, 0.0, float(eon_d_frame_size[0])/2], + [0.0, eon_d_focal_length, float(eon_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_fcam_intrinsics = np.array([ + [tici_f_focal_length, 0.0, float(tici_f_frame_size[0])/2], + [0.0, tici_f_focal_length, float(tici_f_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_dcam_intrinsics = np.array([ + [tici_d_focal_length, 0.0, float(tici_d_frame_size[0])/2], + [0.0, tici_d_focal_length, float(tici_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_ecam_intrinsics = tici_dcam_intrinsics # aka 'K_inv' aka view_frame_from_camera_frame -eon_intrinsics_inv = np.linalg.inv(eon_intrinsics) +eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics) +eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx + +tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics) +tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics) + + +if not TICI: + FULL_FRAME_SIZE = eon_f_frame_size + FOCAL = eon_f_focal_length + fcam_intrinsics = eon_fcam_intrinsics +else: + FULL_FRAME_SIZE = tici_f_frame_size + FOCAL = tici_f_focal_length + fcam_intrinsics = tici_fcam_intrinsics + +W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] # device/mesh : x->forward, y-> right, z->down @@ -69,9 +107,9 @@ def vp_from_ke(m): return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0]) -def vp_from_rpy(rpy): +def vp_from_rpy(rpy, intrinsics=fcam_intrinsics): e = get_view_frame_from_road_frame(rpy[0], rpy[1], rpy[2], 1.22) - ke = np.dot(eon_intrinsics, e) + ke = np.dot(intrinsics, e) return vp_from_ke(ke) @@ -81,7 +119,7 @@ def roll_from_ke(m): -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1])) -def normalize(img_pts, intrinsics=eon_intrinsics): +def normalize(img_pts, intrinsics=fcam_intrinsics): # normalizes image coordinates # accepts single pt or array of pts intrinsics_inv = np.linalg.inv(intrinsics) @@ -94,7 +132,7 @@ def normalize(img_pts, intrinsics=eon_intrinsics): return img_pts_normalized[:, :2].reshape(input_shape) -def denormalize(img_pts, intrinsics=eon_intrinsics): +def denormalize(img_pts, intrinsics=fcam_intrinsics, width=W, height=H): # denormalizes image coordinates # accepts single pt or array of pts img_pts = np.array(img_pts) @@ -102,9 +140,9 @@ def denormalize(img_pts, intrinsics=eon_intrinsics): img_pts = np.atleast_2d(img_pts) img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1)))) img_pts_denormalized = img_pts.dot(intrinsics.T) - img_pts_denormalized[img_pts_denormalized[:, 0] > W] = np.nan + img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan - img_pts_denormalized[img_pts_denormalized[:, 1] > H] = np.nan + img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan return img_pts_denormalized[:, :2].reshape(input_shape) @@ -137,18 +175,10 @@ def img_from_device(pt_device): return pt_img.reshape(input_shape)[:, :2] -def get_camera_frame_from_calib_frame(camera_frame_from_road_frame): +def get_camera_frame_from_calib_frame(camera_frame_from_road_frame, intrinsics=fcam_intrinsics): camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)] - calib_frame_from_ground = np.dot(eon_intrinsics, + calib_frame_from_ground = np.dot(intrinsics, get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)] ground_from_calib_frame = np.linalg.inv(calib_frame_from_ground) camera_frame_from_calib_frame = np.dot(camera_frame_from_ground, ground_from_calib_frame) return camera_frame_from_calib_frame - - -def pretransform_from_calib(calib): - roll, pitch, yaw, height = calib - view_frame_from_road_frame = get_view_frame_from_road_frame(roll, pitch, yaw, height) - camera_frame_from_road_frame = np.dot(eon_intrinsics, view_frame_from_road_frame) - camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame) - return np.linalg.inv(camera_frame_from_calib_frame) diff --git a/common/transformations/model.py b/common/transformations/model.py index a3b46858b1e4ec..481240f99943b9 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -1,34 +1,33 @@ import numpy as np -from common.transformations.camera import (FULL_FRAME_SIZE, eon_focal_length, +from common.transformations.camera import (FULL_FRAME_SIZE, + FOCAL, get_view_frame_from_road_frame, get_view_frame_from_calib_frame, vp_from_ke) # segnet - SEGNET_SIZE = (512, 384) -segnet_frame_from_camera_frame = np.array([ - [float(SEGNET_SIZE[0])/FULL_FRAME_SIZE[0], 0., ], - [ 0., float(SEGNET_SIZE[1])/FULL_FRAME_SIZE[1]]]) - +def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE): + return np.array([[float(segnet_size[0]) / full_frame_size[0], 0.0], + [0.0, float(segnet_size[1]) / full_frame_size[1]]]) +segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx # model - MODEL_INPUT_SIZE = (320, 160) MODEL_YUV_SIZE = (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1] * 3 // 2) -MODEL_CX = MODEL_INPUT_SIZE[0]/2. +MODEL_CX = MODEL_INPUT_SIZE[0] / 2. MODEL_CY = 21. -model_zoom = 1.25 +model_fl = 728.0 model_height = 1.22 # canonical model transform -model_intrinsics = np.array( - [[ eon_focal_length / model_zoom, 0. , MODEL_CX], - [ 0. , eon_focal_length / model_zoom, MODEL_CY], - [ 0. , 0. , 1.]]) +model_intrinsics = np.array([ + [model_fl, 0.0, MODEL_CX], + [0.0, model_fl, MODEL_CY], + [0.0, 0.0, 1.0]]) # MED model @@ -36,34 +35,45 @@ MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2) MEDMODEL_CY = 47.6 -medmodel_zoom = 1. -medmodel_intrinsics = np.array( - [[ eon_focal_length / medmodel_zoom, 0. , 0.5 * MEDMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / medmodel_zoom, MEDMODEL_CY], - [ 0. , 0. , 1.]]) +medmodel_fl = 910.0 +medmodel_intrinsics = np.array([ + [medmodel_fl, 0.0, 0.5 * MEDMODEL_INPUT_SIZE[0]], + [0.0, medmodel_fl, MEDMODEL_CY], + [0.0, 0.0, 1.0]]) + # CAL model CALMODEL_INPUT_SIZE = (512, 256) CALMODEL_YUV_SIZE = (CALMODEL_INPUT_SIZE[0], CALMODEL_INPUT_SIZE[1] * 3 // 2) CALMODEL_CY = 47.6 -calmodel_zoom = 1.5 -calmodel_intrinsics = np.array( - [[ eon_focal_length / calmodel_zoom, 0. , 0.5 * CALMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / calmodel_zoom, CALMODEL_CY], - [ 0. , 0. , 1.]]) +calmodel_fl = 606.7 +calmodel_intrinsics = np.array([ + [calmodel_fl, 0.0, 0.5 * CALMODEL_INPUT_SIZE[0]], + [0.0, calmodel_fl, CALMODEL_CY], + [0.0, 0.0, 1.0]]) # BIG model - BIGMODEL_INPUT_SIZE = (1024, 512) BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2) -bigmodel_zoom = 1. -bigmodel_intrinsics = np.array( - [[ eon_focal_length / bigmodel_zoom, 0. , 0.5 * BIGMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / bigmodel_zoom, 256+MEDMODEL_CY], - [ 0. , 0. , 1.]]) +bigmodel_fl = 910.0 +bigmodel_intrinsics = np.array([ + [bigmodel_fl, 0.0, 0.5 * BIGMODEL_INPUT_SIZE[0]], + [0.0, bigmodel_fl, 256 + MEDMODEL_CY], + [0.0, 0.0, 1.0]]) + + +# SBIG model (big model with the size of small model) +SBIGMODEL_INPUT_SIZE = (512, 256) +SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2) + +sbigmodel_fl = 455.0 +sbigmodel_intrinsics = np.array([ + [sbigmodel_fl, 0.0, 0.5 * SBIGMODEL_INPUT_SIZE[0]], + [0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)], + [0.0, 0.0, 1.0]]) model_frame_from_road_frame = np.dot(model_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) @@ -80,20 +90,21 @@ model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics)) medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics)) + # 'camera from model camera' def get_model_height_transform(camera_frame_from_road_frame, height): camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 0], - [0, 0, 1], + [1, 0, 0], + [0, 1, 0], + [0, 0, 0], + [0, 0, 1], ])) camera_frame_from_road_high = np.dot(camera_frame_from_road_frame, np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, height - model_height], - [0, 0, 1], + [1, 0, 0], + [0, 1, 0], + [0, 0, height - model_height], + [0, 0, 1], ])) road_high_from_camera_frame = np.linalg.inv(camera_frame_from_road_high) @@ -104,13 +115,14 @@ def get_model_height_transform(camera_frame_from_road_frame, height): # camera_frame_from_model_frame aka 'warp matrix' # was: calibration.h/CalibrationTransform -def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height): +def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height, camera_fl=FOCAL): vp = vp_from_ke(camera_frame_from_road_frame) + model_zoom = camera_fl / model_fl model_camera_from_model_frame = np.array([ - [model_zoom, 0., vp[0] - MODEL_CX * model_zoom], - [ 0., model_zoom, vp[1] - MODEL_CY * model_zoom], - [ 0., 0., 1.], + [model_zoom, 0.0, vp[0] - MODEL_CX * model_zoom], + [0.0, model_zoom, vp[1] - MODEL_CY * model_zoom], + [0.0, 0.0, 1.0], ]) # This function is super slow, so skip it if height is very close to canonical diff --git a/common/transformations/setup.py b/common/transformations/setup.py deleted file mode 100644 index 32546192d4220f..00000000000000 --- a/common/transformations/setup.py +++ /dev/null @@ -1,20 +0,0 @@ -import numpy - -from Cython.Build import cythonize -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module -from common.cython_hacks import BuildExtWithoutPlatformSuffix - -setup( - name='Cython transformations wrapper', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "transformations", - sources=["transformations.pyx"], - language="c++", - extra_compile_args=["-std=c++1z", "-Wno-cpp"], - include_dirs=[numpy.get_include()], - ), - nthreads=4, - ) -) diff --git a/common/transformations/transformations.pyx b/common/transformations/transformations.pyx index 12d6e2ab58753c..ce80d90d29ed07 100644 --- a/common/transformations/transformations.pyx +++ b/common/transformations/transformations.pyx @@ -1,18 +1,20 @@ -from transformations cimport Matrix3, Vector3, Quaternion -from transformations cimport ECEF, NED, Geodetic - -from transformations cimport euler2quat as euler2quat_c -from transformations cimport quat2euler as quat2euler_c -from transformations cimport quat2rot as quat2rot_c -from transformations cimport rot2quat as rot2quat_c -from transformations cimport euler2rot as euler2rot_c -from transformations cimport rot2euler as rot2euler_c -from transformations cimport rot_matrix as rot_matrix_c -from transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c -from transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c -from transformations cimport geodetic2ecef as geodetic2ecef_c -from transformations cimport ecef2geodetic as ecef2geodetic_c -from transformations cimport LocalCoord_c +# distutils: language = c++ +# cython: language_level = 3 +from common.transformations.transformations cimport Matrix3, Vector3, Quaternion +from common.transformations.transformations cimport ECEF, NED, Geodetic + +from common.transformations.transformations cimport euler2quat as euler2quat_c +from common.transformations.transformations cimport quat2euler as quat2euler_c +from common.transformations.transformations cimport quat2rot as quat2rot_c +from common.transformations.transformations cimport rot2quat as rot2quat_c +from common.transformations.transformations cimport euler2rot as euler2rot_c +from common.transformations.transformations cimport rot2euler as rot2euler_c +from common.transformations.transformations cimport rot_matrix as rot_matrix_c +from common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c +from common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c +from common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c +from common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c +from common.transformations.transformations cimport LocalCoord_c import cython diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index e7fd3a257b4c45..482cff9cb6e2ec 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -8,6 +8,11 @@ source "$BASEDIR/launch_env.sh" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +function tici_init { + sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor' + sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu4/governor' +} + function two_init { # Restrict Android and other system processes to the first two cores echo 0-1 > /dev/cpuset/background/cpus @@ -19,15 +24,32 @@ function two_init { # openpilot gets all the cores echo 0-3 > /dev/cpuset/app/cpus + # set up governors + # +50mW offroad, +500mW onroad for 30% more RAM bandwidth + echo "performance" > /sys/class/devfreq/soc:qcom,cpubw/governor + echo 1056000 > /sys/class/devfreq/soc:qcom,m4m/max_freq + echo "performance" > /sys/class/devfreq/soc:qcom,m4m/governor + + # unclear if these help, but they don't seem to hurt + echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor + echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu2/governor + + # GPU + echo "performance" > /sys/class/devfreq/b00000.qcom,kgsl-3d0/governor + + # /sys/class/devfreq/soc:qcom,mincpubw is the only one left at "powersave" + # it seems to gain nothing but a wasted 500mW + # Collect RIL and other possibly long-running I/O interrupts onto CPU 1 echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio) echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage) echo 1 > /proc/irq/35/smp_affinity_list # wifi (wlan_pci) + echo 1 > /proc/irq/6/smp_affinity_list # MDSS + # USB traffic needs realtime handling on cpu 3 [ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco [ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T - # Check for NEOS update if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then if [ -f "$DIR/scripts/continue.sh" ]; then @@ -109,6 +131,10 @@ function launch { two_init fi + if [ -f /TICI ]; then + tici_init + fi + # handle pythonpath ln -sfn $(pwd) /data/pythonpath export PYTHONPATH="$PWD" diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 39dfb770cb6995..52128a89bcccd5 100644 Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ diff --git a/opendbc/can/common.cc b/opendbc/can/common.cc index b303391e9bf0d8..d0ace8c8f0331f 100644 --- a/opendbc/can/common.cc +++ b/opendbc/can/common.cc @@ -97,18 +97,17 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) { // a magic variable padding byte tacked onto the end of the payload. // https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf - uint8_t *dat = (uint8_t *)&d; uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR // CRC the payload first, skipping over the first byte where the CRC lives. for (int i = 1; i < l; i++) { - crc ^= dat[i]; + crc ^= (d >> (i*8)) & 0xFF; crc = crc8_lut_8h2f[crc]; } // Look up and apply the magic final CRC padding byte, which permutes by CAN // address, and additionally (for SOME addresses) by the message counter. - uint8_t counter = dat[1] & 0x0F; + uint8_t counter = ((d >> 8) & 0xFF) & 0x0F; switch(address) { case 0x86: // LWI_01 Steering Angle crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter]; @@ -176,11 +175,9 @@ unsigned int pedal_checksum(uint64_t d, int l) { d >>= ((8-l)*8); // remove padding d >>= 8; // remove checksum - uint8_t *dat = (uint8_t *)&d; - int i, j; for (i = 0; i < l - 1; i++) { - crc ^= dat[i]; + crc ^= (d >> (i*8)) & 0xFF; for (j = 0; j < 8; j++) { if ((crc & 0x80) != 0) { crc = (uint8_t)((crc << 1) ^ poly); diff --git a/opendbc/lexus_nx300_2018_pt_generated.dbc b/opendbc/lexus_nx300_2018_pt_generated.dbc new file mode 100644 index 00000000000000..a9546b7fb9644d --- /dev/null +++ b/opendbc/lexus_nx300_2018_pt_generated.dbc @@ -0,0 +1,404 @@ +CM_ "AUTOGENERATED FILE, DO NOT EDIT" + + +CM_ "Imported file _comma.dbc starts here" +BO_ 359 STEERING_IPAS_COMMA: 8 IPAS + SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX + SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX + SG_ SET_ME_X10 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; + + BO_ 512 GAS_COMMAND: 6 EON + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR + SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR + + BO_ 513 GAS_SENSOR: 6 INTERCEPTOR + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON + SG_ STATE : 39|4@0+ (1,0) [0|15] "" EON + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" EON + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" EON + + VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + + +CM_ "Imported file _toyota_2017.dbc starts here" +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: XXX DSU HCU EPS IPAS CGW + +BO_ 36 KINEMATICS: 8 XXX + SG_ ACCEL_Y : 33|10@0+ (0.03589,-18.375) [0|65535] "m/s^2" XXX + SG_ YAW_RATE : 1|10@0+ (0.244,-125) [0|65535] "deg/sec" XXX + SG_ STEERING_TORQUE : 17|10@0+ (1,-512) [0|65535] "" XXX + +BO_ 37 STEER_ANGLE_SENSOR: 8 XXX + SG_ STEER_ANGLE : 3|12@0- (1.5,0) [-500|500] "deg" XXX + SG_ STEER_FRACTION : 39|4@0- (0.1,0) [-0.7|0.7] "deg" XXX + SG_ STEER_RATE : 35|12@0- (1,0) [-2000|2000] "deg/s" XXX + +BO_ 166 BRAKE: 8 XXX + SG_ BRAKE_AMOUNT : 7|8@0+ (1,0) [0|255] "" XXX + SG_ BRAKE_PEDAL : 23|8@0+ (1,0) [0|255] "" XXX + +BO_ 170 WHEEL_SPEEDS: 8 XXX + SG_ WHEEL_SPEED_FR : 7|16@0+ (0.01,-67.67) [0|250] "kph" XXX + SG_ WHEEL_SPEED_FL : 23|16@0+ (0.01,-67.67) [0|250] "kph" XXX + SG_ WHEEL_SPEED_RR : 39|16@0+ (0.01,-67.67) [0|250] "kph" XXX + SG_ WHEEL_SPEED_RL : 55|16@0+ (0.01,-67.67) [0|250] "kph" XXX + +BO_ 180 SPEED: 8 XXX + SG_ ENCODER : 39|8@0+ (1,0) [0|255] "" XXX + SG_ SPEED : 47|16@0+ (0.01,0) [0|250] "kph" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 353 DSU_SPEED: 7 XXX + SG_ FORWARD_SPEED : 15|16@0- (0.00390625,-30) [0|255] "kph" XXX + +BO_ 452 ENGINE_RPM: 8 CGW + SG_ RPM : 7|16@0- (0.78125,0) [0|0] "rpm" SCS + +BO_ 466 PCM_CRUISE: 8 XXX + SG_ GAS_RELEASED : 4|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_ACTIVE : 5|1@0+ (1,0) [0|1] "" XXX + SG_ STANDSTILL_ON : 12|1@0+ (1,0) [0|1] "" XXX + SG_ ACCEL_NET : 23|16@0- (0.001,0) [-20|20] "m/s2" XXX + SG_ CRUISE_STATE : 55|4@0+ (1,0) [0|15] "" XXX + SG_ CANCEL_REQ : 49|1@1+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 467 PCM_CRUISE_2: 8 XXX + SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "kph" XXX + SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "kph" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 552 ACCELEROMETER: 8 XXX + SG_ ACCEL_Z : 22|15@0- (1,0) [0|32767] "" XXX + SG_ ACCEL_X : 6|15@0- (0.001,0) [-20|20] "m/s2" XXX + +BO_ 560 BRAKE_MODULE2: 7 XXX + SG_ BRAKE_PRESSED : 26|1@0+ (1,0) [0|1] "" XXX + +BO_ 614 STEERING_IPAS: 8 IPAS + SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX + SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX + SG_ SET_ME_X10 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 643 PRE_COLLISION: 7 DSU + SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ FORCE : 23|16@0- (2,0) [0|255] "N" XXX + SG_ SET_ME_X002 : 33|8@0+ (1,0) [0|3] "" XXX + SG_ BRAKE_STATUS : 39|3@0+ (1,0) [0|255] "" XXX + SG_ STATE : 36|3@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X003 : 40|1@0+ (1,0) [0|1] "" XXX + SG_ PRECOLLISION_ACTIVE : 41|1@0+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 740 STEERING_LKA: 5 XXX + SG_ LKA_STATE : 31|8@0+ (1,0) [0|255] "" XXX + SG_ STEER_REQUEST : 0|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 6|6@0+ (1,0) [0|63] "" XXX + SG_ SET_ME_1 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_TORQUE_CMD : 15|16@0- (1,0) [0|65535] "" XXX + SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX + +BO_ 742 LEAD_INFO: 8 DSU + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" HCU + SG_ LEAD_REL_SPEED : 23|12@0- (0.025,0) [-100|100] "m/s" HCU + SG_ LEAD_LONG_DIST : 7|13@0+ (0.05,0) [0|300] "m" HCU + +BO_ 835 ACC_CONTROL: 8 DSU + SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU + SG_ SET_ME_X01 : 23|2@0+ (1,0) [0|3] "" HCU + SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX + SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX + SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU + SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU + SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX + +BO_ 836 PRE_COLLISION_2: 8 DSU + SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX + +BO_ 869 DSU_CRUISE : 7 DSU + SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX + SG_ SET_BTN : 2|1@0+ (1,0) [0|0] "" XXX + SG_ CANCEL_BTN : 1|1@0+ (1,0) [0|0] "" XXX + SG_ MAIN_ON : 0|1@0+ (1,0) [0|0] "" XXX + SG_ SET_SPEED : 15|8@0+ (1,0) [0|0] "km/h" XXX + SG_ CRUISE_REQUEST : 31|8@0+ (100,-12800) [0|0] "N" XXX + SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|0] "m" XXX + +BO_ 921 PCM_CRUISE_SM: 8 XXX + SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX + SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX + +BO_ 951 ESP_CONTROL: 8 ESP + SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX + SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX + +BO_ 1041 ACC_HUD: 8 DSU + SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX + +BO_ 1042 LKAS_HUD: 8 XXX + SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX + SG_ RIGHT_LINE : 3|2@0+ (1,0) [0|3] "" XXX + SG_ LEFT_LINE : 5|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_X01 : 7|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_X01_2 : 11|2@0+ (1,0) [0|3] "" XXX + SG_ LDA_ALERT : 9|2@0+ (1,0) [0|3] "" XXX + SG_ TWO_BEEPS : 12|1@0+ (1,0) [0|1] "" XXX + SG_ ADJUSTING_CAMERA : 13|1@0+ (1,0) [0|1] "" XXX + SG_ LDA_MALFUNCTION : 15|1@0+ (1,0) [0|1] "" XXX + SG_ REPEATED_BEEPS : 32|1@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X0C : 23|8@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X2C : 47|8@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X38 : 55|8@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X02 : 63|8@0+ (1,0) [0|1] "" XXX + +BO_ 1043 TIME : 8 CGW + SG_ YEAR : 7|8@0+ (1,0) [0|0] "year" XXX + SG_ MONTH : 15|8@0+ (1,0) [0|0] "month" XXX + SG_ DAY : 23|8@0+ (1,0) [0|0] "day" XXX + SG_ HOUR : 31|8@0+ (1,0) [0|0] "hour" XXX + SG_ MINUTE : 39|8@0+ (1,0) [0|0] "minute" XXX + SG_ GMT_DIFF : 55|1@0+ (1,0) [0|0] "" XXX + SG_ GMTDIFF_HOURS : 54|4@0+ (1,0) [0|0] "hours" XXX + SG_ GMTDIFF_MINUTES : 50|6@0+ (1,0) [0|0] "minutes" XXX + SG_ SUMMER : 60|1@0+ (1,0) [0|0] "" XXX + +BO_ 1408 VIN_PART_1: 8 CGW + SG_ VIN_1 : 7|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_2 : 15|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_3 : 23|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_4 : 31|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_5 : 39|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_6 : 47|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_7 : 55|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_8 : 63|8@0+ (1,0) [0|0] "" XXX + +BO_ 1409 VIN_PART_2: 8 CGW + SG_ VIN_9 : 7|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_10 : 15|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_11 : 23|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_12 : 31|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_13 : 39|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_14 : 47|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_15 : 55|8@0+ (1,0) [0|0] "" XXX + SG_ VIN_16 : 63|8@0+ (1,0) [0|0] "" XXX + +BO_ 1410 VIN_PART_3: 8 CGW + SG_ VIN_17 : 7|8@0+ (1,0) [0|0] "" XXX + +BO_ 1553 UI_SETTING: 8 XXX + SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + +BO_ 1556 STEERING_LEVERS: 8 XXX + SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + +BO_ 1568 SEATS_DOORS: 8 XXX + SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_FL : 45|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_RL : 42|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_RR : 43|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_FR : 44|1@0+ (1,0) [0|1] "" XXX + +BO_ 1570 LIGHT_STALK: 8 SCM + SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + +BO_ 1161 RSA1: 8 FCM + SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY1 : 12|3@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT1 : 9|2@0+ (1,0) [0|0] "" XXX + SG_ SPDVAL1 : 23|8@0+ (1,0) [0|0] "kph" XXX + SG_ SPLSGN1 : 31|4@0+ (1,0) [0|0] "" XXX + SG_ SPLSGN2 : 27|4@0+ (1,0) [0|0] "" XXX + SG_ TSGN2 : 39|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY2 : 44|3@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT2 : 41|2@0+ (1,0) [0|0] "" XXX + SG_ SPDVAL2 : 55|8@0+ (1,0) [0|0] "" XXX + SG_ BZRRQ_P : 63|2@0+ (1,0) [0|0] "" XXX + SG_ BZRRQ_A : 61|2@0+ (1,0) [0|0] "" XXX + SG_ SYNCID1 : 59|4@0+ (1,0) [0|0] "" XXX + +BO_ 1162 RSA2: 8 FCM + SG_ TSGN3 : 7|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY3 : 12|3@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT3 : 9|2@0+ (1,0) [0|0] "" XXX + SG_ SPLSGN3 : 31|4@0+ (1,0) [0|0] "" XXX + SG_ SPLSGN4 : 27|4@0+ (1,0) [0|0] "" XXX + SG_ TSGN4 : 39|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY4 : 44|3@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT4 : 41|2@0+ (1,0) [0|0] "" XXX + SG_ DPSGNREQ : 54|1@0+ (1,0) [0|0] "" XXX + SG_ SGNNUMP : 53|3@0+ (1,0) [0|0] "" XXX + SG_ SGNNUMA : 50|3@0+ (1,0) [0|0] "" XXX + SG_ SPDUNT : 63|2@0+ (1,0) [0|0] "" XXX + SG_ TSRWMSG : 61|2@0+ (1,0) [0|0] "" XXX + SG_ SYNCID2 : 59|4@0+ (1,0) [0|0] "" XXX + +BO_ 1163 RSA3: 8 FCM + SG_ TSREQPD : 7|1@0+ (1,0) [0|0] "" XXX + SG_ TSRMSW : 6|1@0+ (1,0) [0|0] "" XXX + SG_ OTSGNNTM : 5|2@0+ (1,0) [0|0] "" XXX + SG_ NTLVLSPD : 3|2@0+ (1,0) [0|0] "" XXX + SG_ OVSPNTM : 1|2@0+ (1,0) [0|0] "" XXX + SG_ OVSPVALL : 11|4@0+ (1,-5) [0|0] "" XXX + SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX + SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX + SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX + +CM_ SG_ 36 ACCEL_Y "unit is tbd"; +CM_ SG_ 36 YAW_RATE "verify"; +CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; +CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; +CM_ SG_ 37 STEER_RATE "factor is tbd"; +CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; +CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; +CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; +CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque"; +CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value"; +CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active"; +CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control"; +CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control"; +CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking"; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; +CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit"; +CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; +CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces"; +CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts"; +CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; +CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70" +CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; +CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; +CM_ SG_ 1163 TSREQPD "always 1"; +CM_ SG_ 1163 TSRMSW "always 1"; +CM_ SG_ 1163 OTSGNNTM "always 3"; +CM_ SG_ 1163 NTLVLSPD "always 3"; +CM_ SG_ 1163 OVSPNTM "always 3"; +CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; +CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; +CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; +CM_ SG_ 1163 TSRSPU "always 1"; + +VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; +VAL_ 614 STATE 3 "enabled" 1 "disabled"; +VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; +VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking"; +VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted"; +VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none"; +VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none"; +VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none"; +VAL_ 1042 LEFT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none"; +VAL_ 1553 UNITS 1 "km" 2 "miles"; +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 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; + + +CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; + +CM_ "lexus_nx300_2018_pt.dbc starts here" + + + +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 + SG_ BRAKE_PRESSED : 37|1@0+ (1,0) [0|1] "" XXX + +BO_ 705 GAS_PEDAL: 8 XXX + SG_ GAS_RELEASED : 3|1@0+ (1,0) [0|1] "" XXX + SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX + +BO_ 608 STEER_TORQUE_SENSOR: 8 XXX + SG_ STEER_TORQUE_EPS : 47|16@0- (0.73,0) [-20000|20000] "" XXX + SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX + SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX + +BO_ 610 EPS_STATUS: 5 EPS + SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX + SG_ LKA_STATE : 31|7@0+ (1,0) [0|127] "" XXX + SG_ TYPE : 24|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX + +BO_ 956 GEAR_PACKET: 8 XXX + SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX + SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX + +CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force"; +CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; +CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered"; +CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; +VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; +VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; +VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; +VAL_ 956 SPORT_ON 0 "off" 1 "on"; +VAL_ 956 ECON_ON 0 "off" 1 "on"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index e09310e4bc1147..ce38bf50a5884f 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -424,14 +424,6 @@ BO_ 956 GEAR_PACKET: 8 XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX -BO_ 1653 Date_Time: 8 XXX - SG_ Year : 23|8@0+ (1,0) [0|255] "" XXX - SG_ Month : 31|8@0+ (1,0) [0|255] "" XXX - SG_ Day : 39|8@0+ (1,0) [0|255] "" XXX - SG_ Hour : 47|8@0+ (1,0) [0|255] "" XXX - SG_ Minute : 55|8@0+ (1,0) [0|255] "" XXX - SG_ Second : 63|8@0+ (1,0) [0|255] "" XXX - CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h index 96eb6268bd3dc0..91f65a246b3490 100644 --- a/panda/board/boards/uno.h +++ b/panda/board/boards/uno.h @@ -72,11 +72,9 @@ void uno_set_usb_power_mode(uint8_t mode) { bool valid = false; switch (mode) { case USB_POWER_CLIENT: - uno_set_phone_power(false); valid = true; break; case USB_POWER_CDP: - uno_set_phone_power(true); uno_bootkick(); valid = true; break; diff --git a/release/build_release2.sh b/release/build_release2.sh index 9378bb723b5d10..f47686a6be8bb2 100755 --- a/release/build_release2.sh +++ b/release/build_release2.sh @@ -35,7 +35,9 @@ else git checkout --orphan release2-staging fi -VERSION=$(cat selfdrive/common/version.h | awk -F\" '{print $2}') +VERSION=$(cat selfdrive/common/version.h | awk -F[\"-] '{print $2}') +echo "#define COMMA_VERSION \"$VERSION-release\"" > selfdrive/common/version.h + git commit -m "openpilot v$VERSION" # Build signed panda firmware diff --git a/selfdrive/assets/offroad/circled-checkmark-empty.png b/selfdrive/assets/offroad/circled-checkmark-empty.png new file mode 100644 index 00000000000000..e6740f11056bf6 Binary files /dev/null and b/selfdrive/assets/offroad/circled-checkmark-empty.png differ diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index 94e630a641ca67..f2a1f3f7bd9f2d 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,8 +1,6 @@ -Import('env', 'common', 'cereal', 'messaging', 'cython_dependencies') +Import('env', 'envCython', 'common', 'cereal', 'messaging') env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) -env.Command(['boardd_api_impl.so', 'boardd_api_impl.cpp'], - cython_dependencies + ['libcan_list_to_can_capnp.a', 'boardd_api_impl.pyx', 'boardd_setup.py'], - "cd selfdrive/boardd && python3 boardd_setup.py build_ext --inplace") +envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 7fa6d7ae3c5046..1033c082567ba9 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -56,7 +56,9 @@ struct tm get_time(){ } bool time_valid(struct tm sys_time){ - return 1900 + sys_time.tm_year >= 2019; + int year = 1900 + sys_time.tm_year; + int month = 1 + sys_time.tm_mon; + return (year > 2020) || (year == 2020 && month >= 10); } void safety_setter_thread() { @@ -107,6 +109,8 @@ void safety_setter_thread() { cereal::CarParams::Reader car_params = cmsg.getRoot(); cereal::CarParams::SafetyModel safety_model = car_params.getSafetyModel(); + panda->set_unsafe_mode(0); // see safety_declarations.h for allowed values + auto safety_param = car_params.getSafetyParam(); LOGW("setting safety model: %d with param %d", (int)safety_model, safety_param); diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py deleted file mode 100644 index 01ca052486d048..00000000000000 --- a/selfdrive/boardd/boardd_setup.py +++ /dev/null @@ -1,22 +0,0 @@ -from distutils.core import Extension, setup -from Cython.Build import cythonize - -from common.cython_hacks import BuildExtWithoutPlatformSuffix - -libraries = ['can_list_to_can_capnp', 'capnp', 'kj'] - -setup(name='Boardd API Implementation', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "boardd_api_impl", - libraries=libraries, - library_dirs=[ - './', - ], - sources=['boardd_api_impl.pyx'], - language="c++", - extra_compile_args=["-std=c++1z", "-Wno-nullability-completeness"], - ) - ) -) diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index f0d7026db7b7b4..c209cc4f8d680a 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -188,6 +188,10 @@ void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int sa usb_write(0xdc, (uint16_t)safety_model, safety_param); } +void Panda::set_unsafe_mode(uint16_t unsafe_mode) { + usb_write(0xdf, unsafe_mode, 0); +} + cereal::HealthData::HwType Panda::get_hw_type() { unsigned char hw_query[1] = {0}; @@ -279,7 +283,6 @@ const char* Panda::get_serial(){ delete[] serial_buf; return NULL; - } void Panda::set_power_saving(bool power_saving){ diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index c3e2dc981e556d..119c123b1f6f10 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -63,6 +63,7 @@ class Panda { // Panda functionality cereal::HealthData::HwType get_hw_type(); void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0); + void set_unsafe_mode(uint16_t unsafe_mode); void set_rtc(struct tm sys_time); struct tm get_rtc(); void set_fan_speed(uint16_t fan_speed); diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index e6b672ef48ab2f..2d2716a8701cee 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -11,10 +11,6 @@ if arch == "aarch64": elif arch == "larch64": libs += ['atomic'] cameras = ['cameras/camera_qcom2.cc'] - # no screen - # env = env.Clone() - # env.Append(CXXFLAGS = '-DNOSCREEN') - # env.Append(CFLAGS = '-DNOSCREEN') else: if USE_WEBCAM: libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio'] @@ -28,8 +24,9 @@ else: if arch == "Darwin": del libs[libs.index('OpenCL')] + del libs[libs.index(gpucommon)][gpucommon.index('GL')] env = env.Clone() - env['FRAMEWORKS'] = ['OpenCL'] + env['FRAMEWORKS'] = ['OpenCL', 'OpenGL'] env.SharedLibrary('snapshot/visionipc', ["#selfdrive/common/visionipc.c", "#selfdrive/common/ipc.c"]) diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 47120bcde3bf38..c389dc4973e442 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -24,6 +24,12 @@ #include "common/util.h" #include "imgproc/utils.h" +const int env_xmin = getenv("XMIN") ? atoi(getenv("XMIN")) : 0; +const int env_xmax = getenv("XMAX") ? atoi(getenv("XMAX")) : -1; +const int env_ymin = getenv("YMIN") ? atoi(getenv("YMIN")) : 0; +const int env_ymax = getenv("YMAX") ? atoi(getenv("YMAX")) : -1; +const int env_scale = getenv("SCALE") ? atoi(getenv("SCALE")) : 1; + static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b) { char args[4096]; snprintf(args, sizeof(args), @@ -164,9 +170,8 @@ bool CameraBuf::acquire() { } assert(clSetKernelArg(krnl_debayer, 2, sizeof(float), &digital_gain) == 0); const size_t debayer_work_size = rgb_height; // doesn't divide evenly, is this okay? - const size_t debayer_local_work_size = 128; assert(clEnqueueNDRangeKernel(q, krnl_debayer, 1, NULL, - &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event) == 0); + &debayer_work_size, NULL, 0, 0, &debayer_event) == 0); #endif } else { assert(cur_rgb_buf->len >= frame_size); @@ -208,7 +213,6 @@ void CameraBuf::stop() { void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt) { framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); framed.setTimestampEof(frame_data.timestamp_eof); framed.setFrameLength(frame_data.frame_length); framed.setIntegLines(frame_data.integ_lines); @@ -220,6 +224,27 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setGainFrac(frame_data.gain_frac); } +void fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, int h, int stride) { + if (dat != nullptr) { + int scale = env_scale; + int x_min = env_xmin; int y_min = env_ymin; int x_max = w-1; int y_max = h-1; + if (env_xmax != -1) x_max = env_xmax; + if (env_ymax != -1) y_max = env_ymax; + int new_width = (x_max - x_min + 1) / scale; + int new_height = (y_max - y_min + 1) / scale; + uint8_t *resized_dat = new uint8_t[new_width*new_height*3]; + + int goff = x_min*3 + y_min*stride; + for (int r=0;rbuf; @@ -283,21 +308,14 @@ void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { } } -void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { +void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { const CameraBuf *b = &c->buf; uint32_t lum_binning[256] = {0}; for (int y = y_start; y < y_end; y += y_skip) { for (int x = x_start; x < x_end; x += x_skip) { - if (!front) { - uint8_t lum = pix_ptr[(y * b->yuv_width) + x]; - lum_binning[lum]++; - } else { - // TODO: should get rid of RGB here - const uint8_t *pix = &pix_ptr[y * b->rgb_stride + x * 3]; - unsigned int lum = (unsigned int)(pix[0] + pix[1] + pix[2]); - lum_binning[std::min(lum / 3, 255u)]++; - } + uint8_t lum = pix_ptr[(y * b->yuv_width) + x]; + lum_binning[lum]++; } } @@ -403,12 +421,15 @@ void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, i y_end = 1148; skip = 4; #endif - set_exposure_target(c, (const uint8_t *)b->cur_rgb_buf->addr, 1, x_start, x_end, 2, y_start, y_end, skip); + set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x_start, x_end, 2, y_start, y_end, skip); } MessageBuilder msg; auto framed = msg.initEvent().initFrontFrame(); framed.setFrameType(cereal::FrameData::FrameType::FRONT); fill_frame_data(framed, b->cur_frame_data, cnt); + if (env_send_front) { + fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); + } pm->send("frontFrame", msg); } diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 0170db27ecbabe..fc5fa229d37520 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -34,6 +35,10 @@ #define LOG_CAMERA_ID_QCAMERA 3 #define LOG_CAMERA_ID_MAX 4 +const bool env_send_front = getenv("SEND_FRONT") != NULL; +const bool env_send_rear = getenv("SEND_REAR") != NULL; +const bool env_send_wide = getenv("SEND_WIDE") != NULL; + typedef struct CameraInfo { const char* name; int frame_width, frame_height; @@ -58,6 +63,7 @@ typedef struct LogCameraInfo { typedef struct FrameMetadata { uint32_t frame_id; + uint64_t timestamp_sof; // only set on tici uint64_t timestamp_eof; unsigned int frame_length; unsigned int integ_lines; @@ -128,8 +134,9 @@ class CameraBuf { typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt); +void fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, int h, int stride); void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr); -void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); +void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); std::thread start_process_thread(MultiCameraState *cameras, const char *tname, CameraState *cs, int priority, process_thread_cb callback); void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt); diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index d90d900cfe8bde..e0cdf7a23f24ff 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -1,4 +1,3 @@ -#include #include #include #include @@ -333,7 +332,8 @@ void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { s->rear.device = s->device; s->front.device = s->device; - s->sm = new SubMaster({"driverState", "sensorEvents"}); + s->sm_front = new SubMaster({"driverState"}); + s->sm_rear = new SubMaster({"sensorEvents"}); s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"}); int err; @@ -1399,7 +1399,7 @@ static void camera_open(CameraState *s, bool rear) { err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data); LOG("ois init settings: %d", err); } else { - // leeco actuator + // leeco actuator (DW9800W H-Bridge Driver IC) // from sniff s->infinity_dac = 364; @@ -1407,6 +1407,7 @@ static void camera_open(CameraState *s, bool rear) { { .reg_write_type = MSM_ACTUATOR_WRITE_DAC, .hw_mask = 0, + // MSB here at address 3 .reg_addr = 3, .hw_shift = 0, .data_type = 9, @@ -1417,11 +1418,14 @@ static void camera_open(CameraState *s, bool rear) { }; struct reg_settings_t actuator_init_settings[] = { - { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, - { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, - { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, - { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // PD = power down + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // 0 = power up + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // RING = SAC mode + { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // 0x40 = SAC3 mode { .reg_addr=7, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=113, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, + // 0x71 = DIV1 | DIV0 | SACT0 -- Tvib x 1/4 (quarter) + // SAC Tvib = 6.3 ms + 0.1 ms = 6.4 ms / 4 = 1.6 ms + // LSC 1-step = 252 + 1*4 = 256 ms / 4 = 64 ms }; struct region_params_t region_params[] = { @@ -2042,7 +2046,7 @@ static void* ops_thread(void* arg) { } void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) { - common_camera_process_front(s->sm, s->pm, c, cnt); + common_camera_process_front(s->sm_front, s->pm, c, cnt); } // called by processing_thread @@ -2051,11 +2055,11 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { // cache rgb roi and write to cl // gz compensation - s->sm->update(0); - if (s->sm->updated("sensorEvents")) { + s->sm_rear->update(0); + if (s->sm_rear->updated("sensorEvents")) { float vals[3] = {0.0}; bool got_accel = false; - auto sensor_events = (*(s->sm))["sensorEvents"].getSensorEvents(); + auto sensor_events = (*(s->sm_rear))["sensorEvents"].getSensorEvents(); for (auto sensor_event : sensor_events) { if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) { auto v = sensor_event.getAcceleration().getV(); @@ -2136,6 +2140,9 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { MessageBuilder msg; auto framed = msg.initEvent().initFrame(); fill_frame_data(framed, b->cur_frame_data, cnt); + if (env_send_rear) { + fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); + } framed.setFocusVal(kj::ArrayPtr(&s->rear.focus[0], NUM_FOCUS)); framed.setFocusConf(kj::ArrayPtr(&s->rear.confidence[0], NUM_FOCUS)); framed.setSharpnessScore(kj::ArrayPtr(&s->lapres[0], ARRAYSIZE(s->lapres))); @@ -2154,7 +2161,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { const int exposure_height = 314; const int skip = 1; if (cnt % 3 == 0) { - set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, 0, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip); + set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip); } } @@ -2287,6 +2294,7 @@ void cameras_close(MultiCameraState *s) { clReleaseProgram(s->prg_rgb_laplacian); clReleaseKernel(s->krnl_rgb_laplacian); - delete s->sm; + delete s->sm_front; + delete s->sm_rear; delete s->pm; } diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 49afb0f668a96b..63a4281202296b 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -142,7 +142,8 @@ typedef struct MultiCameraState { CameraState rear; CameraState front; - SubMaster *sm; + SubMaster *sm_front; + SubMaster *sm_rear; PubMaster *pm; } MultiCameraState; diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 712a08809b8b6d..4809315df25cde 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -205,6 +205,7 @@ void* visionserver_client_thread(void* arg) { stream_i == VISION_STREAM_YUV_WIDE) { CameraBuf *b = get_camerabuf_by_type(s, (VisionStreamType)stream_i); rep.d.stream_acq.extra.frame_id = b->yuv_metas[idx].frame_id; + rep.d.stream_acq.extra.timestamp_sof = b->yuv_metas[idx].timestamp_sof; rep.d.stream_acq.extra.timestamp_eof = b->yuv_metas[idx].timestamp_eof; } vipc_send(fd, &rep); diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 3df7c4c5c81091..2af6df269e0721 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -9,12 +9,11 @@ import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint -from cereal import car, log +from cereal import car EventName = car.CarEvent.EventName -HwType = log.HealthData.HwType -def get_startup_event(car_recognized, controller_available, hw_type): +def get_startup_event(car_recognized, controller_available): if comma_remote and tested_branch: event = EventName.startup else: @@ -24,8 +23,6 @@ def get_startup_event(car_recognized, controller_available, hw_type): event = EventName.startupNoCar elif car_recognized and not controller_available: event = EventName.startupNoControl - elif hw_type == HwType.greyPanda: - event = EventName.startupGreyPanda return event @@ -84,11 +81,11 @@ def only_toyota_left(candidate_cars): # **** for use live only **** -def fingerprint(logcan, sendcan, has_relay): +def fingerprint(logcan, sendcan): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) - if has_relay and not fixed_fingerprint and not skip_fw_query: + if not fixed_fingerprint and not skip_fw_query: # Vin query only reliably works thorugh OBDII bus = 1 @@ -144,8 +141,7 @@ def fingerprint(logcan, sendcan, has_relay): # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately if only_toyota_left(candidate_cars[b]): frame_fingerprint = 100 # 1s - if len(candidate_cars[b]) == 1: - if frame > frame_fingerprint: + if len(candidate_cars[b]) == 1 and frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] @@ -171,15 +167,15 @@ def fingerprint(logcan, sendcan, has_relay): return car_fingerprint, finger, vin, car_fw, source -def get_car(logcan, sendcan, has_relay=False): - candidate, fingerprints, vin, car_fw, source = fingerprint(logcan, sendcan, has_relay) +def get_car(logcan, sendcan): + candidate, fingerprints, vin, car_fw, source = fingerprint(logcan, sendcan) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" CarInterface, CarController, CarState = interfaces[candidate] - car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw) + car_params = CarInterface.get_params(candidate, fingerprints, car_fw) car_params.carVin = vin car_params.carFw = car_fw car_params.fingerprintSource = source diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index fbc469ebeb0656..e433665d992f9a 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.car.chrysler.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.chrysler.values import CAR +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -11,11 +11,11 @@ def compute_gb(accel, speed): return float(accel) / 3.0 @staticmethod - def get_params(candidate, fingerprint=None, has_relay=False, car_fw=None): + def get_params(candidate, fingerprint=None, car_fw=None): if fingerprint is None: fingerprint = gen_empty_fingerprint() - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "chrysler" ret.safetyModel = car.CarParams.SafetyModel.chrysler @@ -52,8 +52,7 @@ def get_params(candidate, fingerprint=None, has_relay=False, car_fw=None): # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay - print("ECU Camera Simulated: {0}".format(ret.enableCamera)) + ret.enableCamera = True return ret diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index b7fb53f530c61c..88110f0530a91d 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -91,8 +91,3 @@ class CAR: } STEER_THRESHOLD = 120 - - -ECU_FINGERPRINT = { - Ecu.fwdCamera: [0x292], # lkas cmd -} diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 6d6c82f0b8fa67..9def1ca86b8d8e 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -2,8 +2,8 @@ from cereal import car from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV -from selfdrive.car.ford.values import MAX_ANGLE, Ecu, ECU_FINGERPRINT, FINGERPRINTS -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.ford.values import MAX_ANGLE +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -14,8 +14,8 @@ def compute_gb(accel, speed): return float(accel) / 3.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "ford" ret.safetyModel = car.CarParams.SafetyModel.ford ret.dashcamOnly = True @@ -43,7 +43,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerControlType = car.CarParams.SteerControlType.angle - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay + ret.enableCamera = True cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) return ret diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 7c4d0aed98b2af..315c398ab69016 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -15,10 +15,6 @@ class CAR: }], } -ECU_FINGERPRINT = { - Ecu.fwdCamera: [970, 973, 984] -} - DBC = { CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'), } diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index ca53460520668e..29d5db5a9e4e04 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -127,8 +127,8 @@ def match_fw_to_car(fw_versions): if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None: continue - # TODO: COROLLA_TSS2 engine can show on two different addresses - if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR] and found_version is None: + # TODO: on some toyota, the engine can show on two different addresses + if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS] and found_version is None: continue # ignore non essential ecus diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 0fc3d278ce4161..2f7d8f473bfb1e 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,9 +1,9 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.gm.values import CAR, Ecu, ECU_FINGERPRINT, CruiseButtons, \ - AccState, FINGERPRINTS -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.gm.values import CAR, CruiseButtons, \ + AccState +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -16,8 +16,8 @@ def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "gm" ret.safetyModel = car.CarParams.SafetyModel.gm ret.enableCruise = False # stock cruise control is kept off @@ -29,7 +29,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay + ret.enableCamera = True ret.openpilotLongitudinalControl = ret.enableCamera tire_stiffness_factor = 0.444 # not optimized yet diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index efe696f8a6efeb..5e50e6166eaf16 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -70,10 +70,6 @@ class CanBus: STEER_THRESHOLD = 1.0 -ECU_FINGERPRINT = { - Ecu.fwdCamera: [384, 715] # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" -} - DBC = { CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 60f3a94e481948..b73af39a66841c 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -145,22 +145,22 @@ def update(self, enabled, CS, frame, actuators, # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, - lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl)) + lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame//10) % 4 - can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) + can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 - can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) + can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint)) elif CS.out.cruiseState.standstill: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: # Send gas and brake commands. @@ -174,7 +174,7 @@ def update(self, enabled, CS, frame, actuators, apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, - pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) + pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 56e16e6d5c3528..16d85892a621b6 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -336,7 +336,7 @@ def update(self, cp, cp_cam, cp_body): @staticmethod def get_can_parser(CP): signals, checks = get_can_signals(CP) - bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0 + bus_pt = 1 if CP.carFingerprint in HONDA_BOSCH else 0 return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt) @staticmethod @@ -361,8 +361,7 @@ def get_cam_can_parser(CP): if CP.carFingerprint in [CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: checks = [(0x194, 100)] - bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) @staticmethod def get_body_can_parser(CP): diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 45aea3fc1cf4d7..ef256470225a68 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -7,24 +7,19 @@ # 2 = ACC-CAN - camera side # 3 = F-CAN A - OBDII port -# CAN bus layout with giraffe -# 0 = F-CAN B - powertrain -# 1 = ACC-CAN - camera side -# 2 = ACC-CAN - radar side +def get_pt_bus(car_fingerprint): + return 1 if car_fingerprint in HONDA_BOSCH else 0 -def get_pt_bus(car_fingerprint, has_relay): - return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0 - -def get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled=False): +def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False): if radar_disabled: # when radar is disabled, steering commands are sent directly to powertrain bus - return get_pt_bus(car_fingerprint, has_relay) + return get_pt_bus(car_fingerprint) # normally steering commands are sent to radar, which forwards them to powertrain bus - return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0 + return 0 -def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay, stock_brake): +def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, stock_brake): # TODO: do we loose pressure if we keep pump off for long? brakelights = apply_brake > 0 brake_rq = apply_brake > 0 @@ -45,13 +40,13 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ "AEB_REQ_2": 0, "AEB_STATUS": 0, } - bus = get_pt_bus(car_fingerprint, has_relay) + bus = get_pt_bus(car_fingerprint) return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx) -def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, car_fingerprint, has_relay): +def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, car_fingerprint): commands = [] - bus = get_pt_bus(car_fingerprint, has_relay) + bus = get_pt_bus(car_fingerprint) control_on = 5 if enabled else 0 # no gas = -30000 @@ -84,31 +79,31 @@ def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, ca return commands -def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay, radar_disabled): +def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, radar_disabled): values = { "STEER_TORQUE": apply_steer if lkas_active else 0, "STEER_TORQUE_REQUEST": lkas_active, } - bus = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled) + bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled) return packer.make_can_msg("STEERING_CONTROL", bus, values, idx) -def create_bosch_supplemental_1(packer, car_fingerprint, idx, has_relay): +def create_bosch_supplemental_1(packer, car_fingerprint, idx): # non-active params values = { "SET_ME_X04": 0x04, "SET_ME_X80": 0x80, "SET_ME_X10": 0x10, } - bus = get_lkas_cmd_bus(car_fingerprint, has_relay) + bus = get_lkas_cmd_bus(car_fingerprint) return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx) -def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, openpilot_longitudinal_control, stock_hud): +def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, openpilot_longitudinal_control, stock_hud): commands = [] - bus_pt = get_pt_bus(car_fingerprint, has_relay) + bus_pt = get_pt_bus(car_fingerprint) radar_disabled = car_fingerprint in HONDA_BOSCH and openpilot_longitudinal_control - bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled) + bus_lkas = get_lkas_cmd_bus(car_fingerprint, radar_disabled) if openpilot_longitudinal_control: if car_fingerprint in HONDA_BOSCH: @@ -158,10 +153,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, return commands -def spam_buttons_command(packer, button_val, idx, car_fingerprint, has_relay): +def spam_buttons_command(packer, button_val, idx, car_fingerprint): values = { 'CRUISE_BUTTONS': button_val, 'CRUISE_SETTING': 0, } - bus = get_pt_bus(car_fingerprint, has_relay) + bus = get_pt_bus(car_fingerprint) return packer.make_can_msg("SCM_BUTTONS", bus, values, idx) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 7331c673c73303..6d474dcd85cf40 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -6,8 +6,8 @@ from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.events import ET -from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, Ecu, ECU_FINGERPRINT, FINGERPRINTS -from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH +from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING from selfdrive.car.interfaces import CarInterfaceBase @@ -119,19 +119,18 @@ def calc_accel_override(a_ego, a_target, v_ego, v_target): return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "honda" if candidate in HONDA_BOSCH: - ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe - rdr_bus = 0 if has_relay else 2 - ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay + ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness + ret.enableCamera = True ret.radarOffCan = True ret.openpilotLongitudinalControl = False else: ret.safetyModel = car.CarParams.SafetyModel.hondaNidec - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay + ret.enableCamera = True ret.enableGasInterceptor = 0x201 in fingerprint[0] ret.openpilotLongitudinalControl = ret.enableCamera @@ -253,7 +252,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 77d815301b7b95..5eca73c2d792b5 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -231,6 +231,7 @@ class CAR: b'37805-6A0-A850\x00\x00', b'37805-6A0-C540\x00\x00', b'37805-6A1-H650\x00\x00', + b'37805-6M4-B730\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-6A7-A220\x00\x00', @@ -239,6 +240,7 @@ class CAR: b'28101-6A7-A330\x00\x00', b'28101-6A7-A510\x00\x00', b'28101-6A9-H140\x00\x00', + b'28101-6A9-H420\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TVA-A230\x00\x00', @@ -249,6 +251,7 @@ class CAR: b'46114-TVA-A080\x00\x00', b'46114-TVA-A120\x00\x00', b'46114-TVE-H550\x00\x00', + b'46114-TVE-H560\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TVA-A010\x00\x00', @@ -259,6 +262,7 @@ class CAR: b'78109-TVA-C010\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', @@ -266,10 +270,12 @@ class CAR: (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TVA-A060\x00\x00', b'36161-TVE-H050\x00\x00', + b'36161-TBX-H130\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TVA-A460\x00\x00', b'77959-TVA-H230\x00\x00', + b'77959-TBX-H230\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TVA-B050\x00\x00', @@ -281,12 +287,14 @@ class CAR: b'36802-TVA-A160\x00\x00', b'36802-TVA-A170\x00\x00', b'36802-TVE-H070\x00\x00', + b'36802-TBX-H140\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TVA-A140\x00\x00', b'39990-TVA-A150\x00\x00', # Are these two different steerRatio? b'39990-TVA-A160\x00\x00', # Sport, Sport 2.0T and Touring 2.0T have different ratios b'39990-TVE-H130\x00\x00', + b'39990-TBX-H120\x00\x00', ], }, CAR.ACCORDH: { @@ -936,11 +944,23 @@ class CAR: ], }, CAR.HRV: { - (Ecu.gateway, 0x18daeff1, None): [b'38897-T7A-A010\x00\x00'], - (Ecu.eps, 0x18da30f1, None): [b'39990-THX-A020\x00\x00'], - (Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T7A-A240\x00\x00'], - (Ecu.srs, 0x18da53f1, None): [b'77959-T7A-A230\x00\x00'], - (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-THX-A210\x00\x00'], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T7A-A010\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-THX-A020\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T7A-A140\x00\x00', + b'36161-T7A-A240\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T7A-A230\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-THX-A110\x00\x00', + b'78109-THX-A210\x00\x00', + ], }, } @@ -1016,10 +1036,4 @@ class CAR: CAR.INSIGHT: 1., } -# msgs sent for steering controller by camera module on can 0. -# those messages are mutually exclusive on CRV and non-CRV cars -ECU_FINGERPRINT = { - Ecu.fwdCamera: [0xE4, 0x194], # steer torque cmd -} - HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G]) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 7364356157c670..4a296ab9cb942e 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -15,10 +15,7 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, # initialize to no line visible sys_state = 1 if left_lane and right_lane or sys_warning: # HUD alert only display when LKAS status is active - if enabled or sys_warning: - sys_state = 3 - else: - sys_state = 4 + sys_state = 3 if enabled or sys_warning else 4 elif left_lane: sys_state = 5 elif right_lane: @@ -37,14 +34,14 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, class CarController(): def __init__(self, dbc_name, CP, VM): + self.p = SteerLimitParams(CP) + self.packer = CANPacker(dbc_name) + self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint - self.packer = CANPacker(dbc_name) self.steer_rate_limited = False self.last_resume_frame = 0 - self.p = SteerLimitParams(CP) - def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart): # Steering Torque @@ -64,7 +61,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, self.apply_steer_last = apply_steer - sys_warning, sys_state, left_lane_warning, right_lane_warning =\ + sys_warning, sys_state, left_lane_warning, right_lane_warning = \ process_hud_alert(enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) @@ -83,7 +80,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, self.last_resume_frame = frame # 20 Hz LFA MFA message - if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ]: + if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) return can_sends diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 1e7b7510184ea7..81398c23da8acc 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -15,11 +15,9 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_LdwsRHWarning"] = right_lane_depart values["CR_Lkas_StrToqReq"] = apply_steer values["CF_Lkas_ActToi"] = steer_req - values["CF_Lkas_ToiFlt"] = 0 values["CF_Lkas_MsgCount"] = frame % 0x10 - values["CF_Lkas_Chksum"] = 0 - if car_fingerprint in [CAR.SONATA, CAR.PALISADE]: + if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV]: values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 76569d09156e06..4300e61d6701d4 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.hyundai.values import CAR +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @@ -12,8 +12,8 @@ def compute_gb(accel, speed): return float(accel) / 3.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "hyundai" ret.safetyModel = car.CarParams.SafetyModel.hyundai @@ -58,13 +58,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRatio = 13.75 * 1.15 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_SORENTO: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1985. + STD_CARGO_KG - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275. + STD_CARGO_KG @@ -79,44 +72,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGain = 3.5 + ret.lateralTuning.indi.outerLoopGain = 2.0 + ret.lateralTuning.indi.timeConstant = 1.4 + ret.lateralTuning.indi.actuatorEffectiveness = 2.3 ret.minSteerSpeed = 60 * CV.KPH_TO_MS - elif candidate == CAR.GENESIS_G70: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1640. + STD_CARGO_KG - ret.wheelbase = 2.84 - ret.steerRatio = 16.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - elif candidate == CAR.GENESIS_G80: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 2060. + STD_CARGO_KG - ret.wheelbase = 3.01 - ret.steerRatio = 16.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - elif candidate == CAR.GENESIS_G90: - ret.mass = 2200 - ret.wheelbase = 3.15 - ret.steerRatio = 12.069 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 3558. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 - tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - elif candidate == CAR.KIA_STINGER: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1825. + STD_CARGO_KG - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KONA: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1275. + STD_CARGO_KG @@ -142,7 +103,32 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate == CAR.KIA_FORTE: + elif candidate == CAR.VELOSTER: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 3558. * CV.LB_TO_KG + ret.wheelbase = 2.80 + ret.steerRatio = 13.75 * 1.15 + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + + # Kia + elif candidate == CAR.KIA_SORENTO: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1985. + STD_CARGO_KG + ret.wheelbase = 2.78 + ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.KIA_NIRO_EV: + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1737. + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 13.73 # Spec + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 @@ -150,18 +136,52 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - elif candidate == CAR.VELOSTER: + elif candidate == CAR.KIA_STINGER: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1825. + STD_CARGO_KG + ret.wheelbase = 2.78 + ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + + elif candidate == CAR.KIA_FORTE: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 - ret.steerRatio = 13.75 * 1.15 + ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + + # Genesis + elif candidate == CAR.GENESIS_G70: + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGain = 2.5 + ret.lateralTuning.indi.outerLoopGain = 3.5 + ret.lateralTuning.indi.timeConstant = 1.4 + ret.lateralTuning.indi.actuatorEffectiveness = 1.8 + ret.steerActuatorDelay = 0.1 + ret.mass = 1640.0 + STD_CARGO_KG + ret.wheelbase = 2.84 + ret.steerRatio = 13.56 + elif candidate == CAR.GENESIS_G80: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 2060. + STD_CARGO_KG + ret.wheelbase = 3.01 + ret.steerRatio = 16.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] + elif candidate == CAR.GENESIS_G90: + ret.mass = 2200 + ret.wheelbase = 3.15 + ret.steerRatio = 12.069 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] + # these cars require a special panda safety mode due to missing counters and checksums in the messages - if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019, - CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70]: + if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019, + CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80]: ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy ret.centerToFront = ret.wheelbase * 0.4 @@ -175,7 +195,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay + ret.enableCamera = True return ret diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 6b67e84fc715ed..8f1512c28b69b2 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -7,7 +7,7 @@ # Steer torque limits class SteerLimitParams: def __init__(self, CP): - if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE]: + if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70]: self.STEER_MAX = 384 else: self.STEER_MAX = 255 @@ -19,19 +19,12 @@ def __init__(self, CP): class CAR: + # Hyundai ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017" ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" - GENESIS_G70 = "GENESIS G70 2018" - GENESIS_G80 = "GENESIS G80 2017" - GENESIS_G90 = "GENESIS G90 2017" HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" IONIQ = "HYUNDAI IONIQ ELECTRIC PREMIUM SE 2020" IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019" - KIA_FORTE = "KIA FORTE E 2018" - KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016" - KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" - KIA_SORENTO = "KIA SORENTO GT LINE 2018" - KIA_STINGER = "KIA STINGER GT2 2018" KONA = "HYUNDAI KONA 2020" KONA_EV = "HYUNDAI KONA ELECTRIC 2019" SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" @@ -40,6 +33,19 @@ class CAR: PALISADE = "HYUNDAI PALISADE 2020" VELOSTER = "HYUNDAI VELOSTER 2019" + # Kia + KIA_FORTE = "KIA FORTE E 2018" + KIA_NIRO_EV = "KIA NIRO EV 2020" + KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016" + KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" + KIA_SORENTO = "KIA SORENTO GT LINE 2018" + KIA_STINGER = "KIA STINGER GT2 2018" + + # Genesis + GENESIS_G70 = "GENESIS G70 2018" + GENESIS_G80 = "GENESIS G80 2017" + GENESIS_G90 = "GENESIS G90 2017" + class Buttons: NONE = 0 @@ -91,7 +97,7 @@ class Buttons: {66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8}, ], CAR.KIA_OPTIMA: [{ - 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8, 1371: 8, 1397: 8, 1961: 8 }], CAR.KIA_SORENTO: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 @@ -127,7 +133,10 @@ class Buttons: 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832 : 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1193: 8, 1265: 4,1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1378: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8 }], CAR.KONA_EV: [{ - 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 1157: 4, 1193: 8, 1379: 8, 1988: 8, 1996: 8 + }], + CAR.KIA_NIRO_EV: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 516: 8, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1193: 8, 1225: 8, 1260: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 }], CAR.KIA_FORTE: [{ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1427: 6, 1456: 4, 1470: 8 @@ -146,35 +155,38 @@ class Buttons: }] } -ECU_FINGERPRINT = { - Ecu.fwdCamera: [832, 1156, 1191, 1342] -} - # Don't use these fingerprints for fingerprinting, they are still used for ECU detection IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA] FW_VERSIONS = { 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 ', ], (Ecu.esp, 0x7d1, None): [ + b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02', + b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04', ], (Ecu.engine, 0x7e0, None): [ + b'HM6M2_0a0_BD0', b'\xf1\x87391162M003\xf1\xa0000F', b'\xf1\x87391162M003\xf1\xa00240', ], (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', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422', 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', ], }, @@ -235,11 +247,13 @@ class Buttons: (Ecu.engine, 0x7e0, None): [b'\x01TJS-JNU06F200H0A', ], (Ecu.eps, 0x7d4, None): [b'\xf1\x00JSL MDPS C 1.00 1.03 56340-J3000 8308', ], (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JS LKAS AT USA LHD 1.00 1.02 95740-J3000 K32', ], - (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80', ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80', + b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\x00\x00\x00\x00', + ], }, CAR.GENESIS_G70: { (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 \xf1\xa01.02', ], - (Ecu.esp, 0x7d1, None): [b'\xf1\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7d4, None): [b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', ], (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ], @@ -253,6 +267,30 @@ class Buttons: (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ], (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ], }, + CAR.KONA_EV: { + (Ecu.esp, 0x7D1, None): [b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05', ], + (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', ], + }, + 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 \xf1\xa01.00', + ], + (Ecu.esp, 0x7D1, None): [ + b'\xf1\xa01.06', + b'\xf1\xa01.07', + ], + (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', + ], + (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', + ], + }, 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',], @@ -272,15 +310,15 @@ class Buttons: # which message has the gear "use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]), "use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_2019, CAR.VELOSTER]), - "use_elect_gears": set([CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]), + "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]), # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 - "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_FORTE, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]), + "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]), "use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA]), } -EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV]) +EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV]) DBC = { CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), @@ -292,6 +330,7 @@ class Buttons: CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None), CAR.IONIQ: dbc_dict('hyundai_kia_generic', None), CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a4dcb905c518b5..f7078ed238233e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -42,15 +42,15 @@ def compute_gb(accel, speed): raise NotImplementedError @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): raise NotImplementedError # returns a set of default params to avoid repetition in car specific params @staticmethod - def get_std_params(candidate, fingerprint, has_relay): + def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate - ret.isPandaBlack = has_relay + ret.isPandaBlack = True # TODO: deprecate this field # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 81dff12b9c016f..fc1219453b5fd4 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.mazda.values import CAR, LKAS_LIMITS, FINGERPRINTS, ECU_FINGERPRINT, Ecu -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected +from selfdrive.car.mazda.values import CAR, LKAS_LIMITS +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -15,8 +15,8 @@ def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "mazda" ret.safetyModel = car.CarParams.SafetyModel.mazda @@ -67,7 +67,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay + ret.enableCamera = True return ret diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 4e194d61d15639..81c2b2b4a43fdb 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -71,10 +71,6 @@ class Buttons: ], } -ECU_FINGERPRINT = { - Ecu.fwdCamera: [579], # steer torque cmd -} - DBC = { CAR.CX5: dbc_dict('mazda_2017', None), diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 4b703d5e90cb34..f75d1c58ffcd26 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -33,8 +33,8 @@ def compute_gb(accel, speed): return accel @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "mock" ret.safetyModel = car.CarParams.SafetyModel.noOutput ret.mass = 1700. diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 9c419c5a8f41c0..9f2ac20e0a5844 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -14,9 +14,9 @@ def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "nissan" ret.safetyModel = car.CarParams.SafetyModel.nissan diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 6d65f64f3d9688..8012624a3c914b 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -11,8 +11,8 @@ def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "subaru" ret.radarOffCan = True @@ -24,11 +24,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, # Subaru port is a community feature, since we don't own one to test ret.communityFeature = True - ret.dashcamOnly = candidate in PREGLOBAL_CARS - # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches) - # was never released ret.enableCamera = True ret.steerRateCost = 0.7 diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 8cbb6fe72dacb1..937728d88588f1 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -78,10 +78,6 @@ class CAR: CAR.OUTBACK_PREGLOBAL_2018: 75, } -ECU_FINGERPRINT = { - Ecu.fwdCamera: [290, 356], # steer torque cmd -} - DBC = { CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None), CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 069d5af00b6f25..be05c9d065cd90 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -25,36 +25,35 @@ def test_car_interfaces(self): car_fw = [] - for has_relay in [True, False]: - car_params = CarInterface.get_params(car_name, fingerprints, has_relay, 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) - 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.assertGreater(car_params.lateralTuning.indi.outerLoopGain, 1e-3) + 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.assertGreater(car_params.lateralTuning.indi.outerLoopGain, 1e-3) - # 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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 2f8c0bc9a60e69..1fa84f9155f062 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -14,8 +14,8 @@ def compute_gb(accel, speed): return float(accel) / 3.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "toyota" ret.safetyModel = car.CarParams.SafetyModel.toyota @@ -243,7 +243,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00007 - elif candidate == CAR.LEXUS_NXH: + elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.66 @@ -253,6 +253,16 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 + elif candidate == CAR.PRIUS_TSS2: + stop_and_go = True + ret.safetyParam = 73 + ret.wheelbase = 2.70002 #from toyota online sepc. + ret.steerRatio = 13.4 # True steerRation from older prius + tire_stiffness_factor = 0.6371 # hand-tune + ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.15]] + ret.lateralTuning.pid.kf = 0.00007818594 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 @@ -265,7 +275,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay + ret.enableCamera = True # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it smartDsu = 0x2FF in fingerprint[0] # In TSS2 cars the camera does long control @@ -317,8 +327,6 @@ def update(self, c, can_strings): # events events = self.create_common_events(ret) - if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera and not self.CP.isPandaBlack: - events.add(EventName.invalidGiraffeToyota) if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: events.add(EventName.lowSpeedLockout) if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 89b27737e61b77..924e02efd137df 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -13,6 +13,7 @@ class SteerLimitParams: class CAR: PRIUS = "TOYOTA PRIUS 2017" + PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021" RAV4H = "TOYOTA RAV4 HYBRID 2017" RAV4 = "TOYOTA RAV4 2017" COROLLA = "TOYOTA COROLLA 2017" @@ -39,28 +40,28 @@ class CAR: LEXUS_CTH = "LEXUS CT 200H 2018" RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019" LEXUS_NXH = "LEXUS NX300H 2018" - + LEXUS_NX = "LEXUS NX300 2018" # addr: (ecu, cars, bus, 1/freq*100, vl) STATIC_MSGS = [ - (0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), + (0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), (0x128, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), - (0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'), - (0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), + (0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'), + (0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), (0X161, Ecu.dsu, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), (0x2E6, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), (0x2E7, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), (0x33E, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), - (0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), + (0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), (0x365, Ecu.dsu, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), - (0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), + (0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), (0x366, Ecu.dsu, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), (0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), (0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'), - (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), + (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_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), ] ECU_FINGERPRINT = { @@ -265,11 +266,17 @@ class CAR: }], CAR.LEXUS_NXH: [{ 36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 822: 8, 824: 8, 835: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 889: 8, 891: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 987: 8, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 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, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 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, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], + 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.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 }] } # 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] +IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, CAR.LEXUS_NX] FW_VERSIONS = { CAR.AVALON: { @@ -300,6 +307,7 @@ class CAR: b'\x018966306Q3100\x00\x00\x00\x00', b'\x018966306Q4000\x00\x00\x00\x00', b'\x018966306Q4100\x00\x00\x00\x00', + b'\x018966306Q4200\x00\x00\x00\x00', b'\x018966333P3100\x00\x00\x00\x00', b'\x018966333P3200\x00\x00\x00\x00', b'\x018966333P4200\x00\x00\x00\x00', @@ -309,6 +317,7 @@ class CAR: b'\x018966333P4700\x00\x00\x00\x00', b'\x018966333Q6000\x00\x00\x00\x00', b'\x018966333Q6200\x00\x00\x00\x00', + b'\x018966333Q6300\x00\x00\x00\x00', b'\x018966333W6000\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ @@ -317,6 +326,7 @@ class CAR: b'8821F0603300 ', b'8821F0607200 ', b'8821F0608000 ', + b'8821F0608200 ', b'8821F0609100 ', ], (Ecu.esp, 0x7b0, None): [ @@ -330,6 +340,7 @@ class CAR: b'8965B33540\x00\x00\x00\x00\x00\x00', b'8965B33542\x00\x00\x00\x00\x00\x00', b'8965B33580\x00\x00\x00\x00\x00\x00', + b'8965B33581\x00\x00\x00\x00\x00\x00', b'8965B33621\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 @@ -338,6 +349,7 @@ class CAR: b'8821F0603300 ', b'8821F0607200 ', b'8821F0608000 ', + b'8821F0608200 ', b'8821F0609100 ', ], (Ecu.fwdCamera, 0x750, 0x6d): [ @@ -554,8 +566,10 @@ class CAR: 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', @@ -680,6 +694,7 @@ class CAR: (Ecu.engine, 0x7e0, None): [ b'\x0230E40000\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', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702100\x00\x00\x00\x00', @@ -731,23 +746,33 @@ class CAR: (Ecu.engine, 0x700, None): [ b'\x018966353M7100\x00\x00\x00\x00', b'\x018966353Q2300\x00\x00\x00\x00', + b'\x018966353R1100\x00\x00\x00\x00', b'\x018966353R8100\x00\x00\x00\x00', ], + (Ecu.engine, 0x7e0, None): [ + b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\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', ], (Ecu.dsu, 0x791, None): [ b'881515306400\x00\x00\x00\x00', b'881515306500\x00\x00\x00\x00', + b'881515306200\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B53271\x00\x00\x00\x00\x00\x00', + b'8965B53280\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702300\x00\x00\x00\x00', + b'8821F4702100\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F5301300\x00\x00\x00\x00', b'8646F5301400\x00\x00\x00\x00', + b'8646F5301200\x00\x00\x00\x00', ], }, CAR.PRIUS: { @@ -923,9 +948,11 @@ class CAR: b'\x01896634A19100\x00\x00\x00\x00', b'\x01896634A20000\x00\x00\x00\x00', b'\x01896634A22000\x00\x00\x00\x00', + b'\x01896634A45000\x00\x00\x00\x00', b'\x01896634A46000\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\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', ], @@ -943,6 +970,7 @@ class CAR: (Ecu.eps, 0x7a1, None): [ b'8965B42170\x00\x00\x00\x00\x00\x00', b'8965B42171\x00\x00\x00\x00\x00\x00', + b'8965B42180\x00\x00\x00\x00\x00\x00', b'8965B42181\x00\x00\x00\x00\x00\x00', b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00', @@ -963,6 +991,7 @@ class CAR: }, CAR.RAV4H_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x01896634A15000\x00\x00\x00\x00', b'\x018966342M5000\x00\x00\x00\x00', b'\x018966342X6000\x00\x00\x00\x00', b'\x018966342W8000\x00\x00\x00\x00', @@ -978,7 +1007,9 @@ class CAR: b'F152642531\x00\x00\x00\x00\x00\x00', b'F152642532\x00\x00\x00\x00\x00\x00', b'F152642521\x00\x00\x00\x00\x00\x00', + b'F152642540\x00\x00\x00\x00\x00\x00', b'F152642541\x00\x00\x00\x00\x00\x00', + b'F152642542\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B42170\x00\x00\x00\x00\x00\x00', @@ -1068,9 +1099,30 @@ class CAR: ], (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', ], }, + CAR.LEXUS_NX: { + (Ecu.engine, 0x700, None): [ + b'\x01896637851000\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152678140\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881517803100\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B78060\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F7801100\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', @@ -1219,6 +1271,23 @@ class CAR: b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', ], }, + CAR.PRIUS_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152647520\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B47070\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4707000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, } STEER_THRESHOLD = 100 @@ -1251,8 +1320,10 @@ class CAR: CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), 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.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), } NO_DSU_CAR = set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]) -TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]) -NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]) # no resume button press required +TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2]) +NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2]) # no resume button press required diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index c041c6c44cfa3f..252306322bf56c 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -18,8 +18,8 @@ def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) # VW port is a community feature, since we don't own one to test ret.communityFeature = True diff --git a/selfdrive/common/clutil.c b/selfdrive/common/clutil.c index 00d63cdcacee97..dbfc55453f26f3 100644 --- a/selfdrive/common/clutil.c +++ b/selfdrive/common/clutil.c @@ -39,7 +39,7 @@ void clu_init(void) { cl_device_id cl_get_device_id(cl_device_type device_type) { bool opencl_platform_found = false; cl_device_id device_id = NULL; - + cl_uint num_platforms = 0; int err = clGetPlatformIDs(0, NULL, &num_platforms); assert(err == 0); @@ -66,7 +66,7 @@ cl_device_id cl_get_device_id(cl_device_type device_type) { break; } free(platform_ids); - + if (!opencl_platform_found) { printf("No valid openCL platform found\n"); assert(opencl_platform_found); diff --git a/selfdrive/common/i2c.cc b/selfdrive/common/i2c.cc index a37b144dc297a1..95d0aedd5ef4ed 100644 --- a/selfdrive/common/i2c.cc +++ b/selfdrive/common/i2c.cc @@ -12,7 +12,10 @@ #ifdef QCOM2 // TODO: decide if we want to isntall libi2c-dev everywhere -#include +extern "C" { + #include + #include +} I2CBus::I2CBus(uint8_t bus_id){ char bus_name[20]; diff --git a/selfdrive/common/mat.h b/selfdrive/common/mat.h index 1c20eae17fd034..626f3404feab49 100644 --- a/selfdrive/common/mat.h +++ b/selfdrive/common/mat.h @@ -1,5 +1,4 @@ -#ifndef COMMON_MAT_H -#define COMMON_MAT_H +#pragma once typedef struct vec3 { float v[3]; @@ -17,7 +16,7 @@ typedef struct mat4 { float v[4*4]; } mat4; -static inline mat3 matmul3(const mat3 a, const mat3 b) { +static inline mat3 matmul3(const mat3 &a, const mat3 &b) { mat3 ret = {{0.0}}; for (int r=0; r<3; r++) { for (int c=0; c<3; c++) { @@ -31,7 +30,7 @@ static inline mat3 matmul3(const mat3 a, const mat3 b) { return ret; } -static inline vec3 matvecmul3(const mat3 a, const vec3 b) { +static inline vec3 matvecmul3(const mat3 &a, const vec3 &b) { vec3 ret = {{0.0}}; for (int r=0; r<3; r++) { for (int c=0; c<3; c++) { @@ -41,7 +40,7 @@ static inline vec3 matvecmul3(const mat3 a, const vec3 b) { return ret; } -static inline mat4 matmul(const mat4 a, const mat4 b) { +static inline mat4 matmul(const mat4 &a, const mat4 &b) { mat4 ret = {{0.0}}; for (int r=0; r<4; r++) { for (int c=0; c<4; c++) { @@ -55,7 +54,7 @@ static inline mat4 matmul(const mat4 a, const mat4 b) { return ret; } -static inline vec4 matvecmul(const mat4 a, const vec4 b) { +static inline vec4 matvecmul(const mat4 &a, const vec4 &b) { vec4 ret = {{0.0}}; for (int r=0; r<4; r++) { for (int c=0; c<4; c++) { @@ -67,7 +66,7 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) { // scales the input and output space of a transformation matrix // that assumes pixel-center origin. -static inline mat3 transform_scale_buffer(const mat3 in, float s) { +static inline mat3 transform_scale_buffer(const mat3 &in, float s) { // in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s mat3 transform_out = {{ @@ -84,5 +83,3 @@ static inline mat3 transform_scale_buffer(const mat3 in, float s) { return matmul3(transform_in, matmul3(in, transform_out)); } - -#endif diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 77f4a1b7a17155..abd47a7836790b 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,6 +1,9 @@ #pragma once #define MODEL_PATH_DISTANCE 192 +#define TRAJECTORY_SIZE 33 +#define MIN_DRAW_DISTANCE 10.0 +#define MAX_DRAW_DISTANCE 100.0 #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 #define DESIRE_PRED_SIZE 32 diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index c89144d4969fab..da799d71d7a46f 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -49,30 +49,17 @@ void params_sig_handler(int signal) { } static int fsync_dir(const char* path){ - int result = 0; int fd = open(path, O_RDONLY, 0755); - if (fd < 0){ - result = -1; - goto cleanup; - } - - result = fsync(fd); - if (result < 0) { - goto cleanup; - } - - cleanup: - int result_close = 0; - if (fd >= 0){ - result_close = close(fd); + return -1; } + int result = fsync(fd); + int result_close = close(fd); if (result_close < 0) { - return result_close; - } else { - return result; + result = result_close; } + return result; } static int mkdir_p(std::string path) { @@ -237,10 +224,6 @@ int Params::write_db_value(const char* key, const char* value, size_t value_size } int Params::delete_db_value(std::string key) { - return delete_db_value(key.c_str()); -} - -int Params::delete_db_value(const char* key) { int lock_fd = -1; int result; std::string path; @@ -256,7 +239,7 @@ int Params::delete_db_value(const char* key) { } // Delete value. - path = params_path + "/d/" + std::string(key); + path = params_path + "/d/" + key; result = remove(path.c_str()); if (result != 0) { result = ERR_NO_VALUE; diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index d8c96a8cb2b348..8da077cfdc3834 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -33,7 +33,6 @@ class Params { // Delete a value from the params database. // Inputs are the same as read_db_value, without value and value_sz. int delete_db_value(std::string key); - int delete_db_value(const char* key); // Reads a value from the params database, blocking until successful. // Inputs are the same as read_db_value. diff --git a/selfdrive/common/spinner.h b/selfdrive/common/spinner.h deleted file mode 100644 index fd35dcc7d48c31..00000000000000 --- a/selfdrive/common/spinner.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef COMMON_SPINNER_H -#define COMMON_SPINNER_H - -#ifdef __cplusplus -extern "C" { -#endif - -int spin(int argc, char** argv); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index fb8618cd8a46cf..e50f4f6698762d 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.7.10-release" +#define COMMA_VERSION "0.8.0-ce8a040c-2020-11-24T13:40:02" diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h index 1b216f52555303..e20500db6b14c1 100644 --- a/selfdrive/common/visionipc.h +++ b/selfdrive/common/visionipc.h @@ -57,6 +57,7 @@ typedef struct VisionStreamBufs { typedef struct VIPCBufExtra { // only for yuv uint32_t frame_id; + uint64_t timestamp_sof; uint64_t timestamp_eof; } VIPCBufExtra; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a9149806b2312c..22e63a798b598a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -52,7 +52,7 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.sm = sm if self.sm is None: - self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', + self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'frontFrame', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) self.can_sock = can_sock @@ -61,12 +61,10 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.can_sock = messaging.sub_sock('can', timeout=can_timeout) # wait for one health and one CAN packet - hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType - has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos] print("Waiting for CAN messages...") get_one_can(self.can_sock) - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay) + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) # read params params = Params() @@ -131,7 +129,7 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.sm['dMonitoringState'].awarenessStatus = 1. self.sm['dMonitoringState'].faceDetected = False - self.startup_event = get_startup_event(car_recognized, controller_available, hw_type) + self.startup_event = get_startup_event(car_recognized, controller_available) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) @@ -141,8 +139,6 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.events.add(EventName.communityFeatureDisallowed, static=True) if not car_recognized: self.events.add(EventName.carUnrecognized, static=True) - if hw_type == HwType.whitePanda: - self.events.add(EventName.whitePandaUnsupported, static=True) # controlsd is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) @@ -205,7 +201,8 @@ def update_events(self, CS): if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL): self.events.add(EventName.canError) - if self.mismatch_counter >= 200: + if (self.sm['health'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \ + self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) if not self.sm.alive['plan'] and self.sm.alive['pathPlan']: # only plan not being received: radar not communicating @@ -235,8 +232,11 @@ def update_events(self, CS): self.events.add(EventName.relayMalfunction) if self.sm['plan'].fcw: self.events.add(EventName.fcw) - if self.sm['model'].frameDropPerc > 1 and (not SIMULATION): - self.events.add(EventName.modeldLagging) + if not self.sm.alive['frontFrame'] and (self.sm.frame > 5 / DT_CTRL) and not SIMULATION: + self.events.add(EventName.cameraMalfunction) + + if self.sm['model'].frameDropPerc > 20 and not SIMULATION: + self.events.add(EventName.modeldLagging) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ @@ -456,7 +456,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \ - (self.state == State.softDisabling) + (self.state == State.softDisabling) steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index f343f4cd5e7b04..a47eb8c0a22fa8 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -36,5 +36,9 @@ "Offroad_NeosUpdate": { "text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.", "severity": 0 + }, + "Offroad_HardwareUnsupported": { + "text": "White and grey panda are unsupported. Upgrade to comma two or black panda.", + "severity": 0 } } diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 0c094d40a1c8e5..d0b10ec84e1c5d 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -252,31 +252,6 @@ def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: boo Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), }, - EventName.startupGreyPanda: { - ET.PERMANENT: Alert( - "WARNING: Grey panda is deprecated", - "Upgrade to comma two or black panda", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), - }, - - EventName.invalidGiraffeToyota: { - ET.PERMANENT: Alert( - "Unsupported Giraffe Configuration", - "Visit comma.ai/tg", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - }, - - EventName.whitePandaUnsupported: { - ET.PERMANENT: Alert( - "White Panda No Longer Supported", - "Upgrade to comma two or black panda", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - ET.NO_ENTRY: NoEntryAlert("Unsupported Hardware"), - }, - EventName.invalidLkasSetting: { ET.PERMANENT: Alert( "Stock LKAS is turned on", @@ -480,6 +455,10 @@ def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: boo ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Contact Support"), }, + EventName.cameraMalfunction: { + ET.PERMANENT: NormalPermanentAlert("Camera Malfunction", "Contact Support"), + }, + # ********** events that affect controls state transitions ********** EventName.pcmEnable: { diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 5b9485a29a4ee6..789ef8571fa036 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -5,9 +5,9 @@ CAMERA_OFFSET = 0.06 # m from center car to camera -def compute_path_pinv(l=50): +def compute_path_pinv(length=50): deg = 3 - x = np.arange(l*1.0) + x = np.arange(length*1.0) X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T pinv = np.linalg.pinv(X) return pinv @@ -21,31 +21,7 @@ def eval_poly(poly, x): return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3 -def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego): - # This will improve behaviour when lanes suddenly widen - # these numbers were tested on 2000segments and found to work well - lane_width = min(4.0, lane_width) - width_poly = l_poly - r_poly - prob_mods = [] - for t_check in [0.0, 1.5, 3.0]: - width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) - prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) - mod = min(prob_mods) - l_prob = mod * l_prob - r_prob = mod * r_prob - - path_from_left_lane = l_poly.copy() - path_from_left_lane[3] -= lane_width / 2.0 - path_from_right_lane = r_poly.copy() - path_from_right_lane[3] += lane_width / 2.0 - - lr_prob = l_prob + r_prob - l_prob * r_prob - - d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) - return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly - - -class LanePlanner(): +class LanePlanner: def __init__(self): self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] @@ -59,6 +35,9 @@ def __init__(self): self.l_prob = 0. self.r_prob = 0. + self.l_std = 0. + self.r_std = 0. + self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. @@ -68,7 +47,9 @@ def __init__(self): def parse_model(self, md): if len(md.leftLane.poly): self.l_poly = np.array(md.leftLane.poly) + self.l_std = float(md.leftLane.std) self.r_poly = np.array(md.rightLane.poly) + self.r_std = float(md.rightLane.std) self.p_poly = np.array(md.path.poly) else: self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line @@ -78,24 +59,47 @@ def parse_model(self, md): self.r_prob = md.rightLane.prob # right line prob if len(md.meta.desireState): - self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1] - self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1] + self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft] + self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight] def update_d_poly(self, v_ego): # only offset left and right lane lines; offsetting p_poly does not make sense self.l_poly[3] += CAMERA_OFFSET self.r_poly[3] += CAMERA_OFFSET + # Reduce reliance on lanelines that are too far apart or + # will be in a few seconds + l_prob, r_prob = self.l_prob, self.r_prob + width_poly = self.l_poly - self.r_poly + prob_mods = [] + for t_check in [0.0, 1.5, 3.0]: + width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) + prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) + mod = min(prob_mods) + l_prob *= mod + r_prob *= mod + + # Reduce reliance on uncertain lanelines + l_std_mod = interp(self.l_std, [.15, .3], [1.0, 0.0]) + r_std_mod = interp(self.r_std, [.15, .3], [1.0, 0.0]) + l_prob *= l_std_mod + r_prob *= r_std_mod + # Find current lanewidth - self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty) + self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty) current_lane_width = abs(self.l_poly[3] - self.r_poly[3]) self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5]) self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ (1 - self.lane_width_certainty) * speed_lane_width - self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego) + clipped_lane_width = min(4.0, self.lane_width) + path_from_left_lane = self.l_poly.copy() + path_from_left_lane[3] -= clipped_lane_width / 2.0 + path_from_right_lane = self.r_poly.copy() + path_from_right_lane[3] += clipped_lane_width / 2.0 + + lr_prob = l_prob + r_prob - l_prob * r_prob - def update(self, v_ego, md): - self.parse_model(md) - self.update_d_poly(v_ego) + d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) + self.d_poly = lr_prob * d_poly_lane + (1.0 - lr_prob) * self.p_poly.copy() diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 594147e11d8d28..a58eae0054ddf5 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -43,6 +43,7 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): states[0].x = v_ego * delay states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay + states[0].y = states[0].x * math.sin(states[0].psi / 2) return states diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py index 2695d457d19f72..6c38957be32032 100755 --- a/selfdrive/debug/get_fingerprint.py +++ b/selfdrive/debug/get_fingerprint.py @@ -6,7 +6,6 @@ # - connect to a Panda # - run selfdrive/boardd/boardd # - launching this script -# - turn on the car in STOCK MODE (set giraffe switches properly). # Note: it's very important that the car is in stock mode, in order to collect a complete fingerprint # - since some messages are published at low frequency, keep this script running for at least 30s, # until all messages are received at least once diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index f0ffabdf72dad9..90346d524190d3 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -37,7 +37,7 @@ for route in tqdm(routes): route = route.rstrip() dongle_id, time = route.split('|') - qlog_path = f"cd:/{dongle_id}/{time}/1/qlog.bz2" + qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2" if dongle_id in dongles: continue @@ -65,7 +65,7 @@ live_fingerprint = args.car if live_fingerprint not in list(TOYOTA_FINGERPRINTS.keys()) + list(HONDA_FINGERPRINTS.keys()) + list(HYUNDAI_FINGERPRINTS.keys()): - continue + break candidates = match_fw_to_car(car_fw) if (len(candidates) == 1) and (list(candidates)[0] == live_fingerprint): diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index b6acc12092cc92..9026fc3394bd02 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -196,14 +196,14 @@ def handle_gps(self, current_time, log): orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef) orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)]) 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") - initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps)) - self.reset_kalman(init_orient=initial_pose_ecef_quat) + 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() + 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) @@ -267,12 +267,14 @@ def handle_live_calib(self, current_time, log): self.calib_from_device = self.device_from_calib.T self.calibrated = log.calStatus == 1 - def reset_kalman(self, current_time=None, init_orient=None): + 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 = [] diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 3a9e64e46ce473..6360a555d9a5ab 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -67,7 +67,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { .stream_type = VISION_STREAM_YUV, .filename = "fcamera.hevc", .frame_packet_name = "frame", - .encode_idx_name = "encodeIdx", .fps = MAIN_FPS, .bitrate = MAIN_BITRATE, .is_h265 = true, @@ -78,7 +77,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { .stream_type = VISION_STREAM_YUV_FRONT, .filename = "dcamera.hevc", .frame_packet_name = "frontFrame", - .encode_idx_name = "frontEncodeIdx", .fps = MAIN_FPS, // on EONs, more compressed this way .bitrate = DCAM_BITRATE, .is_h265 = true, @@ -89,7 +87,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { .stream_type = VISION_STREAM_YUV_WIDE, .filename = "ecamera.hevc", .frame_packet_name = "wideFrame", - .encode_idx_name = "wideEncodeIdx", .fps = MAIN_FPS, .bitrate = MAIN_BITRATE, .is_h265 = true, @@ -235,9 +232,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { s.num_encoder += 1; pthread_mutex_unlock(&s.rotate_lock); - PubSocket *idx_sock = PubSocket::create(s.ctx, cameras_logged[cam_idx].encode_idx_name); - assert(idx_sock != NULL); - LoggerHandle *lh = NULL; while (!do_exit) { @@ -377,8 +371,12 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { // publish encode index MessageBuilder msg; - auto eidx = msg.initEvent().initEncodeIdx(); + // this is really ugly + auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() : + (cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx()); 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 @@ -390,10 +388,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { eidx.setSegmentId(out_id); auto bytes = msg.toBytes(); - - if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) { - printf("err sending encodeIdx pkt: %s\n", strerror(errno)); - } if (lh) { lh_log(lh, bytes.begin(), bytes.size(), false); } @@ -453,8 +447,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { visionstream_destroy(&stream); } - delete idx_sock; - if (encoder_inited) { LOG("encoder destroy"); encoder_close(&encoder); @@ -606,6 +598,10 @@ static void bootlog() { int main(int argc, char** argv) { int err; +#ifdef QCOM + setpriority(PRIO_PROCESS, 0, -12); +#endif + if (argc > 1 && strcmp(argv[1], "--bootlog") == 0) { bootlog(); return 0; @@ -620,8 +616,6 @@ int main(int argc, char** argv) { record_front = Params().read_db_bool("RecordFront"); #endif - setpriority(PRIO_PROCESS, 0, -12); - clear_locks(); signal(SIGINT, (sighandler_t)set_do_exit); @@ -751,7 +745,7 @@ int main(int argc, char** argv) { if (s.logger.part > -1) { new_segment = true; - if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE) { + if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE && s.num_encoder > 0) { for (int cid=0;cid<=MAX_CAM_IDX;cid++) { // this *should* be redundant on tici since all camera frames are synced new_segment &= (((s.rotate_state[cid].stream_frame_id >= s.rotate_state[cid].last_rotate_frame_id + segment_length * MAIN_FPS) && diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 879d9aa32ed258..1775c6388b7e32 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -78,8 +78,10 @@ def unblock_stdout(): from multiprocessing import Process # Run scons -spinner = Spinner(noop=(__name__ != "__main__" or not ANDROID)) +spinner = Spinner() spinner.update("0") +if __name__ != "__main__": + spinner.close() if not prebuilt: for retry in [True, False]: @@ -125,7 +127,8 @@ def unblock_stdout(): print("....%d" % i) time.sleep(1) subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) - shutil.rmtree("/tmp/scons_cache") + shutil.rmtree("/tmp/scons_cache", ignore_errors=True) + shutil.rmtree("/data/scons_cache", ignore_errors=True) else: print("scons build failed after retry") sys.exit(1) @@ -138,9 +141,8 @@ def unblock_stdout(): cloudlog.error("scons build failed\n" + error_s) # Show TextWindow - no_ui = __name__ != "__main__" or not ANDROID error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors]) - with TextWindow("openpilot failed to build\n \n" + error_s, noop=no_ui) as t: + with TextWindow("openpilot failed to build\n \n" + error_s) as t: t.wait_for_exit() exit(1) @@ -371,11 +373,8 @@ def kill_managed_process(name): join_process(running[name], 15) if running[name].exitcode is None: cloudlog.critical("unkillable process %s failed to die!" % name) - # TODO: Use method from HARDWARE - if ANDROID: - cloudlog.critical("FORCE REBOOTING PHONE!") - os.system("date >> /sdcard/unkillable_reboot") - os.system("reboot") + os.system("date >> /sdcard/unkillable_reboot") + HARDWARE.reboot() raise RuntimeError else: cloudlog.info("killing %s with SIGKILL" % name) @@ -398,8 +397,10 @@ def cleanup_all_processes(signal, frame): def send_managed_process_signal(name, sig): - if name not in running or name not in managed_processes: + if name not in running or name not in managed_processes or \ + running[name].exitcode is not None: return + cloudlog.info(f"sending signal {sig} to {name}") os.kill(running[name].pid, sig) diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 18f279bf785e01..569bcf0e75b311 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -6,10 +6,10 @@ libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'z TEST_THNEED = False common_src = [ - "models/commonmodel.c", + "models/commonmodel.cc", "runners/snpemodel.cc", "transforms/loadyuv.c", - "transforms/transform.c" + "transforms/transform.cc" ] if arch == "aarch64": @@ -19,16 +19,20 @@ if arch == "aarch64": lenv['CFLAGS'].append("-DUSE_THNEED") lenv['CXXFLAGS'].append("-DUSE_THNEED") elif arch == "larch64": - libs += ['gsl', 'CB', 'pthread'] + libs += ['gsl', 'CB', 'pthread', 'dl'] + if not TEST_THNEED: + common_src += ["thneed/thneed.cc"] + lenv['CFLAGS'].append("-DUSE_THNEED") + lenv['CXXFLAGS'].append("-DUSE_THNEED") else: libs += ['pthread'] - # for tensorflow support - common_src += ['runners/tfmodel.cc'] + # for onnx support + common_src += ['runners/onnxmodel.cc'] - # tell runners to use tensorflow - lenv['CFLAGS'].append("-DUSE_TF_MODEL") - lenv['CXXFLAGS'].append("-DUSE_TF_MODEL") + # tell runners to use onnx + lenv['CFLAGS'].append("-DUSE_ONNX_MODEL") + lenv['CXXFLAGS'].append("-DUSE_ONNX_MODEL") if arch == "Darwin": # fix OpenCL diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 4eeb292ef584a9..3a9b134effc1e5 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include "common/visionbuf.h" #include "common/visionipc.h" @@ -23,7 +24,7 @@ static void set_do_exit(int sig) { int main(int argc, char **argv) { int err; - set_realtime_priority(51); + setpriority(PRIO_PROCESS, 0, -15); #ifdef QCOM2 set_core_affinity(5); @@ -65,7 +66,7 @@ int main(int argc, char **argv) { double t2 = millis_since_boot(); // send dm packet - dmonitoring_publish(pm, extra.frame_id, res); + dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0); LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); last = t1; diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index c8c637b3f25eae..ffb3770d22bf4a 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -38,16 +38,15 @@ void* live_thread(void *arg) { -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; + Eigen::Matrix fcam_intrinsics; #ifndef QCOM2 - Eigen::Matrix eon_intrinsics; - eon_intrinsics << + fcam_intrinsics << 910.0, 0.0, 582.0, 0.0, 910.0, 437.0, 0.0, 0.0, 1.0; float db_s = 0.5; // debayering does a 2x downscale #else - Eigen::Matrix eon_intrinsics; - eon_intrinsics << + fcam_intrinsics << 2648.0, 0.0, 1928.0/2, 0.0, 2648.0, 1208.0/2, 0.0, 0.0, 1.0; @@ -69,7 +68,7 @@ void* live_thread(void *arg) { extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; } - auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen; + auto camera_frame_from_road_frame = fcam_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); @@ -112,7 +111,7 @@ int main(int argc, char **argv) { assert(err == 0); // messaging - PubMaster pm({"model", "cameraOdometry"}); + PubMaster pm({"modelV2", "model", "cameraOdometry"}); SubMaster sm({"pathPlan", "frame"}); #if defined(QCOM) || defined(QCOM2) @@ -148,7 +147,7 @@ int main(int argc, char **argv) { // setup filter to track dropped frames const float dt = 1. / MODEL_FREQ; - const float ts = 5.0; // 5 s filter time constant + const float ts = 10.0; // filter time constant (s) const float frame_filter_k = (dt / ts) / (1. + dt / ts); float frames_dropped = 0; @@ -158,6 +157,7 @@ int main(int argc, char **argv) { uint32_t frame_id = 0, last_vipc_frame_id = 0; double last = 0; int desire = -1; + uint32_t run_count = 0; while (!do_exit) { VIPCBuf *buf; VIPCBufExtra extra; @@ -174,12 +174,14 @@ int main(int argc, char **argv) { if (sm.update(0) > 0){ // TODO: path planner timeout? - desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1; + desire = ((int)sm["pathPlan"].getPathPlan().getDesire()); frame_id = sm["frame"].getFrame().getFrameId(); } double mt1 = 0, mt2 = 0; if (run_model_this_iter) { + run_count++; + float vec_desire[DESIRE_LEN] = {0}; if (desire >= 0 && desire < DESIRE_LEN) { vec_desire[desire] = 1.0; @@ -194,16 +196,19 @@ int main(int argc, char **argv) { model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height, model_transform, NULL, vec_desire); mt2 = millis_since_boot(); + float model_execution_time = (mt2 - mt1) / 1000.0; // 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); - float frame_drop_perc = frames_dropped / MODEL_FREQ; + if (run_count < 10) frames_dropped = 0; // let frame drops warm up + float frame_drop_ratio = frames_dropped / (1 + frames_dropped); - model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); - posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); + model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); + model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); + posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof); - LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc); + LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); last = mt1; last_vipc_frame_id = extra.frame_id; } diff --git a/selfdrive/modeld/models/commonmodel.c b/selfdrive/modeld/models/commonmodel.cc similarity index 83% rename from selfdrive/modeld/models/commonmodel.c rename to selfdrive/modeld/models/commonmodel.cc index c156ad39df7ea9..62b2a12710feee 100644 --- a/selfdrive/modeld/models/commonmodel.c +++ b/selfdrive/modeld/models/commonmodel.cc @@ -59,6 +59,29 @@ void frame_free(ModelFrame* frame) { clReleaseMemObject(frame->transformed_y_cl); } +void softmax(const float* input, float* output, size_t len) { + float max_val = -FLT_MAX; + for(int i = 0; i < len; i++) { + const float v = input[i]; + if( v > max_val ) { + max_val = v; + } + } + + float denominator = 0; + for(int i = 0; i < len; i++) { + float const v = input[i]; + float const v_exp = expf(v - max_val); + denominator += v_exp; + output[i] = v_exp; + } + + const float inv_denominator = 1. / denominator; + for(int i = 0; i < len; i++) { + output[i] *= inv_denominator; + } + +} float sigmoid(float input) { return 1 / (1 + expf(-input)); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 7aed3d7998ee01..06d46b8f2fc42c 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -8,6 +8,7 @@ #include #endif +#include #include "common/mat.h" #include "transforms/transform.h" #include "transforms/loadyuv.h" @@ -16,6 +17,7 @@ extern "C" { #endif +void softmax(const float* input, float* output, size_t len); float softplus(float input); float sigmoid(float input); diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 6be44c926f24b6..fd262801e62c31 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -114,24 +114,39 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ resized_width, resized_height, mode); + // prerotate to be cache aware + uint8_t *resized_buf_rot = get_buffer(s->resized_buf_rot, resized_width*resized_height*3/2); + uint8_t *resized_y_buf_rot = resized_buf_rot; + uint8_t *resized_u_buf_rot = resized_y_buf_rot + (resized_width * resized_height); + uint8_t *resized_v_buf_rot = resized_u_buf_rot + ((resized_width/2) * (resized_height/2)); + + libyuv::I420Rotate(resized_y_buf, resized_width, + resized_u_buf, resized_width/2, + resized_v_buf, resized_width/2, + resized_y_buf_rot, resized_height, + resized_u_buf_rot, resized_height/2, + resized_v_buf_rot, resized_height/2, + // negative height causes a vertical flip to match previous + resized_width, -resized_height, libyuv::kRotate90); + int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); // one shot conversion, O(n) anyway // yuvframe2tensor, normalize - for (int r = 0; r < MODEL_HEIGHT/2; r++) { - for (int c = 0; c < MODEL_WIDTH/2; c++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { + for (int r = 0; r < MODEL_HEIGHT/2; r++) { // Y_ul - net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]); - // Y_ur - net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c)*resized_height]); // Y_dl - net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c)*resized_height]); + // Y_ur + net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c+1)*resized_height]); // Y_dr - net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c+1)*resized_height]); // U - net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + r + (c*resized_height/2)]); // V - net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + r + (c*resized_height/2)]); } } @@ -140,6 +155,10 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ //fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file); //fclose(dump_yuv_file); + // *** testing *** + // idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320) + // imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420)) + //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb"); //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); //fclose(dump_yuv_file2); @@ -165,11 +184,12 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ return ret; } -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res){ +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){ // make msg MessageBuilder msg; auto framed = msg.initEvent().initDriverState(); framed.setFrameId(frame_id); + framed.setModelExecutionTime(execution_time); kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); kj::ArrayPtr face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta)); diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index b1b6819d57f7e4..7360af817c3209 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -29,6 +29,7 @@ typedef struct DMonitoringModelState { bool is_rhd; float output[OUTPUT_SIZE]; std::vector resized_buf; + std::vector resized_buf_rot; std::vector cropped_buf; std::vector premirror_cropped_buf; std::vector net_input_buf; @@ -36,7 +37,7 @@ typedef struct DMonitoringModelState { void dmonitoring_init(DMonitoringModelState* s); DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height); -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res); +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time); void dmonitoring_free(DMonitoringModelState* s); #ifdef __cplusplus diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9303245d778d96..c55af9e0623be0 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -9,14 +9,17 @@ #include "common/params.h" #include "driving.h" -#define PATH_IDX 0 -#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1 -#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2 -#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2 -#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION -#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2 -#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2 -#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define MIN_VALID_LEN 10.0 +#define TRAJECTORY_SIZE 33 +#define TRAJECTORY_TIME 10.0 +#define TRAJECTORY_DISTANCE 192.0 +#define PLAN_IDX 0 +#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE) +#define LL_PROB_IDX LL_IDX + 4*2*2*33 +#define RE_IDX LL_PROB_IDX + 4 +#define LEAD_IDX RE_IDX + 2*2*2*33 +#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE) +#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3 #define META_IDX DESIRE_STATE_IDX + DESIRE_LEN #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE @@ -29,6 +32,8 @@ // #define DUMP_YUV Eigen::Matrix vander; +float X_IDXS[TRAJECTORY_SIZE]; +float T_IDXS[TRAJECTORY_SIZE]; void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) { frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); @@ -63,9 +68,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t #endif // Build Vandermonde matrix - for(int i = 0; i < MODEL_PATH_DISTANCE; i++) { + for(int i = 0; i < TRAJECTORY_SIZE; i++) { for(int j = 0; j < POLYFIT_DEGREE - 1; j++) { - vander(i, j) = pow(i, POLYFIT_DEGREE-j-1); + X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2)); + T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2)); + vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1); } } } @@ -76,7 +83,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, float *desire_in) { #ifdef DESIRE if (desire_in != NULL) { - for (int i = 0; i < DESIRE_LEN; i++) { + for (int i = 1; i < DESIRE_LEN; i++) { // Model decides when action is completed // so desire input is just a pulse triggered on rising edge if (desire_in[i] - s->prev_desire[i] > .99) { @@ -107,13 +114,12 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, // net outputs ModelDataRaw net_outputs; - net_outputs.path = &s->output[PATH_IDX]; - net_outputs.left_lane = &s->output[LL_IDX]; - net_outputs.right_lane = &s->output[RL_IDX]; + net_outputs.plan = &s->output[PLAN_IDX]; + net_outputs.lane_lines = &s->output[LL_IDX]; + net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX]; + net_outputs.road_edges = &s->output[RE_IDX]; net_outputs.lead = &s->output[LEAD_IDX]; - net_outputs.long_x = &s->output[LONG_X_IDX]; - net_outputs.long_v = &s->output[LONG_V_IDX]; - net_outputs.long_a = &s->output[LONG_A_IDX]; + net_outputs.lead_prob = &s->output[LEAD_PROB_IDX]; net_outputs.meta = &s->output[DESIRE_STATE_IDX]; net_outputs.pose = &s->output[POSE_IDX]; return net_outputs; @@ -151,35 +157,40 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { out[3] = y0; } -void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) { - float points_arr[MODEL_PATH_DISTANCE]; - float stds_arr[MODEL_PATH_DISTANCE]; +void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) { + float points_arr[TRAJECTORY_SIZE]; + float stds_arr[TRAJECTORY_SIZE]; float poly_arr[POLYFIT_DEGREE]; float std; - float prob; - float valid_len; - - // clamp to 5 and MODEL_PATH_DISTANCE - valid_len = fmin(MODEL_PATH_DISTANCE, fmax(5, data[MODEL_PATH_DISTANCE*2])); - for (int i=0; i poly(&poly_arr[0], ARRAYSIZE(poly_arr)); + path.setPoly(poly); + path.setProb(1.0); + path.setStd(std); + path.setValidLen(valid_len); +} - if (std::getenv("DEBUG")){ - kj::ArrayPtr stds(&stds_arr[0], ARRAYSIZE(stds_arr)); - path.setStds(stds); +void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) { + float points_arr[TRAJECTORY_SIZE]; + float stds_arr[TRAJECTORY_SIZE]; + float poly_arr[POLYFIT_DEGREE]; + float std; - kj::ArrayPtr points(&points_arr[0], ARRAYSIZE(points_arr)); - path.setPoints(points); + for (int i=0; i poly(&poly_arr[0], ARRAYSIZE(poly_arr)); path.setPoly(poly); @@ -188,91 +199,275 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo path.setValidLen(valid_len); } -void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) { - const double x_scale = 10.0; - const double y_scale = 10.0; - - lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE + t_offset])); - lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]); - lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS])); - lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]); - lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1])); - lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]); - lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2])); - lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]); - lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3])); +void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) { + lead.setProb(prob); + lead.setT(t); + float xyva_arr[LEAD_MHP_VALS]; + float xyva_stds_arr[LEAD_MHP_VALS]; + for (int i=0; i xyva(xyva_arr, LEAD_MHP_VALS); + kj::ArrayPtr xyva_stds(xyva_stds_arr, LEAD_MHP_VALS); + lead.setXyva(xyva); + lead.setXyvaStd(xyva_stds); +} + +void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) { + lead.setProb(prob); + lead.setDist(data[0]); + lead.setStd(exp(data[LEAD_MHP_VALS])); + // TODO make all msgs same format + lead.setRelY(-data[1]); + lead.setRelYStd(exp(data[LEAD_MHP_VALS + 1])); + lead.setRelVel(data[2]); + lead.setRelVelStd(exp(data[LEAD_MHP_VALS + 2])); + lead.setRelA(data[3]); + lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3])); } void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { - kj::ArrayPtr desire_state(&meta_data[0], DESIRE_LEN); + float desire_state_softmax[DESIRE_LEN]; + float desire_pred_softmax[4*DESIRE_LEN]; + softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN); + for (int i=0; i<4; i++) { + softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN], + &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN); + } + kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); meta.setDesireState(desire_state); - meta.setEngagedProb(meta_data[DESIRE_LEN]); - meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]); - meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]); - meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]); - kj::ArrayPtr desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE); + meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); + meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); + meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); + meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); + kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); meta.setDesirePrediction(desire_pred); } -void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_x_data, const float * long_v_data, const float * long_a_data) { - // just doing 10 vals, 1 every sec for now - float dist_arr[TIME_DISTANCE/10]; - float speed_arr[TIME_DISTANCE/10]; - float accel_arr[TIME_DISTANCE/10]; - for (int i=0; i dist(&dist_arr[0], ARRAYSIZE(dist_arr)); - longi.setDistances(dist); - kj::ArrayPtr speed(&speed_arr[0], ARRAYSIZE(speed_arr)); - longi.setSpeeds(speed); - kj::ArrayPtr accel(&accel_arr[0], ARRAYSIZE(accel_arr)); - longi.setAccelerations(accel); + kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); + meta.setDesireState(desire_state); + meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); + meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); + meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); + meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); + kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); + meta.setDesirePrediction(desire_pred); } -void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { +void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, + int columns, int column_offset, float * plan_t_arr) { + float x_arr[TRAJECTORY_SIZE]; + float y_arr[TRAJECTORY_SIZE]; + float z_arr[TRAJECTORY_SIZE]; + //float x_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]; + } else { + t_arr[i] = plan_t_arr[i]; + x_arr[i] = X_IDXS[i]; + //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]; + z_arr[i] = data[i*columns + 2 + column_offset]; + //z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; + } + kj::ArrayPtr x(x_arr, TRAJECTORY_SIZE); + kj::ArrayPtr y(y_arr, TRAJECTORY_SIZE); + kj::ArrayPtr z(z_arr, TRAJECTORY_SIZE); + //kj::ArrayPtr x_std(x_std_arr, TRAJECTORY_SIZE); + //kj::ArrayPtr y_std(y_std_arr, TRAJECTORY_SIZE); + //kj::ArrayPtr z_std(z_std_arr, TRAJECTORY_SIZE); + kj::ArrayPtr t(t_arr, TRAJECTORY_SIZE); + xyzt.setX(x); + xyzt.setY(y); + xyzt.setZ(z); + //xyzt.setXStd(x_std); + //xyzt.setYStd(y_std); + //xyzt.setZStd(z_std); + xyzt.setT(t); +} + + +void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, + uint32_t vipc_dropped_frames, float frame_drop, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof, + float model_execution_time) { + // make msg + MessageBuilder msg; + auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModelV2(); uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; + framed.setFrameId(vipc_frame_id); + framed.setFrameAge(frame_age); + framed.setFrameDropPerc(frame_drop * 100); + framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); + + // plan + int plan_mhp_max_idx = 0; + for (int i=1; i + net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) { + plan_mhp_max_idx = i; + } + } + + float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)]; + float plan_t_arr[TRAJECTORY_SIZE]; + for (int i=0; i lane_line_probs(lane_line_probs_arr, 4); + framed.setLaneLineProbs(lane_line_probs); + framed.setLaneLineStds(lane_line_stds_arr); + + // road edges + 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); + road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]); + } + framed.setRoadEdgeStds(road_edge_stds_arr); + + // meta + auto meta = framed.initMeta(); + fill_meta_v2(meta, net_outputs.meta); + + // leads + auto leads = framed.initLeads(LEAD_MHP_SELECTION); + int mdn_max_idx = 0; + float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0}; + for (int t_offset=0; t_offset + net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) { + mdn_max_idx = i; + fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], + sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]); + } + } + } + pm.send("modelV2", msg); +} + +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, + uint32_t vipc_dropped_frames, float frame_drop, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof, + float model_execution_time) { + uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; MessageBuilder msg; auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModel(); framed.setFrameId(vipc_frame_id); framed.setFrameAge(frame_age); framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); + + // Find the distribution that corresponds to the most probable plan + int plan_mhp_max_idx = 0; + for (int i=1; i + net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) { + plan_mhp_max_idx = i; + } + } - fill_path(framed.initPath(), net_outputs.path, false, 0); - fill_path(framed.initLeftLane(), net_outputs.left_lane, true, 1.8); - fill_path(framed.initRightLane(), net_outputs.right_lane, true, -1.8); - fill_longi(framed.initLongitudinal(), net_outputs.long_x, net_outputs.long_v, net_outputs.long_a); + // x pos at 10s is a good valid_len + float valid_len = 0; + float valid_len_candidate; + for (int i=1; i= valid_len){ + valid_len = valid_len_candidate; + } + } + // clamp to 10 and MODEL_PATH_DISTANCE + valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len)); + int valid_len_idx = 0; + for (int i=1; i= X_IDXS[valid_len_idx]){ + valid_len_idx = i; + } + } + + auto lpath = framed.initPath(); + fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx); + + auto left_lane = framed.initLeftLane(); + int ll_idx = 1; + fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx, + sigmoid(net_outputs.lane_lines_prob[ll_idx])); + auto right_lane = framed.initRightLane(); + ll_idx = 2; + fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx, + sigmoid(net_outputs.lane_lines_prob[ll_idx])); // Find the distribution that corresponds to the current lead int mdn_max_idx = 0; int t_offset = 0; - for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { + for (int i=1; i + net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) { mdn_max_idx = i; } } - fill_lead(framed.initLead(), net_outputs.lead, mdn_max_idx, t_offset); + fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset])); // Find the distribution that corresponds to the lead in 2s mdn_max_idx = 0; t_offset = 1; - for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { + for (int i=1; i + net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) { mdn_max_idx = i; } } - fill_lead(framed.initLeadFuture(), net_outputs.lead, mdn_max_idx, t_offset); + fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset])); + fill_meta(framed.initMeta(), net_outputs.meta); pm.send("model", msg); } void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { + uint32_t vipc_dropped_frames, float frame_drop, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { float trans_arr[3]; float trans_std_arr[3]; float rot_arr[3]; @@ -280,10 +475,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, for (int i =0; i < 3; i++) { trans_arr[i] = net_outputs.pose[i]; - trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6; + trans_std_arr[i] = exp(net_outputs.pose[6 + i]); - rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0; - rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0; + rot_arr[i] = net_outputs.pose[3 + i]; + rot_std_arr[i] = exp(net_outputs.pose[9 + i]); } MessageBuilder msg; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 170982db11b78a..346e6e876057b3 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -17,33 +17,40 @@ #include #include "messaging.hpp" +#define MODEL_NAME "supercombo_dlc" + #define MODEL_WIDTH 512 #define MODEL_HEIGHT 256 #define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2 -#define MODEL_NAME "supercombo_dlc" - #define DESIRE_LEN 8 #define TRAFFIC_CONVENTION_LEN 2 -#define LEAD_MDN_N 5 // probs for 5 groups -#define MDN_VALS 4 // output xyva for each lead group -#define SELECTION 3 //output 3 group (lead now, in 2s and 6s) -#define MDN_GROUP_SIZE 11 -#define TIME_DISTANCE 100 + +#define PLAN_MHP_N 5 +#define PLAN_MHP_COLUMNS 30 +#define PLAN_MHP_VALS 30*33 +#define PLAN_MHP_SELECTION 1 +#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION) + +#define LEAD_MHP_N 5 +#define LEAD_MHP_VALS 4 +#define LEAD_MHP_SELECTION 3 +#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION) + #define POSE_SIZE 12 #define MODEL_FREQ 20 #define MAX_FRAME_DROP 0.05 struct ModelDataRaw { - float *path; - float *left_lane; - float *right_lane; + float *plan; + float *lane_lines; + float *lane_lines_prob; + float *road_edges; float *lead; - float *long_x; - float *long_v; - float *long_a; + float *lead_prob; float *desire_state; float *meta; + float *desire_pred; float *pose; }; @@ -71,7 +78,12 @@ void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint64_t timestamp_eof, float model_execution_time); +void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint64_t timestamp_eof, float model_execution_time); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint64_t timestamp_eof); #endif diff --git a/selfdrive/modeld/runners/run.h b/selfdrive/modeld/runners/run.h index 56e78539745470..dea340a0afa41c 100644 --- a/selfdrive/modeld/runners/run.h +++ b/selfdrive/modeld/runners/run.h @@ -7,9 +7,9 @@ #ifdef QCOM #define DefaultRunModel SNPEModel #else - #ifdef USE_TF_MODEL - #include "tfmodel.h" - #define DefaultRunModel TFModel + #ifdef USE_ONNX_MODEL + #include "onnxmodel.h" + #define DefaultRunModel ONNXModel #else #define DefaultRunModel SNPEModel #endif diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 38da13453a2473..2197c24cd41257 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -1,6 +1,7 @@ #pragma clang diagnostic ignored "-Wexceptions" #include +#include #include #include "common/util.h" #include "snpemodel.h" diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc index 11f10cfeb51a99..e08370a70d64c7 100644 --- a/selfdrive/modeld/thneed/thneed.cc +++ b/selfdrive/modeld/thneed/thneed.cc @@ -1,10 +1,11 @@ -#include "thneed.h" #include #include #include #include #include +#include #include +#include "thneed.h" Thneed *g_thneed = NULL; int g_fd = -1; @@ -31,12 +32,20 @@ extern "C" { int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL; #undef ioctl int ioctl(int filedes, unsigned long request, void *argp) { + request &= 0xFFFFFFFF; // needed on QCOM2 if (my_ioctl == NULL) my_ioctl = reinterpret_cast(dlsym(RTLD_NEXT, "ioctl")); Thneed *thneed = g_thneed; // save the fd if (request == IOCTL_KGSL_GPUOBJ_ALLOC) g_fd = filedes; + if (request == IOCTL_KGSL_DRAWCTXT_CREATE) { + struct kgsl_drawctxt_create *create = (struct kgsl_drawctxt_create *)argp; + create->flags &= ~KGSL_CONTEXT_PRIORITY_MASK; + create->flags |= 1 << KGSL_CONTEXT_PRIORITY_SHIFT; // priority from 1-15, 1 is max priority + printf("creating context with flags 0x%x\n", create->flags); + } + if (thneed != NULL) { if (request == IOCTL_KGSL_GPU_COMMAND) { struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp; @@ -440,7 +449,14 @@ cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, c #endif void *dlsym(void *handle, const char *symbol) { + // TODO: Find dlsym in a better way. Currently this is hand looked up in libdl.so +#if defined QCOM void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen-0x2d4); +#elif defined QCOM2 + void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen+0x138); +#else + #error "Unsupported platform for thneed" +#endif if (memcmp("REAL_", symbol, 5) == 0) { return my_dlsym(handle, symbol+5); } else if (strcmp("clEnqueueNDRangeKernel", symbol) == 0) { diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h index 36f0bfed7fdc80..a145a28476b763 100644 --- a/selfdrive/modeld/thneed/thneed.h +++ b/selfdrive/modeld/thneed/thneed.h @@ -1,8 +1,14 @@ #pragma once +#ifndef __user + #define __user __attribute__(()) +#endif + +#include #include #include "include/msm_kgsl.h" #include +#include #include using namespace std; diff --git a/selfdrive/modeld/transforms/transform.c b/selfdrive/modeld/transforms/transform.cc similarity index 96% rename from selfdrive/modeld/transforms/transform.c rename to selfdrive/modeld/transforms/transform.cc index 0b4150ddb27268..53e7fc488ccc20 100644 --- a/selfdrive/modeld/transforms/transform.c +++ b/selfdrive/modeld/transforms/transform.cc @@ -99,14 +99,14 @@ void transform_queue(Transform* s, err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl); assert(err == 0); - const size_t work_size_y[2] = {out_y_width, out_y_height}; + const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height}; err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, (const size_t*)&work_size_y, NULL, 0, 0, NULL); assert(err == 0); - const size_t work_size_uv[2] = {out_uv_width, out_uv_height}; + const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height}; err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width); assert(err == 0); diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index c8d6d158730d4b..d8fa4932ca5385 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -12,7 +12,7 @@ def dmonitoringd_thread(sm=None, pm=None): pm = messaging.PubMaster(['dMonitoringState']) if sm is None: - sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model'], poll=['driverState']) + sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'model'], poll=['driverState']) driver_status = DriverStatus() driver_status.is_rhd_region = Params().get("IsRHD") == b"1" @@ -22,7 +22,6 @@ def dmonitoringd_thread(sm=None, pm=None): sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].vEgo = 0. - sm['carState'].cruiseState.enabled = False sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False @@ -47,7 +46,7 @@ def dmonitoringd_thread(sm=None, pm=None): sm['carState'].steeringPressed or \ sm['carState'].gasPressed if driver_engaged: - driver_status.update(Events(), True, sm['carState'].cruiseState.enabled, sm['carState'].standstill) + driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill) v_cruise_last = v_cruise if sm.updated['model']: @@ -55,14 +54,14 @@ def dmonitoringd_thread(sm=None, pm=None): # Get data from dmonitoringmodeld events = Events() - driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled) + driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state - driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) + driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) # build dMonitoringState packet dat = messaging.new_message('dMonitoringState') diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 51583807526c72..33005e19c6eb00 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -15,8 +15,8 @@ # ****************************************************************************************** _AWARENESS_TIME = 35. # passive wheel touch total timeout -_AWARENESS_PRE_TIME_TILL_TERMINAL = 7. -_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5. +_AWARENESS_PRE_TIME_TILL_TERMINAL = 12. +_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. _DISTRACTED_TIME = 11. _DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. @@ -251,13 +251,13 @@ def update(self, events, driver_engaged, ctrl_active, standstill): if self.awareness <= 0.: # terminal red alert: disengagement required alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive + self.hi_std_alert_enabled = True self.terminal_time += 1 if awareness_prev > 0.: self.terminal_alert_cnt += 1 elif self.awareness <= self.threshold_prompt: # prompt orange alert alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive - self.hi_std_alert_enabled = True elif self.awareness <= self.threshold_pre: # pre green alert alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive diff --git a/selfdrive/rtshield.py b/selfdrive/rtshield.py old mode 100644 new mode 100755 index e76022501b1520..9a638c91697e36 --- a/selfdrive/rtshield.py +++ b/selfdrive/rtshield.py @@ -1,16 +1,20 @@ #!/usr/bin/env python3 +import os import time from common.realtime import set_core_affinity, set_realtime_priority - # RT shield - ensure CPU 3 always remains available for RT processes # runs as SCHED_FIFO with minimum priority to ensure kthreads don't # get scheduled onto CPU 3, but it's always preemptible by realtime # openpilot processes def main(): - set_core_affinity(3) + set_core_affinity(int(os.getenv("CORE", "3"))) set_realtime_priority(1) while True: time.sleep(0.000001) + +if __name__ == "__main__": + main() + diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index facbf6403a16ba..01b0eec844fc3b 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -17,4 +17,7 @@ else: 'sensors/lsm6ds3_gyro.cc', 'sensors/lsm6ds3_temp.cc', ] - env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj']) + libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj'] + if arch == "larch64": + libs.append('i2c') + env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/selfdrive/test/test_cpu_usage.py b/selfdrive/test/test_cpu_usage.py index a86472105d95ca..a2c5a0a1f14a0a 100755 --- a/selfdrive/test/test_cpu_usage.py +++ b/selfdrive/test/test_cpu_usage.py @@ -15,11 +15,11 @@ def cputime_total(ct): def print_cpu_usage(first_proc, last_proc): procs = [ - ("selfdrive.controls.controlsd", 45.0), - ("./loggerd", 33.90), - ("selfdrive.locationd.locationd", 29.5), - ("selfdrive.controls.plannerd", 11.84), - ("selfdrive.locationd.paramsd", 10.5), + ("selfdrive.controls.controlsd", 47.0), + ("./loggerd", 42.0), + ("selfdrive.locationd.locationd", 35.0), + ("selfdrive.locationd.paramsd", 12.0), + ("selfdrive.controls.plannerd", 10.0), ("./_modeld", 7.12), ("./camerad", 7.07), ("./_sensord", 6.17), @@ -102,5 +102,7 @@ def test_cpu_usage(): passed = False try: passed = test_cpu_usage() + except Exception as e: + print("\n\n\n", "TEST FAILED:", str(e), "\n\n\n") finally: sys.exit(int(not passed)) diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 76b9ad3b26b4af..a6f2d4c4e9556e 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -9,7 +9,6 @@ from common.hardware import TICI from selfdrive.swaglog import cloudlog -PANDA_OUTPUT_VOLTAGE = 5.28 CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) # A C2 uses about 1W while idling, and 30h seens like a good shutoff for most cars @@ -62,11 +61,6 @@ def _read_param(path, parser, default=0): return default -def panda_current_to_actual_current(panda_current): - # From white/grey panda schematic - return (3.3 - (panda_current * 3.3 / 4096)) / 8.25 - - class PowerMonitoring: def __init__(self): self.params = Params() @@ -135,11 +129,6 @@ def calculate(self, health): # If the battery is discharging, we can use this measurement # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000)) - elif (health.health.hwType in [log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda]) and (health.health.current > 1): - # If white/grey panda, use the integrated current measurements if the measurement is not 0 - # If the measurement is 0, the current is 400mA or greater, and out of the measurement range of the panda - # This seems to be accurate to about 5% - current_power = (PANDA_OUTPUT_VOLTAGE * panda_current_to_actual_current(health.health.current)) elif (self.next_pulsed_measurement_time is not None) and (self.next_pulsed_measurement_time <= now): # TODO: Figure out why this is off by a factor of 3/4??? FUDGE_FACTOR = 1.33 diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 39e07c0dd70f46..c36e9cdbe07e3d 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -198,7 +198,6 @@ def thermald_thread(): should_start_prev = False handle_fan = None is_uno = False - has_relay = False params = Params() pm = PowerMonitoring() @@ -229,7 +228,6 @@ def thermald_thread(): # Setup fan handler on first connect to panda if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: is_uno = health.health.hwType == log.HealthData.HwType.uno - has_relay = health.health.hwType in [log.HealthData.HwType.blackPanda, log.HealthData.HwType.uno, log.HealthData.HwType.dos] if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") @@ -286,7 +284,7 @@ def thermald_thread(): # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) - if max_cpu_temp > 107. or bat_temp >= 63. or (has_relay and is_offroad_for_5_min and max_cpu_temp > 70.0): + if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min and max_cpu_temp > 70.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 96.0 or bat_temp > 60.: @@ -311,7 +309,7 @@ def thermald_thread(): now = datetime.datetime.utcnow() # show invalid date/time alert - startup_conditions["time_valid"] = now.year >= 2019 + startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10) set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # Show update prompt @@ -350,7 +348,6 @@ def thermald_thread(): startup_conditions["not_uninstalling"] = not params.get("DoUninstall") == b"1" startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version - completed_training = params.get("CompletedTrainingVersion") == training_version panda_signature = params.get("PandaFirmware") startup_conditions["fw_version_match"] = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None) @@ -358,7 +355,8 @@ def thermald_thread(): # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02 - startup_conditions["completed_training"] = completed_training or (current_branch in ['dashcam', 'dashcam-staging']) + 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" # if any CPU gets above 107 or the battery gets above 63, kill all processes @@ -367,6 +365,10 @@ def thermald_thread(): set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) should_start = all(startup_conditions.values()) + startup_conditions["hardware_supported"] = health is not None and health.health.hwType not in [log.HealthData.HwType.whitePanda, + log.HealthData.HwType.greyPanda] + set_offroad_alert_if_changed("Offroad_HardwareUnsupported", health is not None and not startup_conditions["hardware_supported"]) + if should_start: if not should_start_prev: params.delete("IsOffroad") @@ -375,7 +377,6 @@ def thermald_thread(): if started_ts is None: started_ts = sec_since_boot() started_seen = True - os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor') else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) @@ -385,7 +386,6 @@ def thermald_thread(): started_ts = None if off_ts is None: off_ts = sec_since_boot() - os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor') # Offroad power monitoring pm.calculate(health) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index e43f983712b4a5..5a208ffc593e2f 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -21,7 +21,9 @@ def get_tombstones(): # Loop over first 1000 directory entries for _, f in zip(range(1000), d): - if f.name.startswith("tombstone") or f.name.endswith(".crash"): + if f.name.startswith("tombstone"): + files.append((f.path, int(f.stat().st_ctime))) + elif f.name.endswith(".crash") and f.stat().st_mode == 0o100640: files.append((f.path, int(f.stat().st_ctime))) return files diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 55f269cb550617..414f862823863c 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,30 +1,84 @@ -Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') +import os +Import('env', 'arch', 'real_arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') + +qt_env = None +if arch in ["x86_64", "Darwin", "larch64"]: + qt_env = env.Clone() + + if arch == "Darwin": + qt_env['QTDIR'] = "/usr/local/opt/qt" + QT_BASE = "/usr/local/opt/qt/" + qt_dirs = [ + QT_BASE + "include/", + QT_BASE + "include/QtWidgets", + QT_BASE + "include/QtGui", + QT_BASE + "include/QtCore", + QT_BASE + "include/QtDBus", + QT_BASE + "include/QtMultimedia", + ] + qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"] + else: + qt_env['QTDIR'] = "/usr" + qt_dirs = [ + f"/usr/include/{real_arch}-linux-gnu/qt5", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtWidgets", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtCore", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtDBus", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtMultimedia", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.5.1/QtGui", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.12.8/QtGui", + ] + + qt_env.Tool('qt') + qt_env['CPPPATH'] += qt_dirs + qt_flags = [ + "-D_REENTRANT", + "-DQT_NO_DEBUG", + "-DQT_WIDGETS_LIB", + "-DQT_GUI_LIB", + "-DQT_CORE_LIB" + ] + qt_env['CXXFLAGS'] += qt_flags + + src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] libs = [common, 'zmq', 'capnp', 'kj', 'm', cereal, messaging, gpucommon, visionipc] if qt_env is None: - libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'CB', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid', 'OpenCL'] + libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', + 'ui', 'CB', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid', 'OpenCL'] linkflags = ['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'] src += ["android/ui.cc", "android/sl_sound.cc"] env.Program('_ui', src, LINKFLAGS=linkflags, LIBS=libs) - else: qt_libs = ["pthread"] - if arch == "Darwin": - qt_env["FRAMEWORKS"] += ["QtWidgets", "QtGui", "QtCore", "QtDBus", "QtMultimedia"] - else: - qt_libs += ["Qt5Widgets", "Qt5Gui", "Qt5Core", "Qt5DBus", "Qt5Multimedia"] + qt_modules = ["Widgets", "Gui", "Core", "DBus", "Multimedia"] if arch == "larch64": qt_libs += ["GLESv2", "wayland-client"] - else: + elif arch != "Darwin": qt_libs += ["GL"] - qt_src = ["qt/ui.cc", "qt/window.cc", "qt/settings.cc", "qt/qt_sound.cc"] + src + if arch == "Darwin": + qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"] + else: + qt_libs += [f"Qt5{m}" for m in qt_modules] + + qt_src = ["qt/ui.cc", "qt/window.cc", "qt/qt_sound.cc", "qt//offroad/keyboard.cc", "qt/offroad/input_field.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc", "qt/offroad/wifi.cc", "qt/offroad/wifiManager.cc"] + src qt_env.Program("_ui", qt_src, LIBS=qt_libs + libs) + + # spinner and text window + qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs + libs) + qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs + libs) + + # setup and installer + if "BUILD_SETUP" in os.environ: + qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc"], LIBS=qt_libs + libs + ['curl']) + qt_env.Program("qt/setup/installer", ["qt/setup/installer.cc"], LIBS=qt_libs + libs) diff --git a/selfdrive/ui/spinner/Makefile b/selfdrive/ui/android/spinner/Makefile similarity index 77% rename from selfdrive/ui/spinner/Makefile rename to selfdrive/ui/android/spinner/Makefile index 49401067f92241..7ceea5f61fcdc5 100644 --- a/selfdrive/ui/spinner/Makefile +++ b/selfdrive/ui/android/spinner/Makefile @@ -1,7 +1,9 @@ CC = clang CXX = clang++ -PHONELIBS = ../../../phonelibs +ROOT_DIR = ../../.. +PHONELIBS = $(ROOT_DIR)/phonelibs +COMMON = $(ROOT)/selfdrive/common WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=incompatible-pointer-types \ @@ -19,10 +21,10 @@ OPENGL_LIBS = -lGLESv3 FRAMEBUFFER_LIBS = -lutils -lgui -lEGL OBJS = spinner.o \ - ../../common/framebuffer.o \ - ../../common/util.o \ + $(COMMON)/framebuffer.o \ + $(COMMON)/util.o \ $(PHONELIBS)/nanovg/nanovg.o \ - ../../common/spinner.o \ + $(COMMON)/spinner.o \ opensans_semibold.o \ img_spinner_track.o \ img_spinner_comma.o @@ -41,7 +43,7 @@ spinner: $(OBJS) $(OPENGL_LIBS) \ -lm -llog -../../common/framebuffer.o: ../../common/framebuffer.cc +$(COMMON)/framebuffer.o: $(COMMON)/framebuffer.cc @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) -MMD \ -I$(PHONELIBS)/android_frameworks_native/include \ @@ -49,15 +51,15 @@ spinner: $(OBJS) -I$(PHONELIBS)/android_hardware_libhardware/include \ -c -o '$@' '$<' -opensans_semibold.o: ../../assets/fonts/opensans_semibold.ttf +opensans_semibold.o: $(ROOT_DIR)/selfdrive/assets/fonts/opensans_semibold.ttf @echo "[ bin2o ] $@" cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' -img_spinner_track.o: ../../assets/img_spinner_track.png +img_spinner_track.o: $(ROOT_DIR)/selfdrive/assets/img_spinner_track.png @echo "[ bin2o ] $@" cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' -img_spinner_comma.o: ../../assets/img_spinner_comma.png +img_spinner_comma.o: $(ROOT_DIR)/selfdrive/assets/img_spinner_comma.png @echo "[ bin2o ] $@" cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' diff --git a/selfdrive/ui/spinner/spinner b/selfdrive/ui/android/spinner/spinner similarity index 100% rename from selfdrive/ui/spinner/spinner rename to selfdrive/ui/android/spinner/spinner diff --git a/selfdrive/common/spinner.c b/selfdrive/ui/android/spinner/spinner.c similarity index 99% rename from selfdrive/common/spinner.c rename to selfdrive/ui/android/spinner/spinner.c index 2054229cc00707..33ebf7cd38f3d1 100644 --- a/selfdrive/common/spinner.c +++ b/selfdrive/ui/android/spinner/spinner.c @@ -42,8 +42,7 @@ bool stdin_input_available() { return (FD_ISSET(0, &fds)); } -int spin(int argc, char** argv) { - int err; +int main(int argc, char** argv) { bool draw_progress = false; float progress_val = 0.0; diff --git a/selfdrive/ui/text/Makefile b/selfdrive/ui/android/text/Makefile similarity index 84% rename from selfdrive/ui/text/Makefile rename to selfdrive/ui/android/text/Makefile index ac0d720d573d10..3401cae0ff7352 100644 --- a/selfdrive/ui/text/Makefile +++ b/selfdrive/ui/android/text/Makefile @@ -1,7 +1,8 @@ CC = clang CXX = clang++ -PHONELIBS = ../../../phonelibs +PHONELIBS = ../../../../phonelibs +COMMON = ../../../common WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=incompatible-pointer-types \ @@ -19,9 +20,9 @@ OPENGL_LIBS = -lGLESv3 FRAMEBUFFER_LIBS = -lutils -lgui -lEGL OBJS = text.o \ - ../../common/framebuffer.o \ - ../../common/util.o \ - ../../../selfdrive/common/touch.o \ + $(COMMON)/framebuffer.o \ + $(COMMON)/util.o \ + $(COMMON)/touch.o \ $(PHONELIBS)/nanovg/nanovg.o \ opensans_regular.o \ @@ -39,7 +40,7 @@ text: $(OBJS) $(OPENGL_LIBS) \ -lm -llog -opensans_regular.o: ../../assets/fonts/opensans_regular.ttf +opensans_regular.o: ../../../assets/fonts/opensans_regular.ttf @echo "[ bin2o ] $@" cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' @@ -47,7 +48,7 @@ opensans_regular.o: ../../assets/fonts/opensans_regular.ttf mkdir -p $(@D) @echo "[ CC ] $@" $(CC) $(CPPFLAGS) $(CFLAGS) \ - -I../.. \ + -I../../.. \ -I$(PHONELIBS)/android_frameworks_native/include \ -I$(PHONELIBS)/android_system_core/include \ -I$(PHONELIBS)/android_hardware_libhardware/include \ @@ -58,8 +59,8 @@ opensans_regular.o: ../../assets/fonts/opensans_regular.ttf mkdir -p $(@D) @echo "[ CXX ] $@" $(CXX) $(CPPFLAGS) $(CXXFLAGS) \ - -I../../selfdrive \ - -I../../ \ + -I../../../selfdrive \ + -I../../../ \ -I$(PHONELIBS)/android_frameworks_native/include \ -I$(PHONELIBS)/android_system_core/include \ -I$(PHONELIBS)/android_hardware_libhardware/include \ diff --git a/selfdrive/ui/text/text b/selfdrive/ui/android/text/text similarity index 100% rename from selfdrive/ui/text/text rename to selfdrive/ui/android/text/text diff --git a/selfdrive/ui/text/text.c b/selfdrive/ui/android/text/text.c similarity index 100% rename from selfdrive/ui/text/text.c rename to selfdrive/ui/android/text/text.c diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 16db6cad6caa1b..72dae8134a5b02 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -4,6 +4,8 @@ #include #include #include "common/util.h" +#include + #define NANOVG_GLES3_IMPLEMENTATION #include "nanovg_gl.h" @@ -37,27 +39,24 @@ const mat3 intrinsic_matrix = (mat3){{ // Projects a point in car to space to the corresponding point in full frame // image space. -vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) { - const UIScene *scene = &s->scene; - +bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y) { + const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}}; // We'll call the car space point p. // First project into normalized image coordinates with the extrinsics matrix. - const vec4 Ep4 = matvecmul(scene->extrinsic_matrix, car_space_projective); + const vec4 Ep4 = matvecmul(s->scene.extrinsic_matrix, car_space_projective); // The last entry is zero because of how we store E (to use matvecmul). const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}}; const vec3 KEp = matvecmul3(intrinsic_matrix, Ep); // Project. - const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}}; - return p_image; -} + *out_x = KEp.v[0] / KEp.v[2]; + *out_y = KEp.v[1] / KEp.v[2]; -// Calculate an interpolation between two numbers at a specific increment -static float lerp(float v0, float v1, float t) { - return (1 - t) * v0 + t * v1; + return *out_x >= 0 && *out_x <= s->fb_w && *out_y >= 0 && *out_y <= s->fb_h; } + static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, float size, NVGcolor color, int font){ nvgFontFaceId(vg, font); nvgFontSize(vg, size); @@ -67,12 +66,8 @@ static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, f static void draw_chevron(UIState *s, float x_in, float y_in, float sz, NVGcolor fillColor, NVGcolor glowColor) { - const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}}; - const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - - float x = p_full_frame.v[0]; - float y = p_full_frame.v[1]; - if (x < 0 || y < 0.){ + float x, y; + if (!car_space_to_full_frame(s, x_in, y_in, 0.0, &x, &y)) { return; } @@ -134,110 +129,52 @@ static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &le draw_chevron(s, d_rel, lead.getYRel(), 25, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); } -static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { - if (pvd->cnt == 0) return; +static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcolor *color, NVGpaint *paint) { + if (cnt == 0) return; nvgBeginPath(s->vg); - nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y); - for (int i=1; icnt; i++) { - nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); + nvgMoveTo(s->vg, v[0].x, v[0].y); + for (int i = 1; i < cnt; i++) { + nvgLineTo(s->vg, v[i].x, v[i].y); } nvgClosePath(s->vg); - nvgFillColor(s->vg, color); + if (color) { + nvgFillColor(s->vg, *color); + } else if (paint) { + nvgFillPaint(s->vg, *paint); + } nvgFill(s->vg); } -static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) { +static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, track_vertices_data *pvd) { const UIScene *scene = &s->scene; - const float *points = scene->path_points; - const float *mpc_x_coords = &scene->mpc_x[0]; - const float *mpc_y_coords = &scene->mpc_y[0]; - - float off = is_mpc?0.3:0.5; - float lead_d = scene->lead_data[0].getDRel()*2.; - float path_height = is_mpc?(lead_d>5.)?fmin(lead_d, 25.)-fmin(lead_d*0.35, 10.):20. - :(lead_d>0.)?fmin(lead_d, 50.)-fmin(lead_d*0.35, 10.):49.; - path_height = fmin(path_height, scene->model.getPath().getValidLen()); - pvd->cnt = 0; - // left side up - for (int i=0; i<=path_height; i++) { - float px, py, mpx; - if (is_mpc) { - mpx = i==0?0.0:mpc_x_coords[i]; - px = lerp(mpx+1.0, mpx, i/100.0); - py = mpc_y_coords[i] - off; - } else { - px = lerp(i+1.0, i, i/100.0); - py = points[i] - off; - } - - vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) { - continue; - } - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; + const float off = 0.5; + int max_idx = 0; + float lead_d; + if(s->sm->updated("radarState")) { + lead_d = scene->lead_data[0].getDRel()*2.; + } else { + lead_d = MAX_DRAW_DISTANCE; } + float path_length = (lead_d>0.)?lead_d-fmin(lead_d*0.35, 10.):MAX_DRAW_DISTANCE; + path_length = fmin(path_length, scene->max_distance); - // right side down - for (int i=path_height; i>=0; i--) { - float px, py, mpx; - if (is_mpc) { - mpx = i==0?0.0:mpc_x_coords[i]; - px = lerp(mpx+1.0, mpx, i/100.0); - py = mpc_y_coords[i] + off; - } else { - px = lerp(i+1.0, i, i/100.0); - py = points[i] + off; - } - vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) { - continue; - } - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; + vertex_data *v = &pvd->v[0]; + for (int i = 0; line.getX()[i] <= path_length and i < TRAJECTORY_SIZE; i++) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y); + max_idx = i; } -} - -static void update_all_track_data(UIState *s) { - const UIScene *scene = &s->scene; - // Draw vision path - update_track_data(s, false, &s->track_vertices[0]); - - if (scene->controls_state.getEnabled()) { - // Draw MPC path when engaged - update_track_data(s, true, &s->track_vertices[1]); + for (int i = max_idx; i >= 0; i--) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y); } + pvd->cnt = v - pvd->v; } -static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { - if (pvd->cnt == 0) return; - - nvgBeginPath(s->vg); - nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y); - for (int i=1; icnt; i++) { - nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); - } - nvgClosePath(s->vg); - - NVGpaint track_bg; - if (is_mpc) { - // Draw colored MPC track - const NVGcolor clr = bg_colors[s->status]; - track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4, - nvgRGBA(clr.r, clr.g, clr.b, 255), nvgRGBA(clr.r, clr.g, clr.b, 255/2)); - } else { - // Draw white vision track - track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4, - COLOR_WHITE, COLOR_WHITE_ALPHA(0)); - } - nvgFillPaint(s->vg, track_bg); - nvgFill(s->vg); +static void ui_draw_track(UIState *s, track_vertices_data *pvd) { + NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4, + COLOR_WHITE, COLOR_WHITE_ALPHA(0)); + ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg); } static void draw_frame(UIState *s) { @@ -271,72 +208,48 @@ static void draw_frame(UIState *s) { glBindVertexArray(0); } -static inline bool valid_frame_pt(UIState *s, float x, float y) { - return x >= 0 && x <= s->stream.bufs_info.width && y >= 0 && y <= s->stream.bufs_info.height; -} - -static void update_lane_line_data(UIState *s, const float *points, float off, model_path_vertices_data *pvd, float valid_len) { - pvd->cnt = 0; - int rcount = fmin(MODEL_PATH_MAX_VERTICES_CNT / 2, valid_len); - for (int i = 0; i < rcount; i++) { - float px = (float)i; - float py = points[i] - off; - const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) - continue; - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; +static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float off, line_vertices_data *pvd, float max_distance) { + // TODO check that this doesn't overflow max vertex buffer + int max_idx; + vertex_data *v = &pvd->v[0]; + for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y); + max_idx = i; } - for (int i = rcount - 1; i > 0; i--) { - float px = (float)i; - float py = points[i] + off; - const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) - continue; - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; + for (int i = max_idx - 1; i > 0; i--) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y); } + pvd->cnt = v - pvd->v; } -static void update_all_lane_lines_data(UIState *s, const cereal::ModelData::PathData::Reader &path, const float *points, model_path_vertices_data *pstart) { - update_lane_line_data(s, points, 0.025*path.getProb(), pstart, path.getValidLen()); - update_lane_line_data(s, points, fmin(path.getStd(), 0.7), pstart + 1, path.getValidLen()); -} - -static void ui_draw_lane(UIState *s, model_path_vertices_data *pstart, NVGcolor color) { - ui_draw_lane_line(s, pstart, color); - color.a /= 25; - ui_draw_lane_line(s, pstart + 1, color); -} -static void ui_draw_vision_lanes(UIState *s) { +static void ui_draw_vision_lane_lines(UIState *s) { const UIScene *scene = &s->scene; - model_path_vertices_data *pvd = &s->model_path_vertices[0]; - if(s->sm->updated("model")) { - update_all_lane_lines_data(s, scene->model.getLeftLane(), scene->left_lane_points, pvd); - update_all_lane_lines_data(s, scene->model.getRightLane(), scene->right_lane_points, pvd + MODEL_LANE_PATH_CNT); + // paint lanelines + line_vertices_data *pvd_ll = &s->lane_line_vertices[0]; + for (int ll_idx = 0; ll_idx < 4; ll_idx++) { + if(s->sm->updated("modelV2")) { + update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance); + } + NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]); + ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr); } - - // Draw left lane edge - ui_draw_lane(s, pvd, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getLeftLane().getProb())); - - // Draw right lane edge - ui_draw_lane(s, pvd + MODEL_LANE_PATH_CNT, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getRightLane().getProb())); - - if(s->sm->updated("radarState")) { - update_all_track_data(s); + + // paint road edges + line_vertices_data *pvd_re = &s->road_edge_vertices[0]; + for (int re_idx = 0; re_idx < 2; re_idx++) { + if(s->sm->updated("modelV2")) { + update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance); + } + NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0)); + ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr); } - - // Draw vision path - ui_draw_track(s, false, &s->track_vertices[0]); - if (scene->controls_state.getEnabled()) { - // Draw MPC path when engaged - ui_draw_track(s, true, &s->track_vertices[1]); + + // paint path + if(s->sm->updated("modelV2")) { + update_track_data(s, scene->model.getPosition(), &s->track_vertices); } + ui_draw_track(s, &s->track_vertices); } // Draw all world space objects. @@ -344,7 +257,9 @@ static void ui_draw_world(UIState *s) { const UIScene *scene = &s->scene; nvgSave(s->vg); - nvgScissor(s->vg, s->video_rect.x, s->video_rect.y, s->video_rect.w, s->video_rect.h); + + // Don't draw on top of sidebar + nvgScissor(s->vg, scene->viz_rect.x, scene->viz_rect.y, scene->viz_rect.w, scene->viz_rect.h); // Apply transformation such that video pixel coordinates match video // 1) Put (0, 0) in the middle of the video @@ -357,7 +272,7 @@ static void ui_draw_world(UIState *s) { nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); // Draw lane edges and vision/mpc tracks - ui_draw_vision_lanes(s); + ui_draw_vision_lane_lines(s); // Draw lead indicators if openpilot is handling longitudinal if (s->longitudinal_control) { diff --git a/selfdrive/ui/qt/offroad/input_field.cc b/selfdrive/ui/qt/offroad/input_field.cc new file mode 100644 index 00000000000000..4696416eaface7 --- /dev/null +++ b/selfdrive/ui/qt/offroad/input_field.cc @@ -0,0 +1,56 @@ +#include +#include +#include +#include +#include + +#include "input_field.hpp" +#include "keyboard.hpp" + +InputField::InputField(QWidget *parent): QWidget(parent) { + l = new QVBoxLayout(); + QHBoxLayout *r = new QHBoxLayout(); + label = new QLabel(this); + label->setText("password"); + r->addWidget(label); + QPushButton* cancel = new QPushButton("cancel"); + QObject::connect(cancel, SIGNAL(released()), this, SLOT(emitEmpty())); + cancel->setFixedHeight(150); + cancel->setFixedWidth(300); + r->addWidget(cancel); + l->addLayout(r); + l->addSpacing(80); + + line = new QLineEdit(""); + l->addWidget(line); + l->addSpacing(200); + + k = new Keyboard(this); + QObject::connect(k, SIGNAL(emitButton(QString)), this, SLOT(getText(QString))); + l->addWidget(k); + setLayout(l); +} + +void InputField::emitEmpty(){ + emitText(""); + line->setText(""); +} +void InputField::getText(QString s){ + if(!QString::compare(s,"⌫")){ + line->backspace(); + } + + if(!QString::compare(s,"⏎")){ + emitText(line->text()); + line->setText(""); + } + + QVector control_buttons {"⇧", "↑", "ABC", "⏎", "#+=", "⌫", "123"}; + for(QString c :control_buttons){ + if(!QString::compare(s, c)){ + return; + } + } + line->insert(s.left(1)); +} + diff --git a/selfdrive/ui/qt/offroad/input_field.hpp b/selfdrive/ui/qt/offroad/input_field.hpp new file mode 100644 index 00000000000000..a56bac26193a8f --- /dev/null +++ b/selfdrive/ui/qt/offroad/input_field.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "keyboard.hpp" + +class InputField : public QWidget { + Q_OBJECT + +public: + explicit InputField(QWidget* parent = 0); + QLabel *label; + +private: + QLineEdit *line; + Keyboard *k; + QVBoxLayout *l; + +public slots: + void emitEmpty(); + void getText(QString s); + +signals: + void emitText(QString s); +}; diff --git a/selfdrive/ui/qt/offroad/keyboard.cc b/selfdrive/ui/qt/offroad/keyboard.cc new file mode 100644 index 00000000000000..0cdc38c562b6a7 --- /dev/null +++ b/selfdrive/ui/qt/offroad/keyboard.cc @@ -0,0 +1,120 @@ +#include +#include +#include +#include +#include +#include + +#include "keyboard.hpp" + +KeyboardLayout::KeyboardLayout(QWidget* parent, std::vector> layout) : QWidget(parent) { + QVBoxLayout* vlayout = new QVBoxLayout; + QButtonGroup* btn_group = new QButtonGroup(this); + + QObject::connect(btn_group, SIGNAL(buttonClicked(QAbstractButton*)), parent, SLOT(handleButton(QAbstractButton*))); + + int i = 0; + for(auto s : layout){ + QHBoxLayout *hlayout = new QHBoxLayout; + + if (i == 1){ + hlayout->addSpacing(50); + } + + for(QString p : s){ + QPushButton* btn = new QPushButton(p); + btn->setFixedHeight(100); + + if (p == QString(" ")){ + btn->setFixedWidth(1024); + } + + btn_group->addButton(btn); + hlayout->addSpacing(5); + hlayout->addWidget(btn); + } + + if (i == 1){ + hlayout->addSpacing(50); + } + + vlayout->addLayout(hlayout); + i++; + } + + setLayout(vlayout); +} + +Keyboard::Keyboard(QWidget *parent) : QWidget(parent) { + main_layout = new QStackedLayout; + + // lowercase + std::vector> lowercase = { + {"q","w","e","r","t","y","u","i","o","p"}, + {"a","s","d","f","g","h","j","k","l"}, + {"⇧","z","x","c","v","b","n","m","⌫"}, + {"123"," ","⏎"}, + }; + main_layout->addWidget(new KeyboardLayout(this, lowercase)); + + // uppercase + std::vector> uppercase = { + {"Q","W","E","R","T","Y","U","I","O","P"}, + {"A","S","D","F","G","H","J","K","L"}, + {"↑","Z","X","C","V","B","N","M","⌫"}, + {"123"," ","⏎"}, + }; + main_layout->addWidget(new KeyboardLayout(this, uppercase)); + + // 1234567890 + std::vector> numbers = { + {"1","2","3","4","5","6","7","8","9","0"}, + {"-","/",":",";","(",")","$","&&","@","\""}, + {"#+=",".",",","?","!","`","⌫"}, + {"ABC"," ","⏎"}, + }; + main_layout->addWidget(new KeyboardLayout(this, numbers)); + + // Special characters + std::vector> specials = { + {"[","]","{","}","#","%","^","*","+","="}, + {"_","\\","|","~","<",">","€","£","¥"," "}, + {"123",".",",","?","!","`","⌫"}, + {"ABC"," ","⏎"}, + }; + main_layout->addWidget(new KeyboardLayout(this, specials)); + + setLayout(main_layout); + main_layout->setCurrentIndex(0); + + setStyleSheet(R"( + QPushButton { font-size: 40px } + * { + background-color: #99777777; + } + )"); +} + + +void Keyboard::handleButton(QAbstractButton* m_button){ + QString id = m_button->text(); + if(!QString::compare(m_button->text(),"↑")||!QString::compare(m_button->text(),"ABC")){ + main_layout->setCurrentIndex(0); + } + if(!QString::compare(m_button->text(),"⇧")){ + main_layout->setCurrentIndex(1); + } + if(!QString::compare(m_button->text(),"123")){ + main_layout->setCurrentIndex(2); + } + if(!QString::compare(m_button->text(),"#+=")){ + main_layout->setCurrentIndex(3); + } + if(!QString::compare(m_button->text(),"⏎")){ + main_layout->setCurrentIndex(0); + } + if("A" <= id && id <= "Z"){ + main_layout->setCurrentIndex(0); + } + emit emitButton(m_button->text()); +} diff --git a/selfdrive/ui/qt/offroad/keyboard.hpp b/selfdrive/ui/qt/offroad/keyboard.hpp new file mode 100644 index 00000000000000..f21d48700cf57d --- /dev/null +++ b/selfdrive/ui/qt/offroad/keyboard.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include + +#include +#include +#include +#include + +class KeyboardLayout : public QWidget { + Q_OBJECT + +public: + explicit KeyboardLayout(QWidget *parent, std::vector> layout); +}; + +class Keyboard : public QWidget { + Q_OBJECT + +public: + explicit Keyboard(QWidget *parent = 0); + +private: + QStackedLayout* main_layout; + +private slots: + void handleButton(QAbstractButton* m_button); + +signals: + void emitButton(QString s); +}; diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc new file mode 100644 index 00000000000000..5e97b15e299d6b --- /dev/null +++ b/selfdrive/ui/qt/offroad/onboarding.cc @@ -0,0 +1,115 @@ +#include +#include +#include +#include + +#include "onboarding.hpp" +#include "common/params.h" + + +QLabel * title_label(QString text) { + QLabel *l = new QLabel(text); + l->setStyleSheet(R"(font-size: 100px;)"); + l->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum); + return l; +} + +QWidget * OnboardingWindow::terms_screen() { + + QGridLayout *main_layout = new QGridLayout(); + main_layout->setMargin(30); + main_layout->setSpacing(30); + + main_layout->addWidget(title_label("Review Terms"), 0, 0, 1, -1); + + QLabel *terms = new QLabel("See terms at https://my.comma.ai/terms"); + terms->setAlignment(Qt::AlignCenter); + terms->setStyleSheet(R"( + font-size: 75px; + border-radius: 10px; + background-color: #292929; + )"); + main_layout->addWidget(terms, 1, 0, 1, -1); + + main_layout->addWidget(new QPushButton("Decline"), 2, 0); + + QPushButton *accept_btn = new QPushButton("Accept"); + main_layout->addWidget(accept_btn, 2, 1); + QObject::connect(accept_btn, &QPushButton::released, [=]() { + Params().write_db_value("HasAcceptedTerms", LATEST_TERMS_VERSION); + updateActiveScreen(); + }); + + QWidget *widget = new QWidget; + widget->setLayout(main_layout); + widget->setStyleSheet(R"( + QPushButton { + font-size: 50px; + padding: 50px; + border-radius: 10px; + background-color: #292929; + } + )"); + + return widget; +} + +QWidget * OnboardingWindow::training_screen() { + + QGridLayout *main_layout = new QGridLayout(); + main_layout->setMargin(30); + main_layout->setSpacing(30); + + main_layout->addWidget(title_label("Training Guide"), 0, 0); + + QPushButton *btn = new QPushButton("Continue"); + main_layout->addWidget(btn, 1, 0); + QObject::connect(btn, &QPushButton::released, [=]() { + Params().write_db_value("CompletedTrainingVersion", LATEST_TRAINING_VERSION); + updateActiveScreen(); + }); + + QWidget *widget = new QWidget; + widget->setLayout(main_layout); + return widget; +} + +void OnboardingWindow::updateActiveScreen() { + + Params params = Params(); + bool accepted_terms = params.get("HasAcceptedTerms", false).compare(LATEST_TERMS_VERSION) == 0; + bool training_done = params.get("CompletedTrainingVersion", false).compare(LATEST_TRAINING_VERSION) == 0; + + if (!accepted_terms) { + swidget->setCurrentIndex(0); + } else if (!training_done) { + swidget->setCurrentIndex(1); + } else { + emit onboardingDone(); + } +} + +OnboardingWindow::OnboardingWindow(QWidget *parent) : QWidget(parent) { + QVBoxLayout * top_layout = new QVBoxLayout; + + swidget = new QStackedWidget(); + swidget->addWidget(terms_screen()); + swidget->addWidget(training_screen()); + + top_layout->addWidget(swidget); + + setLayout(top_layout); + setStyleSheet(R"( + * { + background-color: black; + } + QPushButton { + font-size: 50px; + padding: 50px; + border-radius: 10px; + background-color: #292929; + } + )"); + + updateActiveScreen(); +} diff --git a/selfdrive/ui/qt/offroad/onboarding.hpp b/selfdrive/ui/qt/offroad/onboarding.hpp new file mode 100644 index 00000000000000..137ace8226ea6b --- /dev/null +++ b/selfdrive/ui/qt/offroad/onboarding.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include + +// TODO: this is defined in python too +#define LATEST_TERMS_VERSION "2" +#define LATEST_TRAINING_VERSION "0.2.0" + +class OnboardingWindow : public QWidget { + Q_OBJECT + +public: + explicit OnboardingWindow(QWidget *parent = 0); + +private: + QWidget * terms_screen(); + QWidget * training_screen(); + QStackedWidget *swidget; + +signals: + void onboardingDone(); + +public slots: + void updateActiveScreen(); +}; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc new file mode 100644 index 00000000000000..eb2424082f7d04 --- /dev/null +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -0,0 +1,273 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "wifi.hpp" +#include "settings.hpp" +#include "input_field.hpp" + +#include "common/params.h" +#include "common/utilpp.h" + + +ParamsToggle::ParamsToggle(QString param, QString title, QString description, QString icon_path, QWidget *parent): QFrame(parent) , param(param) { + QHBoxLayout *hlayout = new QHBoxLayout; + QVBoxLayout *vlayout = new QVBoxLayout; + + hlayout->addSpacing(25); + if (icon_path.length()){ + QPixmap pix(icon_path); + QLabel *icon = new QLabel(); + icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); + icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + hlayout->addWidget(icon); + } else{ + hlayout->addSpacing(100); + } + hlayout->addSpacing(25); + + checkbox = new QCheckBox(title); + QLabel *label = new QLabel(description); + label->setWordWrap(true); + + // TODO: show descriptions on tap + //vlayout->addSpacing(50); + vlayout->addWidget(checkbox); + //vlayout->addWidget(label); + //vlayout->addSpacing(50); + hlayout->addLayout(vlayout); + + setLayout(hlayout); + + checkbox->setChecked(Params().read_db_bool(param.toStdString().c_str())); + + setStyleSheet(R"( + QCheckBox { + font-size: 70px; + } + QCheckBox::indicator { + width: 100px; + height: 100px; + } + QCheckBox::indicator:unchecked { + image: url(../assets/offroad/circled-checkmark-empty.png); + } + QCheckBox::indicator:checked { + image: url(../assets/offroad/circled-checkmark.png); + } + QLabel { font-size: 40px } + * { + background-color: #114265; + } + )"); + + QObject::connect(checkbox, SIGNAL(stateChanged(int)), this, SLOT(checkboxClicked(int))); +} + +void ParamsToggle::checkboxClicked(int state){ + char value = state ? '1': '0'; + Params().write_db_value(param.toStdString().c_str(), &value, 1); +} + +QWidget * toggles_panel() { + + QVBoxLayout *toggles_list = new QVBoxLayout(); + toggles_list->setSpacing(25); + + toggles_list->addWidget(new ParamsToggle("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(new ParamsToggle("LaneChangeEnabled", + "Enable Lane Change Assist", + "Perform assisted lane changes with openpilot by checking your surroundings for safety, activating the turn signal and gently nudging the steering wheel towards your desired lane. openpilot is not capable of checking if a lane change is safe. You must continuously observe your surroundings to use this feature.", + "../assets/offroad/icon_road.png" + )); + toggles_list->addWidget(new ParamsToggle("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(new ParamsToggle("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(new ParamsToggle("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(new ParamsToggle("IsMetric", + "Use Metric System", + "Display speed in km/h instead of mp/h.", + "../assets/offroad/icon_metric.png" + )); + toggles_list->addWidget(new ParamsToggle("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" + )); + + QWidget *widget = new QWidget; + widget->setLayout(toggles_list); + return widget; +} + +QWidget * device_panel() { + + QVBoxLayout *device_layout = new QVBoxLayout; + device_layout->setSpacing(50); + + Params params = Params(); + std::vector> labels = { + {"Dongle ID", params.get("DongleId", false)}, + //{"Serial Number", "abcdefghijk"}, + }; + + for (auto l : labels) { + QString text = QString::fromStdString(l.first + ": " + l.second); + device_layout->addWidget(new QLabel(text)); + } + + QPushButton *clear_cal_btn = new QPushButton("Reset Calibration"); + device_layout->addWidget(clear_cal_btn); + QObject::connect(clear_cal_btn, &QPushButton::released, [=]() { + Params().delete_db_value("CalibrationParams"); + }); + + std::map power_btns = { + {"Power Off", "sudo poweroff"}, + {"Reboot", "sudo reboot"}, + }; + + for (auto b : power_btns) { + QPushButton *btn = new QPushButton(QString::fromStdString(b.first)); + device_layout->addWidget(btn); +#ifdef __aarch64__ + QObject::connect(btn, &QPushButton::released, + [=]() {std::system(b.second);}); +#endif + } + + QWidget *widget = new QWidget; + widget->setLayout(device_layout); + widget->setStyleSheet(R"( + QPushButton { + padding: 60px; + } + )"); + return widget; +} + +QWidget * developer_panel() { + QVBoxLayout *main_layout = new QVBoxLayout; + + // TODO: enable SSH toggle and github keys + + Params params = Params(); + std::string brand = params.read_db_bool("Passive") ? "dashcam" : "openpilot"; + std::string os_version = util::read_file("/VERSION"); + std::vector> labels = { + {"Version", brand + " v" + params.get("Version", false)}, + {"OS Version", os_version}, + {"Git Branch", params.get("GitBranch", false)}, + {"Git Commit", params.get("GitCommit", false).substr(0, 10)}, + {"Panda Firmware", params.get("PandaFirmwareHex", false)}, + }; + + for (auto l : labels) { + QString text = QString::fromStdString(l.first + ": " + l.second); + main_layout->addWidget(new QLabel(text)); + } + + QWidget *widget = new QWidget; + widget->setLayout(main_layout); + return widget; +} + +QWidget * network_panel() { + QVBoxLayout *main_layout = new QVBoxLayout; + + main_layout->addWidget(new WifiUI()); + + QWidget *widget = new QWidget; + widget->setLayout(main_layout); + return widget; +} + + +void SettingsWindow::setActivePanel() { + QPushButton *btn = qobject_cast(sender()); + panel_layout->setCurrentWidget(panels[btn->text()]); +} + +SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { + + // sidebar + QVBoxLayout *sidebar_layout = new QVBoxLayout(); + panel_layout = new QStackedLayout(); + + // close button + QPushButton *close_button = new QPushButton("X"); + close_button->setStyleSheet(R"( + QPushButton { + padding: 50px; + font-weight: bold; + font-size: 100px; + } + )"); + sidebar_layout->addWidget(close_button); + QObject::connect(close_button, SIGNAL(released()), this, SIGNAL(closeSettings())); + + // setup panels + panels = { + {"device", device_panel()}, + {"toggles", toggles_panel()}, + {"developer", developer_panel()}, + {"network", network_panel()}, + }; + + for (auto &panel : panels) { + QPushButton *btn = new QPushButton(panel.first); + btn->setStyleSheet(R"( + QPushButton { + padding-top: 35px; + padding-bottom: 35px; + font-size: 60px; + text-align: right; + border: none; + background: none; + font-weight: bold; + } + )"); + + sidebar_layout->addWidget(btn); + panel_layout->addWidget(panel.second); + QObject::connect(btn, SIGNAL(released()), this, SLOT(setActivePanel())); + } + + QHBoxLayout *settings_layout = new QHBoxLayout(); + settings_layout->addSpacing(45); + settings_layout->addLayout(sidebar_layout); + settings_layout->addSpacing(45); + settings_layout->addLayout(panel_layout); + settings_layout->addSpacing(45); + setLayout(settings_layout); + + setStyleSheet(R"( + * { + color: white; + font-size: 50px; + } + )"); +} diff --git a/selfdrive/ui/qt/settings.hpp b/selfdrive/ui/qt/offroad/settings.hpp similarity index 77% rename from selfdrive/ui/qt/settings.hpp rename to selfdrive/ui/qt/offroad/settings.hpp index e65e75bc76e2ae..e4f65a6944c8a7 100644 --- a/selfdrive/ui/qt/settings.hpp +++ b/selfdrive/ui/qt/offroad/settings.hpp @@ -4,25 +4,36 @@ #include #include #include +#include + class ParamsToggle : public QFrame { Q_OBJECT +public: + explicit ParamsToggle(QString param, QString title, QString description, QString icon, QWidget *parent = 0); + private: QCheckBox *checkbox; QString param; -public: - explicit ParamsToggle(QString param, QString title, QString description, QString icon, QWidget *parent = 0); + public slots: void checkboxClicked(int state); }; - - class SettingsWindow : public QWidget { Q_OBJECT + public: explicit SettingsWindow(QWidget *parent = 0); + signals: void closeSettings(); + +private: + std::map panels; + QStackedLayout *panel_layout; + +private slots: + void setActivePanel(); }; diff --git a/selfdrive/ui/qt/offroad/wifi.cc b/selfdrive/ui/qt/offroad/wifi.cc new file mode 100644 index 00000000000000..00088d7bd2e134 --- /dev/null +++ b/selfdrive/ui/qt/offroad/wifi.cc @@ -0,0 +1,192 @@ +#include +#include +#include +#include +#include +#include + +#include "wifi.hpp" + + +void clearLayout(QLayout* layout) { + while (QLayoutItem* item = layout->takeAt(0)) { + if (QWidget* widget = item->widget()){ + widget->deleteLater(); + } + if (QLayout* childLayout = item->layout()) { + clearLayout(childLayout); + } + delete item; + } +} + +WifiUI::WifiUI(QWidget *parent) : QWidget(parent) { + wifi = new WifiManager; + + QVBoxLayout * top_layout = new QVBoxLayout; + swidget = new QStackedWidget; + + // Networks page + wifi_widget = new QWidget; + vlayout = new QVBoxLayout; + wifi_widget->setLayout(vlayout); + swidget->addWidget(wifi_widget); + + // Keyboard page + a = new InputField(); + QObject::connect(a, SIGNAL(emitText(QString)), this, SLOT(receiveText(QString))); + swidget->addWidget(a); + swidget->setCurrentIndex(0); + + top_layout->addWidget(swidget); + setLayout(top_layout); + a->setStyleSheet(R"( + QLineEdit { + background-color: #114265; + } + )"); + + // TODO: implement (not) connecting with wrong password + + // Update network list + timer = new QTimer(this); + QObject::connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); + timer->start(400); + + // Scan on startup + wifi->request_scan(); + page = 0; +} + +void WifiUI::refresh() { + if (!this->isVisible()) { + return; + } + + wifi->request_scan(); + wifi->refreshNetworks(); + + clearLayout(vlayout); + + connectButtons = new QButtonGroup(this); + QObject::connect(connectButtons, SIGNAL(buttonClicked(QAbstractButton*)), this, SLOT(handleButton(QAbstractButton*))); + + int i = 0; + for (Network &network : wifi->seen_networks){ + QHBoxLayout *hlayout = new QHBoxLayout; + + if(page * networks_per_page <= i && i < (page + 1) * networks_per_page){ + // SSID + hlayout->addSpacing(50); + hlayout->addWidget(new QLabel(QString::fromUtf8(network.ssid))); + + // 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); + hlayout->addSpacing(20); + + // connect button + QPushButton* btn = new QPushButton(network.connected ? "Connected" : "Connect"); + btn->setFixedWidth(300); + btn->setDisabled(network.connected || network.security_type == SecurityType::UNSUPPORTED); + hlayout->addWidget(btn); + hlayout->addSpacing(20); + + connectButtons->addButton(btn, i); + + QWidget * w = new QWidget; + w->setLayout(hlayout); + vlayout->addWidget(w); + w->setStyleSheet(R"( + QLabel { + font-size: 40px; + } + QPushButton:enabled { + background-color: #114265; + } + QPushButton:disabled { + background-color: #323C43; + } + * { + background-color: #114265; + } + )"); + } + i+=1; + } + QHBoxLayout *prev_next_buttons = new QHBoxLayout; + QPushButton* prev = new QPushButton("Previous"); + prev->setEnabled(page); + prev->setFixedHeight(100); + + QPushButton* next = new QPushButton("Next"); + next->setFixedHeight(100); + //If there are more visible networks then we can show, enable going to next page + if(wifi->seen_networks.size() > (page + 1) * networks_per_page){ + next->setEnabled(true); + }else{ + next->setDisabled(true); + } + QObject::connect(prev, SIGNAL(released()), this, SLOT(prevPage())); + QObject::connect(next, SIGNAL(released()), this, SLOT(nextPage())); + prev_next_buttons->addWidget(prev); + prev_next_buttons->addWidget(next); + + QWidget * w = new QWidget; + w->setLayout(prev_next_buttons); + w->setStyleSheet(R"( + QPushButton:enabled { + background-color: #114265; + } + QPushButton:disabled { + background-color: #323C43; + } + * { + background-color: #114265; + } + )"); + vlayout->addWidget(w); +} + +void WifiUI::handleButton(QAbstractButton* button) { + QPushButton* btn = static_cast(button); + qDebug() << connectButtons->id(btn); + Network n = wifi->seen_networks[connectButtons->id(btn)]; + + a->label->setText("Enter password for \"" + n.ssid + "\""); + + if(n.security_type == SecurityType::OPEN){ + wifi->connect(n); + } else if (n.security_type == SecurityType::WPA){ + QString password = getStringFromUser(); + if(password.size()){ + wifi->connect(n, password); + } + } else { + qDebug() << "Cannot determine network's security type"; + } +} + +QString WifiUI::getStringFromUser(){ + swidget->setCurrentIndex(1); + loop.exec(); + swidget->setCurrentIndex(0); + return text; +} + +void WifiUI::receiveText(QString t) { + loop.quit(); + text = t; +} +void WifiUI::prevPage() { + page--; + refresh(); +} +void WifiUI::nextPage() { + page++; + refresh(); +} diff --git a/selfdrive/ui/qt/offroad/wifi.hpp b/selfdrive/ui/qt/offroad/wifi.hpp new file mode 100644 index 00000000000000..0343e6be5eabf5 --- /dev/null +++ b/selfdrive/ui/qt/offroad/wifi.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "wifiManager.hpp" +#include "input_field.hpp" + + +class WifiUI : public QWidget { + Q_OBJECT + +private: + WifiManager* wifi; + int page; + const int networks_per_page = 10; + + QStackedWidget* swidget; + QVBoxLayout* vlayout; + QWidget * wifi_widget; + + InputField *a; + QEventLoop loop; + QTimer * timer; + QString text; + QButtonGroup *connectButtons; + + QString getStringFromUser(); + +public: + explicit WifiUI(QWidget *parent = 0); + +private slots: + void handleButton(QAbstractButton* m_button); + void refresh(); + void receiveText(QString text); + void prevPage(); + void nextPage(); +}; diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc new file mode 100644 index 00000000000000..987b09c8e4a6e0 --- /dev/null +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -0,0 +1,261 @@ +#include +#include + +#include "wifiManager.hpp" + + +QString nm_path = "/org/freedesktop/NetworkManager"; +QString nm_settings_path = "/org/freedesktop/NetworkManager/Settings"; + +QString nm_iface = "org.freedesktop.NetworkManager"; +QString props_iface = "org.freedesktop.DBus.Properties"; +QString nm_settings_iface = "org.freedesktop.NetworkManager.Settings"; +QString nm_settings_conn_iface = "org.freedesktop.NetworkManager.Settings.Connection"; +QString device_iface = "org.freedesktop.NetworkManager.Device"; +QString wireless_device_iface = "org.freedesktop.NetworkManager.Device.Wireless"; +QString ap_iface = "org.freedesktop.NetworkManager.AccessPoint"; +QString connection_iface = "org.freedesktop.NetworkManager.Connection.Active"; + +QString nm_service = "org.freedesktop.NetworkManager"; + + +template +T get_response(QDBusMessage response){ + QVariant first = response.arguments().at(0); + QDBusVariant dbvFirst = first.value(); + QVariant vFirst = dbvFirst.variant(); + return vFirst.value(); +} + +bool compare_by_strength(const Network &a, const Network &b){ + if (a.connected) return true; + if (b.connected) return false; + return a.strength > b.strength; +} + +WifiManager::WifiManager(){ + qDBusRegisterMetaType(); + + adapter = get_adapter(); + has_adapter = adapter != ""; +} + +void WifiManager::refreshNetworks(){ + if (!has_adapter) return; + + bus = QDBusConnection::systemBus(); + seen_networks.clear(); + seen_ssids.clear(); + + for (Network &network : get_networks()){ + if(seen_ssids.count(network.ssid)){ + continue; + } + seen_ssids.push_back(network.ssid); + seen_networks.push_back(network); + } +} + +QList WifiManager::get_networks(){ + QList r; + QDBusInterface nm(nm_service, adapter, wireless_device_iface, bus); + QDBusMessage response = nm.call("GetAllAccessPoints"); + QVariant first = response.arguments().at(0); + + QString active_ap = get_active_ap(); + + const QDBusArgument &args = first.value(); + args.beginArray(); + while (!args.atEnd()) { + QDBusObjectPath path; + args >> path; + + QByteArray ssid = get_property(path.path(), "Ssid"); + unsigned int strength = get_ap_strength(path.path()); + SecurityType security = getSecurityType(path.path()); + Network network = {path.path(), ssid, strength, path.path()==active_ap, security}; + + if (ssid.length()){ + r.push_back(network); + } + } + args.endArray(); + + std::sort(r.begin(), r.end(), compare_by_strength); + return r; +} + +SecurityType WifiManager::getSecurityType(QString path){ + int sflag = get_property(path, "Flags").toInt(); + int wpaflag = get_property(path, "WpaFlags").toInt(); + int rsnflag = get_property(path, "RsnFlags").toInt(); + int wpa_props = wpaflag | rsnflag; + + if(sflag == 0){ + return SecurityType::OPEN; + } else if((sflag & 0x1) && (wpa_props & (0x333) && !(wpa_props & 0x200))) { + return SecurityType::WPA; + } else { + // qDebug() << "Cannot determine security type for " << get_property(path, "Ssid") << " with flags"; + // qDebug() << "flag " << sflag; + // qDebug() << "WpaFlag " << wpaflag; + // qDebug() << "RsnFlag " << rsnflag; + return SecurityType::UNSUPPORTED; + } +} + +void WifiManager::connect(Network n){ + return connect(n, "", ""); +} + +void WifiManager::connect(Network n, QString password){ + return connect(n, "", password); +} + +void WifiManager::connect(Network n, QString username, QString password){ + QString active_ap = get_active_ap(); + if (active_ap!="") { + clear_connections(get_property(active_ap, "Ssid")); + } + clear_connections(n.ssid); + qDebug() << "Connecting to"<< n.ssid << "with username, password =" << username << "," < result = nm_settings.call("AddConnection", QVariant::fromValue(connection)); + if (!result.isValid()) { + qDebug() << result.error().name() << result.error().message(); + } else { + qDebug() << result.value().path(); + } +} + +void WifiManager::print_active_connections(){ + //TO-DO clean up, the code is not currently in use. + QDBusInterface nm(nm_service, nm_path, props_iface, bus); + QDBusMessage response = nm.call("Get", nm_iface, "ActiveConnections"); + QVariant first = response.arguments().at(0); + QDBusVariant dbvFirst = first.value(); + QVariant vFirst = dbvFirst.variant(); + QDBusArgument step4 = vFirst.value(); + QDBusObjectPath path; + step4.beginArray(); + while (!step4.atEnd()){ + step4 >> path; + qDebug()<(); + args.beginArray(); + while (!args.atEnd()) { + QDBusObjectPath path; + args >> path; + QDBusInterface nm2(nm_service, path.path(), nm_settings_conn_iface, bus); + QDBusMessage response = nm2.call("GetSettings"); + + const QDBusArgument &dbusArg = response.arguments().at(0).value(); + + QMap > map; + dbusArg >> map; + for(QString outer_key : map.keys()) { + QMap innerMap = map.value(outer_key); + for(QString inner_key : innerMap.keys()) { + if(inner_key == "ssid"){ + QString value = innerMap.value(inner_key).value(); + if(value == ssid){ + // qDebug()<<"Deleting "<(response); + return resp; +} + +QString WifiManager::get_active_ap(){ + QDBusInterface device_props(nm_service, adapter, props_iface, bus); + QDBusMessage response = device_props.call("Get", wireless_device_iface, "ActiveAccessPoint"); + QDBusObjectPath r = get_response(response); + return r.path(); +} + +QByteArray WifiManager::get_property(QString network_path ,QString property){ + QDBusInterface device_props(nm_service, network_path, props_iface, bus); + QDBusMessage response = device_props.call("Get", ap_iface, property); + return get_response(response); +} + +unsigned int WifiManager::get_ap_strength(QString network_path){ + QDBusInterface device_props(nm_service, network_path, props_iface, bus); + QDBusMessage response = device_props.call("Get", ap_iface, "Strength"); + return get_response(response); +} + +QString WifiManager::get_adapter(){ + + QDBusInterface nm(nm_service, nm_path, nm_iface, bus); + QDBusMessage response = nm.call("GetDevices"); + QVariant first = response.arguments().at(0); + + QString adapter_path = ""; + + const QDBusArgument &args = first.value(); + args.beginArray(); + while (!args.atEnd()) { + QDBusObjectPath path; + args >> path; + + // Get device type + QDBusInterface device_props(nm_service, path.path(), props_iface, bus); + QDBusMessage response = device_props.call("Get", device_iface, "DeviceType"); + uint device_type = get_response(response); + + if (device_type == 2) { // Wireless + adapter_path = path.path(); + break; + } + } + args.endArray(); + + return adapter_path; +} diff --git a/selfdrive/ui/qt/offroad/wifiManager.hpp b/selfdrive/ui/qt/offroad/wifiManager.hpp new file mode 100644 index 00000000000000..e107f6ade31626 --- /dev/null +++ b/selfdrive/ui/qt/offroad/wifiManager.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +enum class SecurityType { + OPEN, + WPA, + UNSUPPORTED +}; + +typedef QMap> Connection; + +struct Network { + QString path; + QByteArray ssid; + unsigned int strength; + bool connected; + SecurityType security_type; +}; + +class WifiManager{ +public: + explicit WifiManager(); + + bool has_adapter; + void request_scan(); + QVector seen_networks; + + void refreshNetworks(); + void connect(Network ssid); + void connect(Network ssid, QString password); + void connect(Network ssid, QString username, QString password); + +private: + QVector seen_ssids; + QString adapter;//Path to network manager wifi-device + QDBusConnection bus = QDBusConnection::systemBus(); + + QString get_adapter(); + QList get_networks(); + void connect(QByteArray ssid, QString username, QString password, SecurityType security_type); + QString get_active_ap(); + void clear_connections(QString ssid); + void print_active_connections(); + uint get_wifi_device_state(); + QByteArray get_property(QString network_path, QString property); + unsigned int get_ap_strength(QString network_path); + SecurityType getSecurityType(QString ssid); +}; diff --git a/selfdrive/ui/qt/qt_sound.cc b/selfdrive/ui/qt/qt_sound.cc index c44e27ec97e41a..c18b2d585023a3 100644 --- a/selfdrive/ui/qt/qt_sound.cc +++ b/selfdrive/ui/qt/qt_sound.cc @@ -1,5 +1,5 @@ #include -#include "qt/qt_sound.hpp" +#include "qt_sound.hpp" QtSound::QtSound() { for (auto &kv : sound_map) { @@ -9,22 +9,22 @@ QtSound::QtSound() { } bool QtSound::play(AudibleAlert alert) { - sounds[alert].setLoopCount(sound_map[alert].second>-1 ? sound_map[alert].second : QSoundEffect::Infinite); - sounds[alert].setVolume(0.9); + int loops = sound_map[alert].second> - 1 ? sound_map[alert].second : QSoundEffect::Infinite; + sounds[alert].setLoopCount(loops); + sounds[alert].setVolume(0.7); sounds[alert].play(); return true; } void QtSound::stop() { for (auto &kv : sounds) { - kv.second.stop(); + // Only stop repeating sounds + if (sound_map[kv.first].second != 0) { + kv.second.stop(); + } } } void QtSound::setVolume(int volume) { // TODO: implement this } - -QtSound::~QtSound() { - -} diff --git a/selfdrive/ui/qt/qt_sound.hpp b/selfdrive/ui/qt/qt_sound.hpp index c2aab2de446ff3..8c7f8aa348e963 100644 --- a/selfdrive/ui/qt/qt_sound.hpp +++ b/selfdrive/ui/qt/qt_sound.hpp @@ -6,7 +6,6 @@ class QtSound : public Sound { public: QtSound(); - ~QtSound(); bool play(AudibleAlert alert); void stop(); void setVolume(int volume); diff --git a/selfdrive/ui/qt/settings.cc b/selfdrive/ui/qt/settings.cc deleted file mode 100644 index 728b3d84b5357e..00000000000000 --- a/selfdrive/ui/qt/settings.cc +++ /dev/null @@ -1,140 +0,0 @@ -#include -#include -#include -#include - -#include "qt/settings.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/params.h" - -ParamsToggle::ParamsToggle(QString param, QString title, QString description, QString icon_path, QWidget *parent): QFrame(parent) , param(param) { - QHBoxLayout *hlayout = new QHBoxLayout; - QVBoxLayout *vlayout = new QVBoxLayout; - - hlayout->addSpacing(25); - if (icon_path.length()){ - QPixmap pix(icon_path); - QLabel *icon = new QLabel(); - icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); - icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); - hlayout->addWidget(icon); - } else{ - hlayout->addSpacing(100); - } - hlayout->addSpacing(25); - - checkbox = new QCheckBox(title); - QLabel *label = new QLabel(description); - label->setWordWrap(true); - - vlayout->addWidget(checkbox); - vlayout->addWidget(label); - hlayout->addLayout(vlayout); - - setLayout(hlayout); - - checkbox->setChecked(Params().read_db_bool(param.toStdString().c_str())); - - setStyleSheet(R"( - QCheckBox { font-size: 70px } - QLabel { font-size: 40px } - * { - background-color: #114265; - } - )"); - - QObject::connect(checkbox, SIGNAL(stateChanged(int)), this, SLOT(checkboxClicked(int))); -} - -void ParamsToggle::checkboxClicked(int state){ - char value = state ? '1': '0'; - Params().write_db_value(param.toStdString().c_str(), &value, 1); -} - -SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { - QWidget *container = new QWidget(this); - - QVBoxLayout *settings_list = new QVBoxLayout(); - settings_list->addWidget(new ParamsToggle("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" - )); - settings_list->addWidget(new ParamsToggle("LaneChangeEnabled", - "Enable Lane Change Assist", - "Perform assisted lane changes with openpilot by checking your surroundings for safety, activating the turn signal and gently nudging the steering wheel towards your desired lane. openpilot is not capable of checking if a lane change is safe. You must continuously observe your surroundings to use this feature.", - "../assets/offroad/icon_road.png" - )); - settings_list->addWidget(new ParamsToggle("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" - )); - settings_list->addWidget(new ParamsToggle("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" - )); - settings_list->addWidget(new ParamsToggle("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" - )); - settings_list->addWidget(new ParamsToggle("IsMetric", - "Use Metric System", - "Display speed in km/h instead of mp/h.", - "../assets/offroad/icon_metric.png" - )); - settings_list->addWidget(new ParamsToggle("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" - )); - - settings_list->setSpacing(25); - - container->setLayout(settings_list); - container->setFixedWidth(1650); - - QScrollArea *scrollArea = new QScrollArea; - scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - scrollArea->setWidget(container); - - QScrollerProperties sp; - sp.setScrollMetric(QScrollerProperties::DecelerationFactor, 2.0); - - QScroller* qs = QScroller::scroller(scrollArea); - qs->setScrollerProperties(sp); - - QHBoxLayout *main_layout = new QHBoxLayout; - main_layout->addSpacing(50); - main_layout->addWidget(scrollArea); - - QPushButton * button = new QPushButton("Close"); - main_layout->addWidget(button); - main_layout->addSpacing(20); - - setLayout(main_layout); - - QScroller::grabGesture(scrollArea, QScroller::LeftMouseButtonGesture); - QObject::connect(button, SIGNAL(clicked()), this, SIGNAL(closeSettings())); - - setStyleSheet(R"( - QPushButton { font-size: 40px } - * { - color: white; - background-color: #072339; - } - )"); -} diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc new file mode 100644 index 00000000000000..540b58f1401b76 --- /dev/null +++ b/selfdrive/ui/qt/spinner.cc @@ -0,0 +1,124 @@ +#include +#include +#include + +#include +#include +#include +#include + +#ifdef QCOM2 +#include +#include +#include +#endif + +#include "spinner.hpp" + + +Spinner::Spinner(QWidget *parent) { + QGridLayout *main_layout = new QGridLayout(); + main_layout->setSpacing(0); + main_layout->setContentsMargins(200, 200, 200, 200); + + const int img_size = 360; + + comma = new QLabel(); + comma->setPixmap(QPixmap("../assets/img_spinner_comma.png").scaled(img_size, img_size, Qt::KeepAspectRatio, Qt::SmoothTransformation)); + comma->setFixedSize(img_size, img_size); + main_layout->addWidget(comma, 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); + + track_img = QPixmap("../assets/img_spinner_track.png").scaled(img_size, img_size, Qt::KeepAspectRatio, Qt::SmoothTransformation); + track = new QLabel(); + track->setPixmap(track_img); + track->setFixedSize(img_size, img_size); + main_layout->addWidget(track, 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); + + text = new QLabel(); + text->setVisible(false); + main_layout->addWidget(text, 1, 0, Qt::AlignHCenter); + + progress_bar = new QProgressBar(); + progress_bar->setRange(5, 100); + progress_bar->setTextVisible(false); + progress_bar->setVisible(false); + main_layout->addWidget(progress_bar, 1, 0, Qt::AlignHCenter); + + setLayout(main_layout); + setStyleSheet(R"( + Spinner { + background-color: black; + } + QLabel { + color: white; + font-size: 80px; + } + QProgressBar { + background-color: #373737; + height: 20px; + width: 1000px; + border solid white; + border-radius: 10px; + } + QProgressBar::chunk { + border-radius: 10px; + background-color: white; + } + )"); + + rotate_timer = new QTimer(this); + rotate_timer->start(1000/30.); + 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() { + transform.rotate(5); + + QPixmap r = track_img.transformed(transform.rotate(5), Qt::SmoothTransformation); + int x = (r.width() - track->width()) / 2; + int y = (r.height() - track->height()) / 2; + track->setPixmap(r.copy(x, y, track->width(), track->height())); +}; + +void Spinner::update(int n) { + std::string line; + std::getline(std::cin, line); + + if (line.length()) { + bool number = std::all_of(line.begin(), line.end(), ::isdigit); + text->setVisible(!number); + progress_bar->setVisible(number); + text->setText(QString::fromStdString(line)); + if (number) { + progress_bar->setValue(std::stoi(line)); + } + } +} + +int main(int argc, char *argv[]) { + QApplication a(argc, argv); + + Spinner *spinner = new Spinner(); + + // TODO: get size from QScreen, doesn't work on tici +#ifdef QCOM2 + int w = 2160, h = 1080; +#else + int w = 1920, h = 1080; +#endif + spinner->setFixedSize(w, h); + spinner->show(); + +#ifdef QCOM2 + QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); + wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", spinner->windowHandle())); + wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); + wl_surface_commit(s); + spinner->showFullScreen(); +#endif + + return a.exec(); +} diff --git a/selfdrive/ui/qt/spinner.hpp b/selfdrive/ui/qt/spinner.hpp new file mode 100644 index 00000000000000..ea0f8afdcf5ca8 --- /dev/null +++ b/selfdrive/ui/qt/spinner.hpp @@ -0,0 +1,27 @@ +#include +#include +#include +#include +#include +#include +#include + +class Spinner : public QWidget { + Q_OBJECT + +public: + explicit Spinner(QWidget *parent = 0); + +private: + QPixmap track_img; + QTimer *rotate_timer; + QLabel *comma, *track; + QLabel *text; + QProgressBar *progress_bar; + QTransform transform; + QSocketNotifier *notifier; + +public slots: + void rotate(); + void update(int n); +}; diff --git a/selfdrive/ui/qt/text.cc b/selfdrive/ui/qt/text.cc new file mode 100644 index 00000000000000..ab7d9f4c4b41d2 --- /dev/null +++ b/selfdrive/ui/qt/text.cc @@ -0,0 +1,90 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#ifdef QCOM2 +#include +#include +#include +#endif + + +int main(int argc, char *argv[]) { + QApplication a(argc, argv); + QWidget *window = new QWidget(); + + // TODO: get size from QScreen, doesn't work on tici +#ifdef QCOM2 + int w = 2160, h = 1080; +#else + int w = 1920, h = 1080; +#endif + window->setFixedSize(w, h); + + QVBoxLayout *main_layout = new QVBoxLayout(); + + QString text = ""; + for (int i = 1; i < argc; i++) { + if (i > 1) { + text.append(" "); + } + text.append(argv[i]); + } + + QLabel *label = new QLabel(text); + label->setAlignment(Qt::AlignTop); + main_layout->addWidget(label); + + QPushButton *btn = new QPushButton(); +#ifdef __aarch64__ + btn->setText("Reboot"); + QObject::connect(btn, &QPushButton::released, [=]() { + std::system("sudo reboot"); + }); +#else + btn->setText("Exit"); + QObject::connect(btn, SIGNAL(released()), &a, SLOT(quit())); +#endif + main_layout->addWidget(btn); + + window->setLayout(main_layout); + window->setStyleSheet(R"( + QWidget { + margin: 60px; + background-color: black; + } + QLabel { + color: white; + font-size: 60px; + } + QPushButton { + color: white; + font-size: 50px; + padding: 60px; + margin-left: 1500px; + border-color: white; + border-width: 2px; + border-style: solid; + border-radius: 20px; + } + )"); + + window->show(); + + +#ifdef QCOM2 + QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); + wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", window->windowHandle())); + wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); + wl_surface_commit(s); + window->showFullScreen(); +#endif + + return a.exec(); +} diff --git a/selfdrive/ui/qt/ui.cc b/selfdrive/ui/qt/ui.cc index aece1742abfe51..de4f6950eeef04 100644 --- a/selfdrive/ui/qt/ui.cc +++ b/selfdrive/ui/qt/ui.cc @@ -25,6 +25,8 @@ int main(int argc, char *argv[]) { w.setFixedSize(vwp_w, vwp_h); w.show(); + a.installEventFilter(&w); + #ifdef QCOM2 QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", w.windowHandle())); diff --git a/selfdrive/ui/qt/wifi.cc b/selfdrive/ui/qt/wifi.cc deleted file mode 100644 index 1d66c2c8623241..00000000000000 --- a/selfdrive/ui/qt/wifi.cc +++ /dev/null @@ -1,69 +0,0 @@ -#include -#include - -typedef QMap > Connection; -Q_DECLARE_METATYPE(Connection) - -void wifi_stuff(){ - qDBusRegisterMetaType(); - - QString nm_path = "/org/freedesktop/NetworkManager"; - QString nm_settings_path = "/org/freedesktop/NetworkManager/Settings"; - - QString nm_iface = "org.freedesktop.NetworkManager"; - QString props_iface = "org.freedesktop.DBus.Properties"; - QString nm_settings_iface = "org.freedesktop.NetworkManager.Settings"; - - QString nm_service = "org.freedesktop.NetworkManager"; - QString device_service = "org.freedesktop.NetworkManager.Device"; - - QDBusConnection bus = QDBusConnection::systemBus(); - - // Get devices - QDBusInterface nm(nm_service, nm_path, nm_iface, bus); - QDBusMessage response = nm.call("GetDevices"); - QVariant first = response.arguments().at(0); - - const QDBusArgument &args = first.value(); - args.beginArray(); - while (!args.atEnd()) { - QDBusObjectPath path; - args >> path; - - // Get device type - QDBusInterface device_props(nm_service, path.path(), props_iface, bus); - QDBusMessage response = device_props.call("Get", device_service, "DeviceType"); - QVariant first = response.arguments().at(0); - QDBusVariant dbvFirst = first.value(); - QVariant vFirst = dbvFirst.variant(); - uint device_type = vFirst.value(); - qDebug() << path.path() << device_type; - } - args.endArray(); - - - // Add connection - Connection connection; - connection["connection"]["type"] = "802-11-wireless"; - connection["connection"]["uuid"] = QUuid::createUuid().toString().remove('{').remove('}'); - connection["connection"]["id"] = "Connection 1"; - - connection["802-11-wireless"]["ssid"] = QByteArray(""); - connection["802-11-wireless"]["mode"] = "infrastructure"; - - connection["802-11-wireless-security"]["key-mgmt"] = "wpa-psk"; - connection["802-11-wireless-security"]["auth-alg"] = "open"; - connection["802-11-wireless-security"]["psk"] = ""; - - connection["ipv4"]["method"] = "auto"; - connection["ipv6"]["method"] = "ignore"; - - - QDBusInterface nm_settings(nm_service, nm_settings_path, nm_settings_iface, bus); - QDBusReply result = nm_settings.call("AddConnection", QVariant::fromValue(connection)); - if (!result.isValid()) { - qDebug() << result.error().name() << result.error().message(); - } else { - qDebug() << result.value().path(); - } -} diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index ceea1219b19167..e5ee59cb0efd15 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -8,11 +8,11 @@ #include #include -#include -#include #include "window.hpp" -#include "settings.hpp" +#include "offroad/input_field.hpp" +#include "offroad/settings.hpp" +#include "offroad/onboarding.hpp" #include "paint.hpp" #include "common/util.h" @@ -23,6 +23,18 @@ volatile sig_atomic_t do_exit = 0; +static void handle_display_state(UIState *s, int dt, bool user_input) { + static int awake_timeout = 0; + awake_timeout = std::max(awake_timeout-dt, 0); + + if (user_input || s->ignition || s->started) { + s->awake = true; + awake_timeout = 30*UI_FREQ; + } else if (awake_timeout == 0){ + s->awake = false; + } +} + static void set_backlight(int brightness){ std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness"); if (brightness_control.is_open()){ @@ -38,18 +50,25 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { set_core_affinity(7); #endif - GLWindow * glWindow = new GLWindow(this); + glWindow = new GLWindow(this); main_layout->addWidget(glWindow); - SettingsWindow * settingsWindow = new SettingsWindow(this); + settingsWindow = new SettingsWindow(this); main_layout->addWidget(settingsWindow); + onboardingWindow = new OnboardingWindow(this); + main_layout->addWidget(onboardingWindow); main_layout->setMargin(0); setLayout(main_layout); QObject::connect(glWindow, SIGNAL(openSettings()), this, SLOT(openSettings())); QObject::connect(settingsWindow, SIGNAL(closeSettings()), this, SLOT(closeSettings())); + // start at onboarding + main_layout->setCurrentWidget(onboardingWindow); + QObject::connect(onboardingWindow, SIGNAL(onboardingDone()), this, SLOT(closeSettings())); + onboardingWindow->updateActiveScreen(); + setStyleSheet(R"( * { color: white; @@ -67,6 +86,14 @@ void MainWindow::closeSettings() { } +bool MainWindow::eventFilter(QObject *obj, QEvent *event){ + if (event->type() == QEvent::MouseButtonPress) { + glWindow->wake(); + } + return false; +} + + GLWindow::GLWindow(QWidget *parent) : QOpenGLWidget(parent) { timer = new QTimer(this); QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); @@ -101,6 +128,8 @@ void GLWindow::initializeGL() { ui_state->fb_h = vwp_h; ui_init(ui_state); + wake(); + timer->start(0); backlight_timer->start(BACKLIGHT_DT * 100); } @@ -113,11 +142,9 @@ void GLWindow::backlightUpdate(){ smooth_brightness = clipped_brightness * k + smooth_brightness * (1.0f - k); int brightness = smooth_brightness; -#ifdef QCOM2 - if (!ui_state->started){ + if (!ui_state->awake){ brightness = 0; } -#endif std::thread{set_backlight, brightness}.detach(); } @@ -130,6 +157,10 @@ void GLWindow::timerUpdate(){ } #endif + // Fix awake timeout if running 1 Hz when offroad + int dt = timer->interval() == 0 ? 1 : 20; + handle_display_state(ui_state, dt, false); + ui_update(ui_state); repaint(); } @@ -142,7 +173,16 @@ void GLWindow::paintGL() { ui_draw(ui_state); } +void GLWindow::wake(){ + // UI state might not be initialized yet + if (ui_state != nullptr){ + handle_display_state(ui_state, 1, true); + } +} + void GLWindow::mousePressEvent(QMouseEvent *e) { + wake(); + // Settings button click if (!ui_state->scene.uilayout_sidebarcollapsed && settings_btn.ptInRect(e->x(), e->y())) { emit openSettings(); diff --git a/selfdrive/ui/qt/window.hpp b/selfdrive/ui/qt/window.hpp index 8c82e6023952b8..1f5a4c10ee72e9 100644 --- a/selfdrive/ui/qt/window.hpp +++ b/selfdrive/ui/qt/window.hpp @@ -2,30 +2,14 @@ #include #include -#include -#include #include #include #include #include "qt/qt_sound.hpp" #include "ui/ui.hpp" - -class MainWindow : public QWidget -{ - Q_OBJECT - -public: - explicit MainWindow(QWidget *parent = 0); - -private: - QStackedLayout *main_layout; - -public slots: - void openSettings(); - void closeSettings(); - -}; +#include "offroad/settings.hpp" +#include "offroad/onboarding.hpp" #ifdef QCOM2 const int vwp_w = 2160; @@ -33,13 +17,15 @@ const int vwp_w = 2160; const int vwp_w = 1920; #endif const int vwp_h = 1080; -class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions -{ + + +class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions { Q_OBJECT public: using QOpenGLWidget::QOpenGLWidget; explicit GLWindow(QWidget *parent = 0); + void wake(); ~GLWindow(); protected: @@ -48,16 +34,16 @@ class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions void resizeGL(int w, int h) override; void paintGL() override; - private: QTimer * timer; QTimer * backlight_timer; - UIState * ui_state; + UIState * ui_state = nullptr; QtSound sound; bool onroad = true; - QLabel * label = NULL; + + // TODO: this shouldn't be here float brightness_b = 0; float brightness_m = 0; float smooth_brightness = 0; @@ -69,3 +55,23 @@ public slots: signals: void openSettings(); }; + +class MainWindow : public QWidget { + Q_OBJECT + +protected: + bool eventFilter(QObject *obj, QEvent *event) override; + +public: + explicit MainWindow(QWidget *parent = 0); + +private: + QStackedLayout *main_layout; + GLWindow *glWindow; + SettingsWindow *settingsWindow; + OnboardingWindow *onboardingWindow; + +public slots: + void openSettings(); + void closeSettings(); +}; diff --git a/selfdrive/ui/spinner b/selfdrive/ui/spinner new file mode 100755 index 00000000000000..c2ef33b7ec5c05 --- /dev/null +++ b/selfdrive/ui/spinner @@ -0,0 +1,12 @@ +#!/bin/sh + +if [ -f /EON ]; then + export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" + exec ./android/spinner/spinner "$1" +else + if [ -f /TICI ] && [ ! -f qt/spinner ]; then + cp qt/spinner_aarch64 qt/spinner + fi + + exec ./qt/spinner "$1" +fi diff --git a/selfdrive/ui/spinner/spinner.c b/selfdrive/ui/spinner/spinner.c deleted file mode 100644 index 14b452a49ba438..00000000000000 --- a/selfdrive/ui/spinner/spinner.c +++ /dev/null @@ -1,21 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "common/framebuffer.h" -#include "common/spinner.h" - -int main(int argc, char** argv) { - int err; - - spin(argc, argv); - - return 0; -} diff --git a/selfdrive/ui/text b/selfdrive/ui/text new file mode 100755 index 00000000000000..085425e8aca700 --- /dev/null +++ b/selfdrive/ui/text @@ -0,0 +1,12 @@ +#!/bin/sh + +if [ -f /EON ]; then + export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" + exec ./android/text/text "$1" +else + if [ -f /TICI ] && [ ! -f qt/text ]; then + cp qt/text_aarch64 qt/text + fi + + exec ./qt/text "$1" +fi diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4ebc50f06765b3..c9f321b0930162 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -24,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param } void ui_init(UIState *s) { - s->sm = new SubMaster({"model", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", + s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents"}); s->started = false; @@ -106,13 +107,6 @@ void ui_update_vision(UIState *s) { s->vision_connected = false; } -static inline void fill_path_points(const cereal::ModelData::PathData::Reader &path, float *points) { - const capnp::List::Reader &poly = path.getPoly(); - for (int i = 0; i < path.getValidLen(); i++) { - points[i] = poly[0] * (i * i * i) + poly[1] * (i * i) + poly[2] * i + poly[3]; - } -} - void update_sockets(UIState *s) { UIScene &scene = s->scene; @@ -178,11 +172,24 @@ void update_sockets(UIState *s) { scene.extrinsic_matrix.v[i] = extrinsicl[i]; } } - if (sm.updated("model")) { - scene.model = sm["model"].getModel(); - fill_path_points(scene.model.getPath(), scene.path_points); - fill_path_points(scene.model.getLeftLane(), scene.left_lane_points); - fill_path_points(scene.model.getRightLane(), scene.right_lane_points); + if (sm.updated("modelV2")) { + scene.model = sm["modelV2"].getModelV2(); + scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); + for (int ll_idx = 0; ll_idx < 4; ll_idx++) { + if (scene.model.getLaneLineProbs().size() > ll_idx) { + scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx]; + } else { + scene.lane_line_probs[ll_idx] = 0.0; + } + } + + for (int re_idx = 0; re_idx < 2; re_idx++) { + if (scene.model.getRoadEdgeStds().size() > re_idx) { + scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx]; + } else { + scene.road_edge_stds[re_idx] = 1.0; + } + } } if (sm.updated("uiLayoutState")) { auto data = sm["uiLayoutState"].getUiLayoutState(); @@ -243,6 +250,7 @@ void ui_update(UIState *s) { s->status = STATUS_OFFROAD; s->active_app = cereal::UiLayoutState::App::HOME; s->scene.uilayout_sidebarcollapsed = false; + s->sound->stop(); } else if (s->started && s->status == STATUS_OFFROAD) { s->status = STATUS_DISENGAGED; s->started_frame = s->sm->frame; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 1e2d91136cba27..023bc866727b05 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -55,9 +55,8 @@ const Rect home_btn = {60, 1080 - 180 - 40, 180, 180}; const int UI_FREQ = 20; // Hz -const int MODEL_PATH_MAX_VERTICES_CNT = 98; -const int MODEL_LANE_PATH_CNT = 2; -const int TRACK_POINTS_MAX_CNT = 50 * 2; +const int MODEL_PATH_MAX_VERTICES_CNT = TRAJECTORY_SIZE*2; +const int TRACK_POINTS_MAX_CNT = TRAJECTORY_SIZE*4; const int SET_SPEED_NA = 255; @@ -83,10 +82,14 @@ static std::map bg_colors = { {STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)}, }; -typedef struct UIScene { +typedef struct { + float x[TRAJECTORY_SIZE]; + float y[TRAJECTORY_SIZE]; + float z[TRAJECTORY_SIZE]; +} line; - float mpc_x[50]; - float mpc_y[50]; + +typedef struct UIScene { mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. bool world_objects_visible; @@ -112,10 +115,17 @@ typedef struct UIScene { cereal::ControlsState::Reader controls_state; cereal::DriverState::Reader driver_state; cereal::DMonitoringState::Reader dmonitoring_state; - cereal::ModelData::Reader model; - float left_lane_points[MODEL_PATH_DISTANCE]; - float path_points[MODEL_PATH_DISTANCE]; - float right_lane_points[MODEL_PATH_DISTANCE]; + cereal::ModelDataV2::Reader model; + line path; + line outer_left_lane_line; + line left_lane_line; + line right_lane_line; + line outer_right_lane_line; + line left_road_edge; + line right_road_edge; + float max_distance; + float lane_line_probs[4]; + float road_edge_stds[2]; } UIScene; typedef struct { @@ -125,7 +135,7 @@ typedef struct { typedef struct { vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; int cnt; -} model_path_vertices_data; +} line_vertices_data; typedef struct { vertex_data v[TRACK_POINTS_MAX_CNT]; @@ -190,8 +200,9 @@ typedef struct UIState { bool alert_blinked; float alert_blinking_alpha; - track_vertices_data track_vertices[2]; - model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2]; + track_vertices_data track_vertices; + line_vertices_data lane_line_vertices[4]; + line_vertices_data road_edge_vertices[2]; Rect video_rect; } UIState; diff --git a/site_scons/site_tools/cython.py b/site_scons/site_tools/cython.py new file mode 100644 index 00000000000000..ad53f2831cc678 --- /dev/null +++ b/site_scons/site_tools/cython.py @@ -0,0 +1,37 @@ +import SCons +from SCons.Action import Action + +cythonAction = Action("$CYTHONCOM") + +def create_builder(env): + try: + cython = env['BUILDERS']['Cython'] + except KeyError: + cython = SCons.Builder.Builder( + action = cythonAction, + emitter = {}, + suffix = cython_suffix_emitter, + single_source = 1) + env['BUILDERS']['Cython'] = cython + return cython + +def cython_suffix_emitter(env, source): + return "$CYTHONCFILESUFFIX" + +def generate(env): + env["CYTHON"] = "cythonize" + env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE" + env["CYTHONCFILESUFFIX"] = ".cpp" + + c_file, _ = SCons.Tool.createCFileBuilders(env) + + c_file.suffix['.pyx'] = cython_suffix_emitter + c_file.add_action('.pyx', cythonAction) + + c_file.suffix['.py'] = cython_suffix_emitter + c_file.add_action('.py', cythonAction) + + create_builder(env) + +def exists(env): + return True