retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.8.0 release

v0.8
Vehicle Researcher 2020-11-24 21:53:25 +00:00
parent 8369e11417
commit 0b384ea101
149 changed files with 3512 additions and 1443 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CONTRIBUTING.md
View File

@ -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

22
README.md
View File

@ -66,7 +66,7 @@ Supported Cars
| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>1</sup> | 25mph |
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>1</sup> | 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 | 25mph<sup>1</sup> | 12mph |
| Honda | HR-V 2019 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
| Honda | Insight 2019-20 | All | Stock | 0mph | 3mph |
| Honda | Inspire 2018 | All | Stock | 0mph | 3mph |
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 0mph |
| Honda | Passport 2019 | All | openpilot | 25mph<sup>1</sup> | 12mph |
| Honda | Pilot 2016-19 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 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 | Stock<sup>3</sup>| 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 | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | NX Hybrid 2018 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | RX 2016-18 | All | Stock<sup>3</sup>| 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 | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
| Toyota | Camry 2018-20 | All | Stock | 0mph<sup>4</sup> | 0mph |
| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph<sup>4</sup> | 0mph |
| Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph<sup>4</sup> | 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 | Stock<sup>3</sup>| 20mph<sup>1</sup> | 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 | Stock<sup>3</sup>| 0mph | 0mph |
@ -108,11 +110,12 @@ Supported Cars
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Toyota | Highlander Hybrid 2020 | All | openpilot | 0mph | 0mph |
| Toyota | Prius 2016-20 | TSS-P | Stock<sup>3</sup>| 0mph | 0mph |
| Toyota | Prius 2021 | All | openpilot | 0mph | 0mph |
| Toyota | Prius Prime 2017-20 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Toyota | Rav4 2016-18 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
| Toyota | Rav4 2019-21 | All | openpilot | 0mph | 0mph |
| Toyota | Rav4 Hybrid 2016-18 | TSS-P | Stock<sup>3</sup>| 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 | Stock<sup>3</sup>| 0mph | 0mph |
<sup>1</sup>[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).*** <br />
@ -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 2018<sup>2</sup>| Adaptive Cruise | openpilot | 0mph | 7mph |
| GMC | Acadia 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Holden | Astra 2017<sup>1</sup> | 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 |
<sup>1</sup>Requires 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).*** <br />
<sup>2</sup>Requires a custom connector for the developer [car harness](https://comma.ai/shop/products/car-harness) <br />
<sup>1</sup>Requires 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).*** <br />
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

12
RELEASES.md
View File

@ -1,3 +1,15 @@
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!
* Toyota Prius 2021 support thanks to rav4kumar!
* 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

76
SConstruct
View File

@ -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'])

BIN
apk/ai.comma.plus.offroad.apk
View File

Binary file not shown.

5
cereal/SConscript
View File

@ -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([

8
cereal/car.capnp
View File

@ -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;
}
}

15
cereal/log.capnp
View File

@ -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;
}
}

16
common/SConscript
View File

@ -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')

1
common/clock.pyx
View File

@ -1,3 +1,4 @@
# distutils: language = c++
# cython: language_level = 3
from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t

20
common/common_pyx_setup.py
View File

@ -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,
),
)

7
common/kalman/SConscript
View File

@ -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')

1
common/kalman/simple_kalman_impl.pyx
View File

@ -1,3 +1,4 @@
# distutils: language = c++
# cython: language_level=3
cdef class KF1D:

10
common/kalman/simple_kalman_setup.py
View File

@ -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"])))

3
common/kalman/tests/test_simple_kalman.py
View File

@ -82,3 +82,6 @@ kf = KF1D(x0=[[x0_0], [x1_0]],
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()

3
common/params_pyx.pyx
View File

@ -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):

33
common/params_pyx_setup.py
View File

@ -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
)
)
)

2
common/realtime.py
View File

@ -5,7 +5,7 @@ import time
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

19
common/spinner.py
View File

@ -4,17 +4,14 @@ from common.basedir import BASEDIR
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

19
common/text_window.py
View File

@ -6,17 +6,14 @@ from common.basedir import BASEDIR
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:

9
common/transformations/SConscript
View File

@ -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')

94
common/transformations/camera.py
View File

@ -1,29 +1,67 @@
import numpy as np
import common.transformations.orientation as orient
FULL_FRAME_SIZE = (1164, 874)
W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
eon_focal_length = FOCAL = 910.0
import common.transformations.orientation as orient
from common.hardware import TICI
## -- 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
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_intrinsics = np.array([
[FOCAL, 0., W/2.],
[ 0., FOCAL, H/2.],
[ 0., 0., 1.]])
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)

94
common/transformations/model.py
View File

@ -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_INPUT_SIZE = (512, 256)
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 @@ medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
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

20
common/transformations/setup.py
View File

@ -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,
)
)

30
common/transformations/transformations.pyx
View File

@ -1,18 +1,20 @@
from transformations cimport Matrix3, Vector3, Quaternion
from transformations cimport ECEF, NED, Geodetic
# distutils: language = c++
# cython: language_level = 3
from common.transformations.transformations cimport Matrix3, Vector3, Quaternion
from common.transformations.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
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

28
launch_chffrplus.sh
View File

@ -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"

BIN
models/supercombo.dlc
View File

Binary file not shown.

9
opendbc/can/common.cc
View File

@ -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);

404
opendbc/lexus_nx300_2018_pt_generated.dbc 100644
View File

@ -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";

8
opendbc/toyota_nodsu_pt_generated.dbc
View File

@ -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";

2
panda/board/boards/uno.h
View File

@ -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;

4
release/build_release2.sh
View File

@ -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

BIN
selfdrive/assets/offroad/circled-checkmark-empty.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.7 KiB

6
selfdrive/boardd/SConscript
View File

@ -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"])

6
selfdrive/boardd/boardd.cc
View File

@ -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>();
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);

22
selfdrive/boardd/boardd_setup.py
View File

@ -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"],
)
)
)

5
selfdrive/boardd/panda.cc
View File

@ -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){

1
selfdrive/boardd/panda.h
View File

@ -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);

7
selfdrive/camerad/SConscript
View File

@ -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"])

49
selfdrive/camerad/cameras/camera_common.cc
View File

@ -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;r<new_height;r++) {
for (int c=0;c<new_width;c++) {
memcpy(&resized_dat[(r*new_width+c)*3], &dat[goff+r*stride*scale+c*3*scale], 3*sizeof(uint8_t));
}
}
framed.setImage(kj::arrayPtr((const uint8_t*)resized_dat, new_width*new_height*3));
delete[] resized_dat;
}
}
void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) {
const CameraBuf *b = &c->buf;
@ -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);
}

9
selfdrive/camerad/cameras/camera_common.h
View File

@ -1,5 +1,6 @@
#pragma once
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <memory>
@ -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 @@ public:
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);

34
selfdrive/camerad/cameras/camera_qcom.cc
View File

@ -1,4 +1,3 @@
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <assert.h>
@ -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<const int16_t>(&s->rear.focus[0], NUM_FOCUS));
framed.setFocusConf(kj::ArrayPtr<const uint8_t>(&s->rear.confidence[0], NUM_FOCUS));
framed.setSharpnessScore(kj::ArrayPtr<const uint16_t>(&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;
}

3
selfdrive/camerad/cameras/camera_qcom.h
View File

@ -142,7 +142,8 @@ typedef struct MultiCameraState {
CameraState rear;
CameraState front;
SubMaster *sm;
SubMaster *sm_front;
SubMaster *sm_rear;
PubMaster *pm;
} MultiCameraState;

1
selfdrive/camerad/main.cc
View File

@ -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);

20
selfdrive/car/car_helpers.py
View File

@ -9,12 +9,11 @@ from selfdrive.swaglog import cloudlog
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

11
selfdrive/car/chrysler/interface.py
View File

@ -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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
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

5
selfdrive/car/chrysler/values.py
View File

@ -91,8 +91,3 @@ DBC = {
}
STEER_THRESHOLD = 120
ECU_FINGERPRINT = {
Ecu.fwdCamera: [0x292], # lkas cmd
}

10
selfdrive/car/ford/interface.py
View File

@ -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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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

4
selfdrive/car/ford/values.py
View File

@ -15,10 +15,6 @@ FINGERPRINTS = {
}],
}
ECU_FINGERPRINT = {
Ecu.fwdCamera: [970, 973, 984]
}
DBC = {
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
}

4
selfdrive/car/fw_versions.py
View File

@ -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

12
selfdrive/car/gm/interface.py
View File

@ -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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
# 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

4
selfdrive/car/gm/values.py
View File

@ -70,10 +70,6 @@ FINGERPRINTS = {
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'),

12
selfdrive/car/honda/carcontroller.py
View File

@ -145,22 +145,22 @@ class CarController():
# 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 @@ class CarController():
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:

5
selfdrive/car/honda/carstate.py
View File

@ -336,7 +336,7 @@ class CarState(CarStateBase):
@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 @@ class CarState(CarStateBase):
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):

41
selfdrive/car/honda/hondacan.py
View File

@ -7,24 +7,19 @@ from selfdrive.car.honda.values import HONDA_BOSCH
# 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, has_relay):
return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0
def get_pt_bus(car_fingerprint):
return 1 if car_fingerprint in HONDA_BOSCH 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)

17
selfdrive/car/honda/interface.py
View File

@ -6,8 +6,8 @@ from common.realtime import DT_CTRL
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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
# 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]]

36
selfdrive/car/honda/values.py
View File

@ -231,6 +231,7 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
(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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
],
},
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 @@ SPEED_FACTOR = {
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])

15
selfdrive/car/hyundai/carcontroller.py
View File

@ -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 @@ class CarController():
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 @@ class CarController():
self.last_resume_frame = frame
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ]:
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

4
selfdrive/car/hyundai/hyundaican.py
View File

@ -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

134
selfdrive/car/hyundai/interface.py
View File

@ -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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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,6 +103,47 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.VELOSTER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75 * 1.15
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
# 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
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.KIA_FORTE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
@ -150,18 +152,36 @@ class CarInterface(CarInterfaceBase):
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:
# 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 = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75 * 1.15
tire_stiffness_factor = 0.5
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.25], [0.05]]
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 @@ class CarInterface(CarInterfaceBase):
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

79
selfdrive/car/hyundai/values.py
View File

@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu
# Steer torque limits
class SteerLimitParams:
def __init__(self, CP):
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE]:
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 @@ class SteerLimitParams:
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 @@ FINGERPRINTS = {
{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 @@ FINGERPRINTS = {
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 @@ FINGERPRINTS = {
}]
}
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 @@ FW_VERSIONS = {
(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 @@ FW_VERSIONS = {
(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 @@ FEATURES = {
# which message has the gear
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
"use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_2019, CAR.VELOSTER]),
"use_elect_gears": set([CAR.KIA_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 @@ DBC = {
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),

6
selfdrive/car/interfaces.py
View File

@ -42,15 +42,15 @@ class CarInterfaceBase():
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

10
selfdrive/car/mazda/interface.py
View File

@ -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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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

4
selfdrive/car/mazda/values.py
View File

@ -71,10 +71,6 @@ FINGERPRINTS = {
],
}
ECU_FINGERPRINT = {
Ecu.fwdCamera: [579], # steer torque cmd
}
DBC = {
CAR.CX5: dbc_dict('mazda_2017', None),

4
selfdrive/car/mock/interface.py
View File

@ -33,8 +33,8 @@ class CarInterface(CarInterfaceBase):
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.

4
selfdrive/car/nissan/interface.py
View File

@ -14,9 +14,9 @@ class CarInterface(CarInterfaceBase):
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

7
selfdrive/car/subaru/interface.py
View File

@ -11,8 +11,8 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
# 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

4
selfdrive/car/subaru/values.py
View File

@ -78,10 +78,6 @@ STEER_THRESHOLD = {
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),

51
selfdrive/car/tests/test_car_interfaces.py
View File

@ -25,36 +25,35 @@ class TestCarInterfaces(unittest.TestCase):
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

20
selfdrive/car/toyota/interface.py
View File

@ -14,8 +14,8 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
# 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:

97
selfdrive/car/toyota/values.py
View File

@ -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 @@ FINGERPRINTS = {
}],
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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
b'8821F0603300 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0609100 ',
],
(Ecu.esp, 0x7b0, None): [
@ -330,6 +340,7 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
b'8821F0603300 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0609100 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
@ -554,8 +566,10 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\x0230E40000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\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 @@ FW_VERSIONS = {
(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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
(Ecu.eps, 0x7a1, None): [
b'8965B42170\x00\x00\x00\x00\x00\x00',
b'8965B42171\x00\x00\x00\x00\x00\x00',
b'8965B42180\x00\x00\x00\x00\x00\x00',
b'8965B42181\x00\x00\x00\x00\x00\x00',
b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00',
b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00',
@ -963,6 +991,7 @@ FW_VERSIONS = {
},
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 @@ FW_VERSIONS = {
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 @@ FW_VERSIONS = {
],
(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 @@ FW_VERSIONS = {
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 @@ DBC = {
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

4
selfdrive/car/volkswagen/interface.py
View File

@ -18,8 +18,8 @@ class CarInterface(CarInterfaceBase):
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

4
selfdrive/common/clutil.c
View File

@ -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);

5
selfdrive/common/i2c.cc
View File

@ -12,7 +12,10 @@
#ifdef QCOM2
// TODO: decide if we want to isntall libi2c-dev everywhere
#include <linux/i2c-dev.h>
extern "C" {
#include <linux/i2c-dev.h>
#include <i2c/smbus.h>
}
I2CBus::I2CBus(uint8_t bus_id){
char bus_name[20];

15
selfdrive/common/mat.h
View File

@ -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

3
selfdrive/common/modeldata.h
View File

@ -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

29
selfdrive/common/params.cc
View File

@ -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 @@ cleanup:
}
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;

1
selfdrive/common/params.h
View File

@ -33,7 +33,6 @@ public:
// 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.

14
selfdrive/common/spinner.h
View File

@ -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

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.7.10-release"
#define COMMA_VERSION "0.8.0-d56e04c0-2020-11-24T21:53:22"

1
selfdrive/common/visionipc.h
View File

@ -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;

22
selfdrive/controls/controlsd.py
View File

@ -52,7 +52,7 @@ class Controls:
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 @@ class Controls:
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 @@ class Controls:
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 @@ class Controls:
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 @@ class Controls:
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 @@ class Controls:
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 @@ class Controls:
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

4
selfdrive/controls/lib/alerts_offroad.json
View File

@ -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
}
}

29
selfdrive/controls/lib/events.py
View File

@ -252,31 +252,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, 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 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, 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: {

72
selfdrive/controls/lib/lane_planner.py
View File

@ -5,9 +5,9 @@ from cereal import log
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 @@ class LanePlanner():
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 @@ class LanePlanner():
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 @@ class LanePlanner():
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
def update(self, v_ego, md):
self.parse_model(md)
self.update_d_poly(v_ego)
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)
self.d_poly = lr_prob * d_poly_lane + (1.0 - lr_prob) * self.p_poly.copy()

1
selfdrive/controls/lib/pathplanner.py
View File

@ -43,6 +43,7 @@ DESIRES = {
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

1
selfdrive/debug/get_fingerprint.py
View File

@ -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

4
selfdrive/debug/test_fw_query_on_routes.py
View File

@ -37,7 +37,7 @@ if __name__ == "__main__":
for route in tqdm(routes):
route = route.rstrip()
dongle_id, time = route.split('|')
qlog_path = f"cd:/{dongle_id}/{time}/1/qlog.bz2"
qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2"
if dongle_id in dongles:
continue
@ -65,7 +65,7 @@ if __name__ == "__main__":
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):

10
selfdrive/locationd/locationd.py
View File

@ -196,14 +196,14 @@ class Localizer():
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 @@ class Localizer():
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 = []

26
selfdrive/loggerd/loggerd.cc
View File

@ -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) &&

21
selfdrive/manager.py
View File

@ -78,8 +78,10 @@ import traceback
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 @@ if not prebuilt:
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 @@ if not prebuilt:
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)

20
selfdrive/modeld/SConscript
View File

@ -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

5
selfdrive/modeld/dmonitoringmodeld.cc
View File

@ -3,6 +3,7 @@
#include <unistd.h>
#include <signal.h>
#include <cassert>
#include <sys/resource.h>
#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;

29
selfdrive/modeld/modeld.cc
View File

@ -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<float, 3, 3> fcam_intrinsics;
#ifndef QCOM2
Eigen::Matrix<float, 3, 3> 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<float, 3, 3> 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<float, 3, 3> 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;
}

23
selfdrive/modeld/models/commonmodel.c → selfdrive/modeld/models/commonmodel.cc
View File

@ -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));

2
selfdrive/modeld/models/commonmodel.h
View File

@ -8,6 +8,7 @@
#include <CL/cl.h>
#endif
#include <float.h>
#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);

40
selfdrive/modeld/models/dmonitoring.cc
View File

@ -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<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
kj::ArrayPtr<const float> face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta));

3
selfdrive/modeld/models/dmonitoring.h
View File

@ -29,6 +29,7 @@ typedef struct DMonitoringModelState {
bool is_rhd;
float output[OUTPUT_SIZE];
std::vector<uint8_t> resized_buf;
std::vector<uint8_t> resized_buf_rot;
std::vector<uint8_t> cropped_buf;
std::vector<uint8_t> premirror_cropped_buf;
std::vector<float> 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

371
selfdrive/modeld/models/driving.cc
View File

@ -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<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> 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<MODEL_PATH_DISTANCE; i++) {
points_arr[i] = data[i] + offset;
stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]) + 1e-6;
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// negative sign because mpc has left positive
points_arr[i] = -data[30*i + 16];
stds_arr[i] = exp(data[30*(33 + i) + 16]);
}
if (has_prob) {
prob = sigmoid(data[MODEL_PATH_DISTANCE*2 + 1]);
} else {
prob = 1.0;
}
std = softplus(data[MODEL_PATH_DISTANCE]) + 1e-6;
poly_fit(points_arr, stds_arr, poly_arr, valid_len);
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
if (std::getenv("DEBUG")){
kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
path.setStds(stds);
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
path.setPoly(poly);
path.setProb(1.0);
path.setStd(std);
path.setValidLen(valid_len);
}
kj::ArrayPtr<const float> points(&points_arr[0], ARRAYSIZE(points_arr));
path.setPoints(points);
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;
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// negative sign because mpc has left positive
points_arr[i] = -data[2*33*ll_idx + 2*i];
stds_arr[i] = exp(data[2*33*(4 + ll_idx) + 2*i]);
}
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
kj::ArrayPtr<const float> 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;
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<LEAD_MHP_VALS; i++) {
xyva_arr[i] = data[LEAD_MHP_VALS + i];
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
}
kj::ArrayPtr<const float> xyva(xyva_arr, LEAD_MHP_VALS);
kj::ArrayPtr<const float> xyva_stds(xyva_stds_arr, LEAD_MHP_VALS);
lead.setXyva(xyva);
lead.setXyvaStd(xyva_stds);
}
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(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<const float> 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<const float> 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<const float> 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<const float> 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<TIME_DISTANCE/10; i++) {
dist_arr[i] = long_x_data[i*10];
speed_arr[i] = long_v_data[i*10];
accel_arr[i] = long_a_data[i*10];
void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
float desire_state_softmax[DESIRE_LEN];
float desire_pred_softmax[4*DESIRE_LEN];
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
for (int i=0; i<4; i++) {
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
}
kj::ArrayPtr<const float> dist(&dist_arr[0], ARRAYSIZE(dist_arr));
longi.setDistances(dist);
kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
longi.setSpeeds(speed);
kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
longi.setAccelerations(accel);
kj::ArrayPtr<const float> 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<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
meta.setDesirePrediction(desire_pred);
}
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<TRAJECTORY_SIZE; i++) {
// column_offset == -1 means this data is X indexed not T indexed
if (column_offset >= 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<const float> x(x_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> y(y_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> z(z_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> z_std(z_std_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> 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<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
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<TRAJECTORY_SIZE; i++) {
plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
}
auto position = framed.initPosition();
fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
auto velocity = framed.initVelocity();
fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
auto orientation = framed.initOrientation();
fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
auto orientation_rate = framed.initOrientationRate();
fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
// lane lines
auto lane_lines = framed.initLaneLines(4);
float lane_line_probs_arr[4];
float lane_line_stds_arr[4];
for (int i = 0; i < 4; i++) {
fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
}
kj::ArrayPtr<const float> 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<LEAD_MHP_SELECTION; t_offset++) {
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION] >
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) {
uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
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);
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);
// Find the distribution that corresponds to the most probable plan
int plan_mhp_max_idx = 0;
for (int i=1; i<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
plan_mhp_max_idx = i;
}
}
// x pos at 10s is a good valid_len
float valid_len = 0;
float valid_len_candidate;
for (int i=1; i<TRAJECTORY_SIZE; i++) {
valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i];
if (valid_len_candidate >= 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<TRAJECTORY_SIZE; i++) {
if (valid_len >= 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<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
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<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
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;

42
selfdrive/modeld/models/driving.h
View File

@ -17,33 +17,40 @@
#include <memory>
#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

Some files were not shown because too many files have changed in this diff Show More